diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 0507c87..f91d864 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -24,7 +24,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.CommandGenericHID; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -223,13 +222,20 @@ public Robot() { CommandScheduler.getInstance().onCommandFinish(cmd -> activeCommands.remove(cmd.getName())); CommandScheduler.getInstance().onCommandInterrupt(cmd -> activeCommands.remove(cmd.getName())); - new Trigger( - NetworkTableInstance.getDefault() - .getTable("Triggers") - .getBooleanTopic("Align Encoders") - .subscribe(false) - ::get) - .onTrue(new InstantCommand(drive::zeroAbsoluteEncoders).ignoringDisable(true)); + var alignEncodersEntry = + NetworkTableInstance.getDefault() + .getTable("Triggers") + .getBooleanTopic("Align Encoders") + .getEntry(false); + alignEncodersEntry.set(false); + new Trigger(alignEncodersEntry::get) + .onTrue( + Commands.runOnce( + () -> { + drive.zeroAbsoluteEncoders(); + alignEncodersEntry.set(false); + }) + .ignoringDisable(true)); Field.plotRegions(); } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 26250ea..2f72562 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -10,12 +10,19 @@ import static edu.wpi.first.units.Units.*; import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.ClosedLoopGeneralConfigs; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.FeedbackConfigs; +import com.ctre.phoenix6.configs.MotionMagicConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.configs.TorqueCurrentConfigs; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; import com.ctre.phoenix6.swerve.SwerveDrivetrain; import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; @@ -33,6 +40,7 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.units.measure.AngularAcceleration; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; @@ -40,7 +48,7 @@ import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.units.measure.Mass; import edu.wpi.first.units.measure.MomentOfInertia; -import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; import frc.robot.Constants.CANBusPorts.SC1; import frc.robot.Constants.MotorConstants.KrakenX60Constants; @@ -65,10 +73,49 @@ public class DriveConstants { public static final Distance WHEEL_RADIUS = Inches.of(2); public static final double WHEEL_RADIUS_METERS = WHEEL_RADIUS.in(Meters); - // public static enum DRIVE_MOTOR_REDUCTIONS { SDS_MK5_R1, SDS_MK5_R2, SDS_MK5_R3 } + private static enum DriveGearRatio { + SDS_MK5i_R1(1, 54.0 / 12.0, 25.0 / 32.0, 30.0 / 15.0), + SDS_MK5i_R2(1, 54.0 / 14.0, 25.0 / 32.0, 30.0 / 15.0), + SDS_MK5i_R3(1, 54.0 / 16.0, 25.0 / 32.0, 30.0 / 15.0); - public static final double DRIVE_MOTOR_REDUCTION = - (54.0 / 14.0) * (25.0 / 32.0) * (30.0 / 15.0); // SDS MK5 R2 + private final int chassisStages; + private final double[] reductions; + + /** + * @param chassisStages the number of leading reduction stages fixed to the chassis reference + * frame; their product defines the couple ratio + * @param reductions all gear reduction stages in order, chassis-frame stages first followed by + * azimuth-frame stages + */ + private DriveGearRatio(int chassisStages, double... reductions) { + this.chassisStages = chassisStages; + this.reductions = reductions; + } + + /** + * Returns the total gear reduction from the drive motor to the wheel, combining all stages + * regardless of reference frame. Used to convert drive motor rotations to wheel rotations. + */ + public double getDriveMotorReduction() { + double product = 1.0; + for (double r : reductions) product *= r; + return product; + } + + /** + * Returns the number of drive motor rotations per one full rotation of the steering azimuth, + * due to mechanical coupling. This is the product of all gear reduction stages that are fixed + * to the chassis reference frame (i.e., before the azimuth pivot). The swerve odometry uses + * this to compensate for the apparent wheel displacement caused by azimuth rotation. + */ + public double getCoupleRatio() { + double product = 1.0; + for (int i = 0; i < chassisStages; i++) product *= reductions[i]; + return product; + } + } + + private static final DriveGearRatio SELECTED_RATIO = DriveGearRatio.SDS_MK5i_R2; public static final DCMotor DRIVE_GEARBOX = DCMotor.getKrakenX60Foc(1); public static final LinearVelocity DRIVETRAIN_SPEED_LIMIT = MetersPerSecond.of( @@ -76,7 +123,7 @@ public class DriveConstants { * (WHEEL_RADIUS_METERS * 2.0 * Math.PI) * DRIVE_GEARBOX.freeSpeed / (2.0 * Math.PI) - / DRIVE_MOTOR_REDUCTION); + / SELECTED_RATIO.getDriveMotorReduction()); // Chassis movement limits private static final LinearVelocity DRIVER_SPEED_LIMIT = MetersPerSecond.of(5); @@ -101,9 +148,7 @@ public class DriveConstants { // Turn motor configuration public static final boolean TURN_INVERTED = false; - public static final double TURN_MOTOR_REDUCTION = 26; // SDS MK5 R2 - // Every 1 rotation of the azimuth results in COUPLE_RATIO drive motor turns - private static final double COUPLE_RATIO = (54.0 / 14.0); // SDS MK4 L2 + public static final double TURN_MOTOR_REDUCTION = 26.0; // SDS MK5i public static final DCMotor TURN_GEARBOX = DCMotor.getKrakenX60Foc(1); // Absolute turn encoder configuration @@ -122,7 +167,7 @@ public class DriveConstants { WHEEL_RADIUS_METERS, DRIVETRAIN_SPEED_LIMIT.in(MetersPerSecond), WHEEL_COF, - DRIVE_GEARBOX.withReduction(DRIVE_MOTOR_REDUCTION), + DRIVE_GEARBOX.withReduction(SELECTED_RATIO.getDriveMotorReduction()), KrakenX60Constants.DEFAULT_SUPPLY_CURRENT_LIMIT, 1), MODULE_TRANSLATIONS); @@ -143,59 +188,13 @@ public class DriveConstants { private static final Slot0Configs DRIVE_GAINS = new Slot0Configs().withKP(10).withKI(0).withKD(0).withKS(0).withKV(0.124); - // The closed-loop output type to use for the steer motors; - // This affects the PID/FF gains for the steer motors - private static final ClosedLoopOutputType STEER_CLOSED_LOOP_OUTPUT = - ClosedLoopOutputType.TorqueCurrentFOC; - // The closed-loop output type to use for the drive motors; - // This affects the PID/FF gains for the drive motors - private static final ClosedLoopOutputType DRIVE_CLOSED_LOOP_OUTPUT = - ClosedLoopOutputType.TorqueCurrentFOC; - - // The type of motor used for the drive motor - private static final DriveMotorArrangement DRIVE_MOTOR_TYPE = - DriveMotorArrangement.TalonFX_Integrated; - // The type of motor used for the steer motor - private static final SteerMotorArrangement STEER_MOTOR_TYPE = - SteerMotorArrangement.TalonFX_Integrated; - - // The remote sensor feedback type to use for the steer motors - private static final SteerFeedbackType STEER_FEEDBACK_TYPE = SteerFeedbackType.FusedCANcoder; - // TorqueCurrent peak at which the wheels start to slip; used for slip detection in // TorqueCurrentFOC control mode. This needs to be tuned to your individual robot. - static final int SLIP_CURRENT = 120; - - // Hardware stator current limit for drive motors - static final int DRIVE_STATOR_CURRENT_LIMIT = KrakenX60Constants.DEFAULT_STATOR_CURRENT_LIMIT; + private static final int SLIP_CURRENT = 120; // Stator current limit for azimuth (steer) motors; lower than drive to reduce brownout risk // since steering requires minimal torque compared to driving. - static final int STEER_STATOR_CURRENT_LIMIT = 60; - - private static final TalonFXConfiguration DRIVE_INITIAL_CONFIGS = - new TalonFXConfiguration() - .withTorqueCurrent( - new TorqueCurrentConfigs() - .withPeakForwardTorqueCurrent(SLIP_CURRENT) - .withPeakReverseTorqueCurrent(-SLIP_CURRENT)) - .withCurrentLimits( - new CurrentLimitsConfigs() - .withStatorCurrentLimit(DRIVE_STATOR_CURRENT_LIMIT) - .withStatorCurrentLimitEnable(true) - .withSupplyCurrentLimit(KrakenX60Constants.DEFAULT_SUPPLY_CURRENT_LIMIT) - .withSupplyCurrentLimitEnable(true)); - - // Azimuth does not require much torque; keep stator limit low to reduce brownout risk - // since steering requires minimal torque compared to driving. - private static final TalonFXConfiguration TURN_INITIAL_CONFIGS = - new TalonFXConfiguration() - .withCurrentLimits( - new CurrentLimitsConfigs() - .withStatorCurrentLimit(STEER_STATOR_CURRENT_LIMIT) - .withStatorCurrentLimitEnable(true) - .withSupplyCurrentLimit(KrakenX60Constants.DEFAULT_SUPPLY_CURRENT_LIMIT) - .withSupplyCurrentLimitEnable(true)); + private static final int STEER_STATOR_CURRENT_LIMIT = 60; private static final boolean INVERT_LEFT_SIDE = false; private static final boolean INVERT_RIGHT_SIDE = false; @@ -203,9 +202,35 @@ public class DriveConstants { // These are only used for simulation private static final MomentOfInertia STEER_INERTIA = KilogramSquareMeters.of(0.004); private static final MomentOfInertia DRIVE_INERTIA = KilogramSquareMeters.of(0.025); - // Simulated voltage necessary to overcome friction - private static final Voltage STEER_FRICTION_VOLTAGE = Volts.of(0.2); - private static final Voltage DRIVE_FRICTION_VOLTAGE = Volts.of(0.2); + + static DCMotorSim createDriveSim() { + return new DCMotorSim( + LinearSystemId.createDCMotorSystem( + DRIVE_GEARBOX, + DRIVE_INERTIA.in(KilogramSquareMeters), + SELECTED_RATIO.getDriveMotorReduction()), + DRIVE_GEARBOX); + } + + static DCMotorSim createTurnSim() { + return new DCMotorSim( + LinearSystemId.createDCMotorSystem( + TURN_GEARBOX, STEER_INERTIA.in(KilogramSquareMeters), TURN_MOTOR_REDUCTION), + TURN_GEARBOX); + } + + private static SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + configureModule( + SwerveModuleConstants + constants) { + constants.DriveMotorInitialConfigs.MotorOutput.Inverted = + constants.DriveMotorInverted + ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive; + constants.SteerMotorInitialConfigs.Feedback.FeedbackRemoteSensorID = constants.EncoderId; + return constants; + } public static final SwerveDrivetrainConstants DRIVETRAIN_CONSTANTS = new SwerveDrivetrainConstants().withCANBusName(SC1.BUS.getName()); @@ -215,87 +240,124 @@ public class DriveConstants { CONSTANT_CREATOR = new SwerveModuleConstantsFactory< TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>() - .withDriveMotorGearRatio(DRIVE_MOTOR_REDUCTION) + .withDriveMotorGearRatio(SELECTED_RATIO.getDriveMotorReduction()) .withSteerMotorGearRatio(TURN_MOTOR_REDUCTION) - .withCouplingGearRatio(COUPLE_RATIO) + .withCouplingGearRatio(SELECTED_RATIO.getCoupleRatio()) .withWheelRadius(WHEEL_RADIUS) .withSteerMotorGains(STEER_GAINS) .withDriveMotorGains(DRIVE_GAINS) - .withSteerMotorClosedLoopOutput(STEER_CLOSED_LOOP_OUTPUT) - .withDriveMotorClosedLoopOutput(DRIVE_CLOSED_LOOP_OUTPUT) + .withSteerMotorClosedLoopOutput(ClosedLoopOutputType.TorqueCurrentFOC) + .withDriveMotorClosedLoopOutput(ClosedLoopOutputType.TorqueCurrentFOC) .withSlipCurrent(Amps.of(SLIP_CURRENT)) .withSpeedAt12Volts(DRIVETRAIN_SPEED_LIMIT) - .withDriveMotorType(DRIVE_MOTOR_TYPE) - .withSteerMotorType(STEER_MOTOR_TYPE) - .withFeedbackSource(STEER_FEEDBACK_TYPE) - .withDriveMotorInitialConfigs(DRIVE_INITIAL_CONFIGS) - .withSteerMotorInitialConfigs(TURN_INITIAL_CONFIGS) + .withDriveMotorType(DriveMotorArrangement.TalonFX_Integrated) + .withSteerMotorType(SteerMotorArrangement.TalonFX_Integrated) + .withFeedbackSource(SteerFeedbackType.FusedCANcoder) + .withDriveMotorInitialConfigs( + new TalonFXConfiguration() + .withMotorOutput( + new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Brake)) + .withSlot0(DRIVE_GAINS) + .withFeedback( + new FeedbackConfigs() + .withSensorToMechanismRatio(SELECTED_RATIO.getDriveMotorReduction())) + .withTorqueCurrent( + new TorqueCurrentConfigs() + .withPeakForwardTorqueCurrent(SLIP_CURRENT) + .withPeakReverseTorqueCurrent(-SLIP_CURRENT)) + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit( + KrakenX60Constants.DEFAULT_STATOR_CURRENT_LIMIT) + .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimit( + KrakenX60Constants.DEFAULT_SUPPLY_CURRENT_LIMIT) + .withSupplyCurrentLimitEnable(true))) + .withSteerMotorInitialConfigs( + new TalonFXConfiguration() + .withMotorOutput( + new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Brake) + .withInverted(InvertedValue.CounterClockwise_Positive)) + .withSlot0(STEER_GAINS) + .withFeedback( + new FeedbackConfigs() + .withFeedbackSensorSource(FeedbackSensorSourceValue.FusedCANcoder) + .withRotorToSensorRatio(TURN_MOTOR_REDUCTION)) + .withMotionMagic( + new MotionMagicConfigs() + .withMotionMagicCruiseVelocity(100.0 / TURN_MOTOR_REDUCTION) + .withMotionMagicAcceleration(100.0 / TURN_MOTOR_REDUCTION / 0.100) + .withMotionMagicExpo_kV(0.12 * TURN_MOTOR_REDUCTION) + .withMotionMagicExpo_kA(0.1)) + .withClosedLoopGeneral( + new ClosedLoopGeneralConfigs().withContinuousWrap(true)) + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(STEER_STATOR_CURRENT_LIMIT) + .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimit( + KrakenX60Constants.DEFAULT_SUPPLY_CURRENT_LIMIT) + .withSupplyCurrentLimitEnable(true))) .withSteerInertia(STEER_INERTIA) - .withDriveInertia(DRIVE_INERTIA) - .withSteerFrictionVoltage(STEER_FRICTION_VOLTAGE) - .withDriveFrictionVoltage(DRIVE_FRICTION_VOLTAGE); + .withDriveInertia(DRIVE_INERTIA); public static final SwerveModuleConstants< TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FRONT_LEFT = - CONSTANT_CREATOR.createModuleConstants( - SC1.FRONT_LEFT_TURN, - SC1.FRONT_LEFT_DRIVE, - SC1.FRONT_LEFT_TURN_ABS_ENC, - Rotations.of(0), - WHEEL_BASE.div(2.0), - TRACK_WIDTH.div(2.0), - INVERT_LEFT_SIDE, - TURN_INVERTED, - TURN_ENCODER_INVERTED); + configureModule( + CONSTANT_CREATOR.createModuleConstants( + SC1.FRONT_LEFT_TURN, + SC1.FRONT_LEFT_DRIVE, + SC1.FRONT_LEFT_TURN_ABS_ENC, + Rotations.of(0), + WHEEL_BASE.div(2.0), + TRACK_WIDTH.div(2.0), + INVERT_LEFT_SIDE, + TURN_INVERTED, + TURN_ENCODER_INVERTED)); public static final SwerveModuleConstants< TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FRONT_RIGHT = - CONSTANT_CREATOR.createModuleConstants( - SC1.FRONT_RIGHT_TURN, - SC1.FRONT_RIGHT_DRIVE, - SC1.FRONT_RIGHT_TURN_ABS_ENC, - Rotations.of(0), - WHEEL_BASE.div(2.0), - TRACK_WIDTH.div(-2.0), - INVERT_RIGHT_SIDE, - TURN_INVERTED, - TURN_ENCODER_INVERTED); + configureModule( + CONSTANT_CREATOR.createModuleConstants( + SC1.FRONT_RIGHT_TURN, + SC1.FRONT_RIGHT_DRIVE, + SC1.FRONT_RIGHT_TURN_ABS_ENC, + Rotations.of(0), + WHEEL_BASE.div(2.0), + TRACK_WIDTH.div(-2.0), + INVERT_RIGHT_SIDE, + TURN_INVERTED, + TURN_ENCODER_INVERTED)); public static final SwerveModuleConstants< TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> BACK_LEFT = - CONSTANT_CREATOR.createModuleConstants( - SC1.BACK_LEFT_TURN, - SC1.BACK_LEFT_DRIVE, - SC1.BACK_LEFT_TURN_ABS_ENC, - Rotations.of(0), - WHEEL_BASE.div(-2.0), - TRACK_WIDTH.div(2.0), - INVERT_LEFT_SIDE, - TURN_INVERTED, - TURN_ENCODER_INVERTED); + configureModule( + CONSTANT_CREATOR.createModuleConstants( + SC1.BACK_LEFT_TURN, + SC1.BACK_LEFT_DRIVE, + SC1.BACK_LEFT_TURN_ABS_ENC, + Rotations.of(0), + WHEEL_BASE.div(-2.0), + TRACK_WIDTH.div(2.0), + INVERT_LEFT_SIDE, + TURN_INVERTED, + TURN_ENCODER_INVERTED)); public static final SwerveModuleConstants< TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> BACK_RIGHT = - CONSTANT_CREATOR.createModuleConstants( - SC1.BACK_RIGHT_TURN, - SC1.BACK_RIGHT_DRIVE, - SC1.BACK_RIGHT_TURN_ABS_ENC, - Rotations.of(0), - WHEEL_BASE.div(-2.0), - TRACK_WIDTH.div(-2.0), - INVERT_RIGHT_SIDE, - TURN_INVERTED, - TURN_ENCODER_INVERTED); - - /** - * Creates a CommandSwerveDrivetrain instance. This should only be called once in your robot - * program,. - */ - // public static CommandSwerveDrivetrain createDrivetrain() { - // return new CommandSwerveDrivetrain( - // DRIVETRAIN_CONSTANTS, FRONT_LEFT, FRONT_RIGHT, BACK_LEFT, BACK_RIGHT); - // } + configureModule( + CONSTANT_CREATOR.createModuleConstants( + SC1.BACK_RIGHT_TURN, + SC1.BACK_RIGHT_DRIVE, + SC1.BACK_RIGHT_TURN_ABS_ENC, + Rotations.of(0), + WHEEL_BASE.div(-2.0), + TRACK_WIDTH.div(-2.0), + INVERT_RIGHT_SIDE, + TURN_INVERTED, + TURN_ENCODER_INVERTED)); /** Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. */ public static class TunerSwerveDrivetrain extends SwerveDrivetrain { diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSimTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSimTalonFX.java new file mode 100644 index 0000000..bd3c2eb --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSimTalonFX.java @@ -0,0 +1,92 @@ +// Copyright (c) 2021-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Modified work Copyright (c) 2026 Triple Helix Robotics, FRC Team 2363 +// https://github.com/TripleHelixProgramming +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.robot.subsystems.drive; + +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.sim.CANcoderSimState; +import com.ctre.phoenix6.sim.TalonFXSimState; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import frc.robot.Robot; + +/** + * Module IO implementation using Phoenix 6 TalonFX sim layer for hardware-in-the-loop style + * simulation. Uses the same hardware configuration and control requests as ModuleIOTalonFX, with + * DCMotorSim physics driving the Phoenix sim state. + */ +public class ModuleIOSimTalonFX extends ModuleIOTalonFXBase { + private final TalonFXSimState driveSimState; + private final TalonFXSimState turnSimState; + private final CANcoderSimState cancoderSimState; + + private final DCMotorSim driveSim; + private final DCMotorSim turnSim; + + public ModuleIOSimTalonFX( + SwerveModuleConstants + constants) { + super(constants); + + driveSimState = driveTalon.getSimState(); + turnSimState = turnTalon.getSimState(); + cancoderSimState = cancoder.getSimState(); + + driveSim = DriveConstants.createDriveSim(); + turnSim = DriveConstants.createTurnSim(); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + // Feed battery voltage into Phoenix sim devices + double busVoltage = RoboRioSim.getVInVoltage(); + driveSimState.setSupplyVoltage(busVoltage); + turnSimState.setSupplyVoltage(busVoltage); + cancoderSimState.setSupplyVoltage(busVoltage); + + // Drive physics: Phoenix output voltage → DCMotorSim → rotor state back to Phoenix + driveSim.setInputVoltage( + MathUtil.clamp(driveSimState.getMotorVoltage(), -busVoltage, busVoltage)); + turnSim.setInputVoltage( + MathUtil.clamp(turnSimState.getMotorVoltage(), -busVoltage, busVoltage)); + driveSim.update(Robot.defaultPeriodSecs); + turnSim.update(Robot.defaultPeriodSecs); + + // Feed physics results back to Phoenix sim state (rotor level = mechanism × gear ratio) + driveSimState.setRawRotorPosition( + Units.radiansToRotations(driveSim.getAngularPosition()) * constants.DriveMotorGearRatio); + driveSimState.setRotorVelocity( + Units.radiansToRotations(driveSim.getAngularVelocity()) * constants.DriveMotorGearRatio); + turnSimState.setRawRotorPosition( + Units.radiansToRotations(turnSim.getAngularPosition()) * constants.SteerMotorGearRatio); + turnSimState.setRotorVelocity( + Units.radiansToRotations(turnSim.getAngularVelocity()) * constants.SteerMotorGearRatio); + // CANcoder reports mechanism position (1 rotation = full wheel turn) + cancoderSimState.setRawPosition(Units.radiansToRotations(turnSim.getAngularPosition())); + cancoderSimState.setVelocity(Units.radiansToRotations(turnSim.getAngularVelocity())); + + readSignalInputs(inputs); + + inputs.driveConnected = true; + inputs.turnConnected = true; + inputs.turnEncoderConnected = true; + + // 50Hz odometry (high-frequency odometry in sim doesn't matter) + inputs.odometryTimestamps = new double[] {Timer.getFPGATimestamp()}; + inputs.odometryDrivePositionsRad = new double[] {inputs.drivePositionRad}; + inputs.odometryTurnPositions = new Rotation2d[] {inputs.turnPosition}; + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSimWPI.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSimWPI.java index baf112f..eb2f576 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSimWPI.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSimWPI.java @@ -1,6 +1,9 @@ // Copyright (c) 2021-2026 Littleton Robotics // http://github.com/Mechanical-Advantage // +// Modified work Copyright (c) 2026 Triple Helix Robotics, FRC Team 2363 +// https://github.com/TripleHelixProgramming +// // Use of this source code is governed by a BSD // license that can be found in the LICENSE file // at the root directory of this project. @@ -13,11 +16,11 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.simulation.DCMotorSim; import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import frc.robot.Robot; /** Physics sim implementation of module IO. */ public class ModuleIOSimWPI implements ModuleIO { @@ -45,18 +48,8 @@ public ModuleIOSimWPI( SwerveModuleConstants constants) { // Create drive and turn sim models - driveSim = - new DCMotorSim( - LinearSystemId.createDCMotorSystem( - DriveConstants.DRIVE_GEARBOX, - constants.DriveInertia, - constants.DriveMotorGearRatio), - DriveConstants.DRIVE_GEARBOX); - turnSim = - new DCMotorSim( - LinearSystemId.createDCMotorSystem( - DriveConstants.TURN_GEARBOX, constants.SteerInertia, constants.SteerMotorGearRatio), - DriveConstants.TURN_GEARBOX); + driveSim = DriveConstants.createDriveSim(); + turnSim = DriveConstants.createTurnSim(); // Enable wrapping for turn PID turnController.enableContinuousInput(-Math.PI, Math.PI); @@ -80,8 +73,8 @@ public void updateInputs(ModuleIOInputs inputs) { double busVoltage = RoboRioSim.getVInVoltage(); driveSim.setInputVoltage(MathUtil.clamp(driveAppliedVolts, -busVoltage, busVoltage)); turnSim.setInputVoltage(MathUtil.clamp(turnAppliedVolts, -busVoltage, busVoltage)); - driveSim.update(0.02); - turnSim.update(0.02); + driveSim.update(Robot.defaultPeriodSecs); + turnSim.update(Robot.defaultPeriodSecs); // Update drive inputs inputs.driveConnected = true; diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index a01ad29..7f4bc71 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -1,40 +1,23 @@ // Copyright (c) 2021-2026 Littleton Robotics // http://github.com/Mechanical-Advantage // +// Modified work Copyright (c) 2026 Triple Helix Robotics, FRC Team 2363 +// https://github.com/TripleHelixProgramming +// // Use of this source code is governed by a BSD // license that can be found in the LICENSE file // at the root directory of this project. package frc.robot.subsystems.drive; -import static frc.robot.util.PhoenixUtil.*; - import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC; -import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.controls.TorqueCurrentFOC; -import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; -import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.ParentDevice; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import com.ctre.phoenix6.signals.SensorDirectionValue; import com.ctre.phoenix6.swerve.SwerveModuleConstants; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; -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 frc.robot.Constants.CANBusPorts.SC1; import java.util.Queue; /** @@ -43,46 +26,13 @@ * *

Device configuration and other behaviors not exposed by TunerConstants can be customized here. */ -public class ModuleIOTalonFX implements ModuleIO { - private final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - constants; - - // Hardware objects - private final TalonFX driveTalon; - private final TalonFX turnTalon; - private final CANcoder cancoder; - private final CANcoderConfiguration cancoderConfig = new CANcoderConfiguration(); - - // Voltage control requests - private final VoltageOut voltageRequest = new VoltageOut(0); - private final PositionVoltage positionVoltageRequest = new PositionVoltage(0.0); - private final VelocityVoltage velocityVoltageRequest = new VelocityVoltage(0.0); - - // Torque-current control requests - private final TorqueCurrentFOC torqueCurrentRequest = new TorqueCurrentFOC(0); - private final PositionTorqueCurrentFOC positionTorqueCurrentRequest = - new PositionTorqueCurrentFOC(0.0); - private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest = - new VelocityTorqueCurrentFOC(0.0); - +public class ModuleIOTalonFX extends ModuleIOTalonFXBase { // Timestamp inputs from Phoenix thread private final Queue timestampQueue; - // Inputs from drive motor - private final StatusSignal drivePosition; + // High-frequency odometry queues private final Queue drivePositionQueue; - private final StatusSignal driveVelocity; - private final StatusSignal driveAppliedVolts; - private final StatusSignal driveCurrent; - - // Inputs from turn motor - private final StatusSignal turnAbsolutePosition; - private final StatusSignal turnPosition; private final Queue turnPositionQueue; - private final StatusSignal turnVelocity; - private final StatusSignal turnAppliedVolts; - private final StatusSignal turnCurrent; // Connection debouncers private final Debouncer driveConnectedDebounce = @@ -95,79 +45,12 @@ public class ModuleIOTalonFX implements ModuleIO { public ModuleIOTalonFX( SwerveModuleConstants constants) { - this.constants = constants; - driveTalon = new TalonFX(constants.DriveMotorId, SC1.BUS); - turnTalon = new TalonFX(constants.SteerMotorId, SC1.BUS); - cancoder = new CANcoder(constants.EncoderId, SC1.BUS); - - // Configure drive motor - var driveConfig = constants.DriveMotorInitialConfigs; - driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - driveConfig.Slot0 = constants.DriveMotorGains; - driveConfig.Feedback.SensorToMechanismRatio = constants.DriveMotorGearRatio; - driveConfig.MotorOutput.Inverted = - constants.DriveMotorInverted - ? InvertedValue.Clockwise_Positive - : InvertedValue.CounterClockwise_Positive; - tryUntilOk(5, () -> driveTalon.getConfigurator().apply(driveConfig, 0.25)); - tryUntilOk(5, () -> driveTalon.setPosition(0.0, 0.25)); - - // Configure turn motor - var turnConfig = constants.SteerMotorInitialConfigs; - turnConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - turnConfig.Slot0 = constants.SteerMotorGains; - turnConfig.Feedback.FeedbackRemoteSensorID = constants.EncoderId; - turnConfig.Feedback.FeedbackSensorSource = - switch (constants.FeedbackSource) { - case RemoteCANcoder -> FeedbackSensorSourceValue.RemoteCANcoder; - case FusedCANcoder -> FeedbackSensorSourceValue.FusedCANcoder; - case SyncCANcoder -> FeedbackSensorSourceValue.SyncCANcoder; - default -> - throw new RuntimeException( - "You are using an unsupported swerve configuration, which this template does not support without manual customization. The 2025 release of Phoenix supports some swerve configurations which were not available during 2025 beta testing, preventing any development and support from the AdvantageKit developers."); - }; - turnConfig.Feedback.RotorToSensorRatio = constants.SteerMotorGearRatio; - turnConfig.MotionMagic.MotionMagicCruiseVelocity = 100.0 / constants.SteerMotorGearRatio; - turnConfig.MotionMagic.MotionMagicAcceleration = - turnConfig.MotionMagic.MotionMagicCruiseVelocity / 0.100; - turnConfig.MotionMagic.MotionMagicExpo_kV = 0.12 * constants.SteerMotorGearRatio; - turnConfig.MotionMagic.MotionMagicExpo_kA = 0.1; - turnConfig.ClosedLoopGeneral.ContinuousWrap = true; - turnConfig.MotorOutput.Inverted = - constants.SteerMotorInverted - ? InvertedValue.Clockwise_Positive - : InvertedValue.CounterClockwise_Positive; - tryUntilOk(5, () -> turnTalon.getConfigurator().apply(turnConfig, 0.25)); + super(constants); - // Configure CANCoder - // CANcoderConfiguration cancoderConfig = constants.EncoderInitialConfigs; - // cancoderConfig.MagnetSensor.MagnetOffset = constants.EncoderOffset; - cancoder.getConfigurator().refresh(cancoderConfig); - cancoderConfig.MagnetSensor.SensorDirection = - constants.EncoderInverted - ? SensorDirectionValue.Clockwise_Positive - : SensorDirectionValue.CounterClockwise_Positive; - cancoder.getConfigurator().apply(cancoderConfig); - - // Create timestamp queue timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); - - // Create drive status signals - drivePosition = driveTalon.getPosition(); drivePositionQueue = PhoenixOdometryThread.getInstance().registerSignal(drivePosition.clone()); - driveVelocity = driveTalon.getVelocity(); - driveAppliedVolts = driveTalon.getMotorVoltage(); - driveCurrent = driveTalon.getStatorCurrent(); - - // Create turn status signals - turnAbsolutePosition = cancoder.getAbsolutePosition(); - turnPosition = turnTalon.getPosition(); turnPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(turnPosition.clone()); - turnVelocity = turnTalon.getVelocity(); - turnAppliedVolts = turnTalon.getMotorVoltage(); - turnCurrent = turnTalon.getStatorCurrent(); - // Configure periodic frames BaseStatusSignal.setUpdateFrequencyForAll( Drive.ODOMETRY_FREQUENCY, drivePosition, turnPosition); BaseStatusSignal.setUpdateFrequencyForAll( @@ -184,30 +67,14 @@ public ModuleIOTalonFX( @Override public void updateInputs(ModuleIOInputs inputs) { - BaseStatusSignal.refreshAll( - driveVelocity, - driveAppliedVolts, - driveCurrent, - turnAbsolutePosition, - turnVelocity, - turnAppliedVolts, - turnCurrent, - drivePosition, - turnPosition); + readSignalInputs(inputs); - // Update drive inputs inputs.driveConnected = driveConnectedDebounce.calculate( drivePosition.getStatus().isOK() && driveVelocity.getStatus().isOK() && driveAppliedVolts.getStatus().isOK() && driveCurrent.getStatus().isOK()); - inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()); - inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()); - inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); - inputs.driveCurrentAmps = driveCurrent.getValueAsDouble(); - - // Update turn inputs inputs.turnConnected = turnConnectedDebounce.calculate( turnPosition.getStatus().isOK() @@ -216,14 +83,7 @@ public void updateInputs(ModuleIOInputs inputs) { && turnCurrent.getStatus().isOK()); inputs.turnEncoderConnected = turnEncoderConnectedDebounce.calculate(turnAbsolutePosition.getStatus().isOK()); - inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()); - inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble()); - inputs.turnZero = Rotation2d.fromRotations(cancoderConfig.MagnetSensor.MagnetOffset); - inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()); - inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); - inputs.turnCurrentAmps = turnCurrent.getValueAsDouble(); - // Update odometry inputs inputs.odometryTimestamps = new double[timestampQueue.size()]; for (int i = 0; i < inputs.odometryTimestamps.length; i++) { inputs.odometryTimestamps[i] = timestampQueue.poll(); @@ -237,48 +97,4 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.odometryTurnPositions[i] = Rotation2d.fromRotations(turnPositionQueue.poll()); } } - - @Override - public void setDriveOpenLoop(double output) { - driveTalon.setControl( - switch (constants.DriveMotorClosedLoopOutput) { - case Voltage -> voltageRequest.withOutput(output); - case TorqueCurrentFOC -> torqueCurrentRequest.withOutput(output); - }); - } - - @Override - public void setTurnOpenLoop(double output) { - turnTalon.setControl( - switch (constants.SteerMotorClosedLoopOutput) { - case Voltage -> voltageRequest.withOutput(output); - case TorqueCurrentFOC -> torqueCurrentRequest.withOutput(output); - }); - } - - @Override - public void setDriveVelocity(double velocityRadPerSec) { - double velocityRotPerSec = Units.radiansToRotations(velocityRadPerSec); - driveTalon.setControl( - switch (constants.DriveMotorClosedLoopOutput) { - case Voltage -> velocityVoltageRequest.withVelocity(velocityRotPerSec); - case TorqueCurrentFOC -> velocityTorqueCurrentRequest.withVelocity(velocityRotPerSec); - }); - } - - @Override - public void setTurnPosition(Rotation2d rotation) { - turnTalon.setControl( - switch (constants.SteerMotorClosedLoopOutput) { - case Voltage -> positionVoltageRequest.withPosition(rotation.getRotations()); - case TorqueCurrentFOC -> - positionTorqueCurrentRequest.withPosition(rotation.getRotations()); - }); - } - - @Override - public void setTurnZero(Rotation2d rotation) { - cancoderConfig.MagnetSensor.MagnetOffset = rotation.getRotations(); - cancoder.getConfigurator().apply(cancoderConfig); - } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXBase.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXBase.java new file mode 100644 index 0000000..3e80317 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXBase.java @@ -0,0 +1,170 @@ +// Copyright (c) 2021-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Modified work Copyright (c) 2026 Triple Helix Robotics, FRC Team 2363 +// https://github.com/TripleHelixProgramming +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.robot.subsystems.drive; + +import static frc.robot.util.PhoenixUtil.*; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.TorqueCurrentFOC; +import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +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 frc.robot.Constants.CANBusPorts.SC1; + +/** Shared base for TalonFX-based module IO implementations (hardware and sim). */ +public abstract class ModuleIOTalonFXBase implements ModuleIO { + protected final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + constants; + + protected final TalonFX driveTalon; + protected final TalonFX turnTalon; + protected final CANcoder cancoder; + private final CANcoderConfiguration cancoderConfig = new CANcoderConfiguration(); + + // Control requests + private final VoltageOut voltageRequest = new VoltageOut(0); + private final PositionVoltage positionVoltageRequest = new PositionVoltage(0.0); + private final VelocityVoltage velocityVoltageRequest = new VelocityVoltage(0.0); + private final TorqueCurrentFOC torqueCurrentRequest = new TorqueCurrentFOC(0); + private final PositionTorqueCurrentFOC positionTorqueCurrentRequest = + new PositionTorqueCurrentFOC(0.0); + private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest = + new VelocityTorqueCurrentFOC(0.0); + + // Status signals + protected final StatusSignal drivePosition; + protected final StatusSignal driveVelocity; + protected final StatusSignal driveAppliedVolts; + protected final StatusSignal driveCurrent; + protected final StatusSignal turnAbsolutePosition; + protected final StatusSignal turnPosition; + protected final StatusSignal turnVelocity; + protected final StatusSignal turnAppliedVolts; + protected final StatusSignal turnCurrent; + + protected ModuleIOTalonFXBase( + SwerveModuleConstants + constants) { + this.constants = constants; + driveTalon = new TalonFX(constants.DriveMotorId, SC1.BUS); + turnTalon = new TalonFX(constants.SteerMotorId, SC1.BUS); + cancoder = new CANcoder(constants.EncoderId, SC1.BUS); + + tryUntilOk( + 5, () -> driveTalon.getConfigurator().apply(constants.DriveMotorInitialConfigs, 0.25)); + tryUntilOk(5, () -> driveTalon.setPosition(0.0, 0.25)); + tryUntilOk( + 5, () -> turnTalon.getConfigurator().apply(constants.SteerMotorInitialConfigs, 0.25)); + + cancoder.getConfigurator().refresh(cancoderConfig); + cancoderConfig.MagnetSensor.SensorDirection = + constants.EncoderInverted + ? SensorDirectionValue.Clockwise_Positive + : SensorDirectionValue.CounterClockwise_Positive; + cancoder.getConfigurator().apply(cancoderConfig); + + drivePosition = driveTalon.getPosition(); + driveVelocity = driveTalon.getVelocity(); + driveAppliedVolts = driveTalon.getMotorVoltage(); + driveCurrent = driveTalon.getStatorCurrent(); + turnAbsolutePosition = cancoder.getAbsolutePosition(); + turnPosition = turnTalon.getPosition(); + turnVelocity = turnTalon.getVelocity(); + turnAppliedVolts = turnTalon.getMotorVoltage(); + turnCurrent = turnTalon.getStatorCurrent(); + } + + /** Refreshes all status signals and populates the non-connection, non-odometry inputs. */ + protected void readSignalInputs(ModuleIOInputs inputs) { + BaseStatusSignal.refreshAll( + drivePosition, + driveVelocity, + driveAppliedVolts, + driveCurrent, + turnAbsolutePosition, + turnPosition, + turnVelocity, + turnAppliedVolts, + turnCurrent); + + inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()); + inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()); + inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); + inputs.driveCurrentAmps = driveCurrent.getValueAsDouble(); + + inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()); + inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble()); + inputs.turnZero = Rotation2d.fromRotations(cancoderConfig.MagnetSensor.MagnetOffset); + inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()); + inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); + inputs.turnCurrentAmps = turnCurrent.getValueAsDouble(); + } + + @Override + public void setDriveOpenLoop(double output) { + driveTalon.setControl( + switch (constants.DriveMotorClosedLoopOutput) { + case Voltage -> voltageRequest.withOutput(output); + case TorqueCurrentFOC -> torqueCurrentRequest.withOutput(output); + }); + } + + @Override + public void setTurnOpenLoop(double output) { + turnTalon.setControl( + switch (constants.SteerMotorClosedLoopOutput) { + case Voltage -> voltageRequest.withOutput(output); + case TorqueCurrentFOC -> torqueCurrentRequest.withOutput(output); + }); + } + + @Override + public void setDriveVelocity(double velocityRadPerSec) { + double velocityRotPerSec = Units.radiansToRotations(velocityRadPerSec); + driveTalon.setControl( + switch (constants.DriveMotorClosedLoopOutput) { + case Voltage -> velocityVoltageRequest.withVelocity(velocityRotPerSec); + case TorqueCurrentFOC -> velocityTorqueCurrentRequest.withVelocity(velocityRotPerSec); + }); + } + + @Override + public void setTurnPosition(Rotation2d rotation) { + turnTalon.setControl( + switch (constants.SteerMotorClosedLoopOutput) { + case Voltage -> positionVoltageRequest.withPosition(rotation.getRotations()); + case TorqueCurrentFOC -> + positionTorqueCurrentRequest.withPosition(rotation.getRotations()); + }); + } + + @Override + public void setTurnZero(Rotation2d rotation) { + cancoderConfig.MagnetSensor.MagnetOffset = rotation.getRotations(); + cancoder.getConfigurator().apply(cancoderConfig); + } +}