diff --git a/src/main/java/frc/JoysticksBindings.java b/src/main/java/frc/JoysticksBindings.java index 4764f1dc51..12cc48560f 100644 --- a/src/main/java/frc/JoysticksBindings.java +++ b/src/main/java/frc/JoysticksBindings.java @@ -1,5 +1,6 @@ package frc; +import edu.wpi.first.wpilibj2.command.RunCommand; import frc.joysticks.JoystickPorts; import frc.joysticks.SmartJoystick; import frc.robot.Robot; @@ -27,6 +28,10 @@ public static void configureBindings(Robot robot) { private static void mainJoystickButtons(Robot robot) { SmartJoystick usedJoystick = MAIN_JOYSTICK; + usedJoystick.A.onTrue(new RunCommand(() -> robot.test.getMotor().setPower(1), robot.getTest())); + usedJoystick.B.onTrue(new RunCommand(() -> robot.test.getMotor().setPower(0), robot.getTest())); + usedJoystick.X.onTrue(new RunCommand(() -> robot.test.getMotor().setPower(0.5), robot.getTest())); + // bindings... } diff --git a/src/main/java/frc/robot/IDs.java b/src/main/java/frc/robot/IDs.java index 8020584df2..e744b4f02b 100644 --- a/src/main/java/frc/robot/IDs.java +++ b/src/main/java/frc/robot/IDs.java @@ -1,8 +1,16 @@ package frc.robot; +import com.revrobotics.spark.SparkLowLevel; +import frc.robot.hardware.phoenix6.BusChain; +import frc.robot.hardware.phoenix6.Phoenix6DeviceID; +import frc.robot.hardware.rev.motors.SparkMaxDeviceID; + public class IDs { public static class TalonFXIDs { + + public static final Phoenix6DeviceID test = new Phoenix6DeviceID(0, BusChain.ROBORIO); + } public static class CANCoderIDs { @@ -15,6 +23,9 @@ public static class CANdleIDs { } public static class SparkMAXIDs { + + public static final SparkMaxDeviceID test = new SparkMaxDeviceID(0, SparkLowLevel.MotorType.kBrushless); + } public static class DigitalInputsIDs { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index fef3018de6..8bafe16f47 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -7,6 +7,8 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.RobotManager; import frc.robot.hardware.phoenix6.BusChain; +import frc.robot.subsystems.TalonFXTestBuilder; +import frc.robot.subsystems.Test; import frc.utils.auto.PathPlannerAutoWrapper; import frc.utils.battery.BatteryUtil; @@ -18,9 +20,11 @@ public class Robot { public static final RobotType ROBOT_TYPE = RobotType.determineRobotType(false); + public final Test test; public Robot() { BatteryUtil.scheduleLimiter(); + this.test = TalonFXTestBuilder.buildTest(); } public void periodic() { @@ -29,6 +33,10 @@ public void periodic() { CommandScheduler.getInstance().run(); // Should be last } + public Test getTest() { + return test; + } + public PathPlannerAutoWrapper getAutonomousCommand() { return new PathPlannerAutoWrapper(); } diff --git a/src/main/java/frc/robot/subsystems/SparkMaxTestBuilder.java b/src/main/java/frc/robot/subsystems/SparkMaxTestBuilder.java new file mode 100644 index 0000000000..2fa2ee3a2a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/SparkMaxTestBuilder.java @@ -0,0 +1,74 @@ +package frc.robot.subsystems; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.IDs; +import frc.robot.RobotConstants; +import frc.robot.hardware.mechanisms.wpilib.SimpleMotorSimulation; +import frc.robot.hardware.rev.motors.BrushlessSparkMAXMotor; +import frc.robot.hardware.rev.motors.SparkMaxWrapper; +import frc.utils.TimedValue; +import frc.utils.time.TimeUtil; + +import java.util.function.Supplier; + +public class SparkMaxTestBuilder { + + public static Test buildTest() { + String logPath = RobotConstants.SUBSYSTEM_LOGPATH_PREFIX + "/Test"; + + SparkMaxWrapper sparkMaxWrapper = new SparkMaxWrapper(IDs.SparkMAXIDs.test); + + BrushlessSparkMAXMotor motor = new BrushlessSparkMAXMotor( + logPath + "/Motor", + sparkMaxWrapper, + new SimpleMotorSimulation(new DCMotorSim(LinearSystemId.createDCMotorSystem(DCMotor.getNEO(1), 0.0001, 1 / 1), DCMotor.getNEO(1))), + new SysIdRoutine.Config() + ); + + TimedValue positionTimedValue = new TimedValue<>( + Rotation2d.fromRotations(sparkMaxWrapper.getEncoder().getPosition()), + TimeUtil.getCurrentTimeSeconds() + ); + + TimedValue velocityTimedValue = new TimedValue<>( + sparkMaxWrapper.getVelocityAnglePerSecond(), + TimeUtil.getCurrentTimeSeconds() + ); + + TimedValue voltageTimedValue = new TimedValue<>(sparkMaxWrapper.getVoltage(), TimeUtil.getCurrentTimeSeconds()); + + TimedValue currentTimedValue = new TimedValue<>(sparkMaxWrapper.getOutputCurrent(), TimeUtil.getCurrentTimeSeconds()); + + + Supplier> positionSignal = () -> { + positionTimedValue.setValue(Rotation2d.fromRotations(sparkMaxWrapper.getEncoder().getPosition())); + positionTimedValue.setTimestamp(TimeUtil.getCurrentTimeSeconds()); + return positionTimedValue; + }; + + Supplier> velocitySignal = () -> { + velocityTimedValue.setValue(sparkMaxWrapper.getVelocityAnglePerSecond()); + velocityTimedValue.setTimestamp(TimeUtil.getCurrentTimeSeconds()); + return velocityTimedValue; + }; + + Supplier> voltageSignal = () -> { + voltageTimedValue.setValue(sparkMaxWrapper.getVoltage()); + voltageTimedValue.setTimestamp(TimeUtil.getCurrentTimeSeconds()); + return voltageTimedValue; + }; + + Supplier> currentSignal = () -> { + currentTimedValue.setValue(sparkMaxWrapper.getOutputCurrent()); + currentTimedValue.setTimestamp(TimeUtil.getCurrentTimeSeconds()); + return currentTimedValue; + }; + + return new Test(logPath, motor, positionSignal, velocitySignal, voltageSignal, currentSignal); + } + +} diff --git a/src/main/java/frc/robot/subsystems/TalonFXTestBuilder.java b/src/main/java/frc/robot/subsystems/TalonFXTestBuilder.java new file mode 100644 index 0000000000..c1f9f38fb9 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/TalonFXTestBuilder.java @@ -0,0 +1,89 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.IDs; +import frc.robot.RobotConstants; +import frc.robot.hardware.mechanisms.wpilib.SimpleMotorSimulation; +import frc.robot.hardware.phoenix6.motors.TalonFXMotor; +import frc.utils.TimedValue; +import frc.utils.time.TimeUtil; + +import java.util.function.Supplier; + +public class TalonFXTestBuilder { + + public static Test buildTest() { + String logPath = RobotConstants.SUBSYSTEM_LOGPATH_PREFIX + "/Test"; + + TalonFXMotor motor = new TalonFXMotor( + logPath + "/Motor", + IDs.TalonFXIDs.test, + new SysIdRoutine.Config(), + new SimpleMotorSimulation(new DCMotorSim(LinearSystemId.createDCMotorSystem(DCMotor.getNEO(1), 0.0001, 1 / 1), DCMotor.getNEO(1))) + ); + + StatusSignal rawPositionSignal = motor.getDevice().getPosition(true); + StatusSignal rawVelocitySignal = motor.getDevice().getVelocity(true); + StatusSignal rawVoltageSignal = motor.getDevice().getMotorVoltage(true); + StatusSignal rawCurrentSignal = motor.getDevice().getStatorCurrent(true); + + rawPositionSignal.setUpdateFrequency(RobotConstants.DEFAULT_SIGNALS_FREQUENCY_HERTZ); + rawVelocitySignal.setUpdateFrequency(RobotConstants.DEFAULT_SIGNALS_FREQUENCY_HERTZ); + rawVoltageSignal.setUpdateFrequency(RobotConstants.DEFAULT_SIGNALS_FREQUENCY_HERTZ); + rawCurrentSignal.setUpdateFrequency(RobotConstants.DEFAULT_SIGNALS_FREQUENCY_HERTZ); + + TimedValue positionTimedValue = new TimedValue<>( + Rotation2d + .fromRotations(BaseStatusSignal.getLatencyCompensatedValueAsDouble(rawPositionSignal.refresh(), rawVelocitySignal.refresh())), + TimeUtil.getCurrentTimeSeconds() + ); + TimedValue velocityTimedValue = new TimedValue<>( + Rotation2d.fromRotations(rawVelocitySignal.refresh().getValueAsDouble()), + TimeUtil.getCurrentTimeSeconds() + ); + TimedValue voltageTimedValue = new TimedValue<>(rawVoltageSignal.refresh().getValueAsDouble(), TimeUtil.getCurrentTimeSeconds()); + TimedValue currentTimedValue = new TimedValue<>(rawCurrentSignal.refresh().getValueAsDouble(), TimeUtil.getCurrentTimeSeconds()); + + Supplier> positionSignal = () -> { + positionTimedValue.setValue( + Rotation2d + .fromRotations(BaseStatusSignal.getLatencyCompensatedValueAsDouble(rawPositionSignal.refresh(), rawVelocitySignal.refresh())) + ); + positionTimedValue.setTimestamp(TimeUtil.getCurrentTimeSeconds()); + return positionTimedValue; + }; + + Supplier> velocitySignal = () -> { + velocityTimedValue.setValue(Rotation2d.fromRotations(rawVelocitySignal.refresh().getValueAsDouble())); + velocityTimedValue.setTimestamp(TimeUtil.getCurrentTimeSeconds()); + return velocityTimedValue; + }; + + Supplier> voltageSignal = () -> { + voltageTimedValue.setValue(rawVoltageSignal.refresh().getValueAsDouble()); + voltageTimedValue.setTimestamp(TimeUtil.getCurrentTimeSeconds()); + return voltageTimedValue; + }; + + Supplier> currentSignal = () -> { + currentTimedValue.setValue(rawCurrentSignal.refresh().getValueAsDouble()); + currentTimedValue.setTimestamp(TimeUtil.getCurrentTimeSeconds()); + return currentTimedValue; + }; + + motor.getDevice().optimizeBusUtilization(); + + return new Test(logPath, motor, positionSignal, velocitySignal, voltageSignal, currentSignal); + } + +} diff --git a/src/main/java/frc/robot/subsystems/Test.java b/src/main/java/frc/robot/subsystems/Test.java new file mode 100644 index 0000000000..a1eab86373 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Test.java @@ -0,0 +1,51 @@ +package frc.robot.subsystems; + +import edu.wpi.first.math.geometry.Rotation2d; +import frc.robot.hardware.interfaces.ControllableMotor; +import frc.utils.TimedValue; +import org.littletonrobotics.junction.Logger; + +import java.util.function.Supplier; + +public class Test extends GBSubsystem { + + ControllableMotor motor; + + Supplier> positionSignal; + Supplier> velocitySignal; + Supplier> voltageSignal; + Supplier> currentSignal; + + public Test( + String logPath, + ControllableMotor motor, + Supplier> positionSignal, + Supplier> velocitySignal, + Supplier> voltageSignal, + Supplier> currentSignal + ) { + super(logPath); + + this.motor = motor; + + this.positionSignal = positionSignal; + this.voltageSignal = voltageSignal; + this.velocitySignal = velocitySignal; + this.currentSignal = currentSignal; + } + + public ControllableMotor getMotor() { + return motor; + } + + @Override + protected void subsystemPeriodic() { + motor.updateSimulation(); + + Logger.recordOutput(getLogPath() + "/Position", positionSignal.get().getValue()); + Logger.recordOutput(getLogPath() + "/Velocity", velocitySignal.get().getValue()); + Logger.recordOutput(getLogPath() + "/Voltage", voltageSignal.get().getValue()); + Logger.recordOutput(getLogPath() + "/Current", currentSignal.get().getValue()); + } + +}