From e8f92cd35cb1d78da17e99a4eaadea58dfeb831f Mon Sep 17 00:00:00 2001 From: caTrixLiu Date: Wed, 25 Dec 2024 19:13:25 +0800 Subject: [PATCH] implemented ctre motor controller closed loops --- src/main/java/frc/robot/RobotContainer.java | 8 +- .../subsystems/drive/IO/GyroIOPigeon2.java | 2 +- .../robot/subsystems/drive/IO/ModuleIO.java | 53 ++- .../subsystems/drive/IO/ModuleIOSim.java | 93 ++++- .../subsystems/drive/IO/ModuleIOSpark.java | 349 +++++++++--------- .../subsystems/drive/IO/ModuleIOTalon.java | 249 ++++++++----- .../subsystems/drive/IO/OdometryThread.java | 2 +- .../robot/subsystems/drive/SwerveModule.java | 38 +- .../java/frc/robot/utils/PhoenixUtil.java | 27 ++ 9 files changed, 520 insertions(+), 301 deletions(-) create mode 100644 src/main/java/frc/robot/utils/PhoenixUtil.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cdcb656..3c98b59 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -89,10 +89,10 @@ public RobotContainer() { drive = new SwerveDrive( SwerveDrive.DriveType.CTRE_ON_CANIVORE, new GyroIOPigeon2(TunerConstants.DrivetrainConstants), - new ModuleIOTalon(TunerConstants.DrivetrainConstants, TunerConstants.FrontLeft, "FrontLeft"), - new ModuleIOTalon(TunerConstants.DrivetrainConstants, TunerConstants.FrontRight, "FrontRight"), - new ModuleIOTalon(TunerConstants.DrivetrainConstants, TunerConstants.BackLeft, "BackLeft"), - new ModuleIOTalon(TunerConstants.DrivetrainConstants, TunerConstants.BackRight, "BackRight")); + new ModuleIOTalon(TunerConstants.FrontLeft, "FrontLeft"), + new ModuleIOTalon(TunerConstants.FrontRight, "FrontRight"), + new ModuleIOTalon(TunerConstants.BackLeft, "BackLeft"), + new ModuleIOTalon(TunerConstants.BackRight, "BackRight")); /* REV Chassis */ // drive = new SwerveDrive( diff --git a/src/main/java/frc/robot/subsystems/drive/IO/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/IO/GyroIOPigeon2.java index 5b4d5d0..ea443d5 100644 --- a/src/main/java/frc/robot/subsystems/drive/IO/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/IO/GyroIOPigeon2.java @@ -37,7 +37,7 @@ public GyroIOPigeon2(int Pigeon2Id, String CANbusName, Pigeon2Configuration Pige yawVelocity = pigeon.getAngularVelocityZWorld(); yawVelocity.setUpdateFrequency(100.0); - yawPositionInput = OdometryThread.registerSignalInput(pigeon.getYaw()); + yawPositionInput = OdometryThread.registerSignalSignal(pigeon.getYaw()); pigeon.optimizeBusUtilization(); } diff --git a/src/main/java/frc/robot/subsystems/drive/IO/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/IO/ModuleIO.java index b2fdbc9..8521f0a 100644 --- a/src/main/java/frc/robot/subsystems/drive/IO/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/IO/ModuleIO.java @@ -5,7 +5,11 @@ package frc.robot.subsystems.drive.IO; +import static edu.wpi.first.units.Units.Volts; + import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; import org.littletonrobotics.junction.AutoLog; public interface ModuleIO { @@ -24,7 +28,9 @@ class ModuleIOInputs { public double[] odometryDriveWheelRevolutions = new double[] {}; public Rotation2d[] odometrySteerPositions = new Rotation2d[] {}; - public boolean hardwareConnected = false; + public boolean driveMotorConnected = false; + public boolean steerMotorConnected = false; + public boolean steerEncoderConnected = false; } /** Updates the inputs */ @@ -32,19 +38,52 @@ class ModuleIOInputs { default void calibrate() {} + default void stop() { + requestDriveOpenLoop(Volts.zero()); + requestSteerOpenLoop(Volts.zero()); + } + + /** + * Run the drive motor at a specified voltage open-loop control + * + * @param output the desired voltage output, from -12v to 12v + */ + default void requestDriveOpenLoop(Voltage output) {} + + /** + * Run the drive motor at a specified current open-loop control + * + * @param output the desired current output + */ + default void requestDriveOpenLoop(Current output) {} + + /** + * Run the steer motor at a specified voltage open-loop control + * + * @param output the desired voltage output, from -12v to 12v + */ + default void requestSteerOpenLoop(Voltage output) {} + + /** + * Run the steer motor at a specified current open-loop control + * + * @param output the desired current output + */ + default void requestSteerOpenLoop(Current output) {} + /** - * Run the drive motor at the specified percent speed. + * Runs a velocity close-loop control on the drive motor * - * @param speedPercent from -1 to 1, where 1 is the forward direction of the wheel + * @param desiredWheelVelocityRadPerSec the desired angular velocity of the wheel, in radians / second */ - default void setDriveVoltage(double speedPercent) {} + default void requestDriveVelocityControl(double desiredWheelVelocityRadPerSec) {} /** - * Run the turn motor at the specified percent power. + * Runs a position close-loop control on the steer motor * - * @param powerPercent from -1 to 1, where 1 is counter-clockwise + * @param desiredSteerAbsoluteFacing the desired facing of the steer */ - default void setSteerPowerPercent(double powerPercent) {} + default void requestSteerPositionControl(Rotation2d desiredSteerAbsoluteFacing) {} /** Enable or disable brake mode on the drive motor. */ default void setDriveBrake(boolean enable) {} diff --git a/src/main/java/frc/robot/subsystems/drive/IO/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/IO/ModuleIOSim.java index 7e702b4..2a1c492 100644 --- a/src/main/java/frc/robot/subsystems/drive/IO/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/IO/ModuleIOSim.java @@ -7,8 +7,14 @@ import static edu.wpi.first.units.Units.*; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; import frc.robot.constants.DriveTrainConstants; import java.util.Arrays; +import org.ironmaple.simulation.SimulatedArena; import org.ironmaple.simulation.drivesims.SwerveModuleSimulation; import org.ironmaple.simulation.motorsims.SimulatedMotorController; @@ -19,9 +25,23 @@ * The flywheel sims are not physically accurate, but provide a decent approximation for the behavior of the module. */ public class ModuleIOSim implements ModuleIO { + private static final double DRIVE_KS = 0.03; + private static final double DRIVE_KP = 0.05; + private static final double STEER_KP = 8.0; + private static final double STEER_KD = 0.0; + private final SwerveModuleSimulation moduleSimulation; private final SimulatedMotorController.GenericMotorController driveMotor, steerMotor; + private boolean driveClosedLoopActivated = false; + private boolean steerClosedLoopActivated = false; + private final PIDController driveController; + private final PIDController steerController; + private double driveAppliedVolts = 0.0; + private double steerAppliedVolts = 0.0; + private double desiredWheelVelocityRadPerSec = 0.0; + private Rotation2d desiredSteerFacing = new Rotation2d(); + public ModuleIOSim(SwerveModuleSimulation moduleSimulation) { this.moduleSimulation = moduleSimulation; this.driveMotor = moduleSimulation @@ -30,6 +50,13 @@ public ModuleIOSim(SwerveModuleSimulation moduleSimulation) { this.steerMotor = moduleSimulation .useGenericControllerForSteer() .withCurrentLimit(DriveTrainConstants.STEER_CURRENT_LIMIT); + + this.driveController = new PIDController(DRIVE_KP, 0.0, 0.0); + this.steerController = new PIDController(STEER_KP, 0.0, STEER_KD); + + // Enable wrapping for turn PID + steerController.enableContinuousInput(-Math.PI, Math.PI); + SimulatedArena.getInstance().addCustomSimulation((subTickNum) -> runControlLoops()); } @Override @@ -55,17 +82,73 @@ public void updateInputs(ModuleIOInputs inputs) { .mapToDouble(angle -> angle.in(Revolutions)) .toArray(); inputs.odometrySteerPositions = moduleSimulation.getCachedSteerAbsolutePositions(); + } + + public void runControlLoops() { + // Run control loops if activated + if (driveClosedLoopActivated) calculateDriveControlLoops(); + else driveController.reset(); + if (steerClosedLoopActivated) calculateSteerControlLoops(); + else steerController.reset(); + + // Feed voltage to motor simulation + driveMotor.requestVoltage(Volts.of(driveAppliedVolts)); + steerMotor.requestVoltage(Volts.of(steerAppliedVolts)); + } + + private void calculateDriveControlLoops() { + double ffVolts = moduleSimulation.driveMotorConfigs.motor.getVoltage( + 0, desiredWheelVelocityRadPerSec * moduleSimulation.DRIVE_GEAR_RATIO) + + Math.signum(desiredWheelVelocityRadPerSec) * DRIVE_KS; + double fbVolts = driveController.calculate( + moduleSimulation.getDriveWheelFinalSpeed().in(RadiansPerSecond), desiredWheelVelocityRadPerSec); + driveAppliedVolts = ffVolts + fbVolts; + } + + private void calculateSteerControlLoops() { + steerAppliedVolts = steerController.calculate( + moduleSimulation.getSteerAbsoluteFacing().getRadians(), desiredSteerFacing.getRadians()); + } + + @Override + public void requestDriveOpenLoop(Voltage output) { + this.driveAppliedVolts = output.in(Volts); + this.driveClosedLoopActivated = false; + } - inputs.hardwareConnected = true; + @Override + public void requestSteerOpenLoop(Voltage output) { + this.steerAppliedVolts = output.in(Volts); + this.steerClosedLoopActivated = false; + } + + @Override + public void requestDriveOpenLoop(Current output) { + DCMotor driveMotorModel = moduleSimulation.driveMotorConfigs.motor; + this.driveAppliedVolts = driveMotorModel.getVoltage( + driveMotorModel.getCurrent(output.in(Amps)), + moduleSimulation.getDriveEncoderUnGearedSpeed().in(RadiansPerSecond)); + this.driveClosedLoopActivated = false; + } + + @Override + public void requestSteerOpenLoop(Current output) { + DCMotor steerMotorModel = moduleSimulation.getSteerMotorConfigs().motor; + this.driveAppliedVolts = steerMotorModel.getVoltage( + steerMotorModel.getCurrent(output.in(Amps)), + moduleSimulation.getDriveEncoderUnGearedSpeed().in(RadiansPerSecond)); + this.steerClosedLoopActivated = false; } @Override - public void setDriveVoltage(double volts) { - driveMotor.requestVoltage(Volts.of(volts)); + public void requestDriveVelocityControl(double desiredWheelVelocityRadPerSec) { + this.desiredWheelVelocityRadPerSec = desiredWheelVelocityRadPerSec; + this.driveClosedLoopActivated = true; } @Override - public void setSteerPowerPercent(double powerPercent) { - steerMotor.requestVoltage(Volts.of(12).times(powerPercent)); + public void requestSteerPositionControl(Rotation2d desiredSteerAbsoluteFacing) { + this.desiredSteerFacing = desiredSteerAbsoluteFacing; + this.steerClosedLoopActivated = true; } } diff --git a/src/main/java/frc/robot/subsystems/drive/IO/ModuleIOSpark.java b/src/main/java/frc/robot/subsystems/drive/IO/ModuleIOSpark.java index 972d0d7..aeb1eb4 100644 --- a/src/main/java/frc/robot/subsystems/drive/IO/ModuleIOSpark.java +++ b/src/main/java/frc/robot/subsystems/drive/IO/ModuleIOSpark.java @@ -1,174 +1,175 @@ -// Original Source: -// https://github.com/Mechanical-Advantage/AdvantageKit/tree/main/example_projects/advanced_swerve_drive/src/main, -// Copyright 2021-2024 FRC 6328 -// Modified by 5516 Iron Maple https://github.com/Shenzhen-Robotics-Alliance/ - -package frc.robot.subsystems.drive.IO; - -import static frc.robot.constants.DriveTrainConstants.*; - -import com.revrobotics.CANSparkBase.IdleMode; -import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.CANSparkLowLevel.PeriodicFrame; -import com.revrobotics.CANSparkMax; -import com.revrobotics.RelativeEncoder; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.AnalogInput; -import edu.wpi.first.wpilibj.RobotController; -import java.util.Queue; - -/** - * Module IO implementation for SparkMax drive motor controller, SparkMax turn motor controller (NEO or NEO 550), and - * analog absolute encoder connected to the RIO - * - *

NOTE: This implementation should be used as a starting point and adapted to different hardware configurations - * (e.g. If using a CANcoder, copy from "ModuleIOTalonFX") - * - *

To calibrate the absolute encoder offsets, point the modules straight (such that forward motion on the drive motor - * will propel the robot forward) and copy the reported values from the absolute encoders using AdvantageScope. These - * values are logged under "/Drive/ModuleX/TurnAbsolutePositionRad" - */ -public class ModuleIOSpark implements ModuleIO { - // Gear ratios for SDS MK4i L2, adjust as necessary - private static final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); - private static final double STEER_GEAR_RATIO = 150.0 / 7.0; - - private final CANSparkMax driveSparkMax; - private final CANSparkMax steerSparkMax; - - private final RelativeEncoder driveEncoder; - private final RelativeEncoder steerRelativeEncoder; - private final AnalogInput turnAbsoluteEncoder; - private final Queue drivePositionInput; - private final Queue steerRelativeEncoderPositionUngeared; - - private final boolean isTurnMotorInverted = true; - private final Rotation2d absoluteEncoderOffset; - - public ModuleIOSpark(int index) { - switch (index) { - case 0 -> { - driveSparkMax = new CANSparkMax(1, MotorType.kBrushless); - steerSparkMax = new CANSparkMax(2, MotorType.kBrushless); - turnAbsoluteEncoder = new AnalogInput(0); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - } - case 1 -> { - driveSparkMax = new CANSparkMax(3, MotorType.kBrushless); - steerSparkMax = new CANSparkMax(4, MotorType.kBrushless); - turnAbsoluteEncoder = new AnalogInput(1); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - } - case 2 -> { - driveSparkMax = new CANSparkMax(5, MotorType.kBrushless); - steerSparkMax = new CANSparkMax(6, MotorType.kBrushless); - turnAbsoluteEncoder = new AnalogInput(2); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - } - case 3 -> { - driveSparkMax = new CANSparkMax(7, MotorType.kBrushless); - steerSparkMax = new CANSparkMax(8, MotorType.kBrushless); - turnAbsoluteEncoder = new AnalogInput(3); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - } - default -> throw new RuntimeException("Invalid module index"); - } - - driveSparkMax.restoreFactoryDefaults(); - steerSparkMax.restoreFactoryDefaults(); - - driveSparkMax.setCANTimeout(250); - steerSparkMax.setCANTimeout(250); - - driveEncoder = driveSparkMax.getEncoder(); - steerRelativeEncoder = steerSparkMax.getEncoder(); - - steerSparkMax.setInverted(isTurnMotorInverted); - driveSparkMax.setSmartCurrentLimit(40); - steerSparkMax.setSmartCurrentLimit(30); - driveSparkMax.enableVoltageCompensation(12.0); - steerSparkMax.enableVoltageCompensation(12.0); - - driveEncoder.setPosition(0.0); - driveEncoder.setMeasurementPeriod(10); - driveEncoder.setAverageDepth(2); - - steerRelativeEncoder.setPosition(0.0); - steerRelativeEncoder.setMeasurementPeriod(10); - steerRelativeEncoder.setAverageDepth(2); - - driveSparkMax.setCANTimeout(0); - steerSparkMax.setCANTimeout(0); - - driveSparkMax.setPeriodicFramePeriod(PeriodicFrame.kStatus2, (int) (1000.0 / ODOMETRY_FREQUENCY)); - steerSparkMax.setPeriodicFramePeriod(PeriodicFrame.kStatus2, (int) (1000.0 / ODOMETRY_FREQUENCY)); - this.drivePositionInput = OdometryThread.registerInput(driveEncoder::getPosition); - this.steerRelativeEncoderPositionUngeared = OdometryThread.registerInput(steerRelativeEncoder::getPosition); - - driveSparkMax.burnFlash(); - steerSparkMax.burnFlash(); - } - - @Override - public void updateInputs(ModuleIOInputs inputs) { - inputs.driveWheelFinalRevolutions = driveEncoder.getPosition() / DRIVE_GEAR_RATIO; - final double RPM_TO_REVOLUTIONS_PER_SECOND = 1.0 / 60.0; - inputs.driveWheelFinalVelocityRevolutionsPerSec = - driveEncoder.getVelocity() / DRIVE_GEAR_RATIO * RPM_TO_REVOLUTIONS_PER_SECOND; - - inputs.driveMotorAppliedVolts = driveSparkMax.getAppliedOutput() * driveSparkMax.getBusVoltage(); - inputs.driveMotorCurrentAmps = driveSparkMax.getOutputCurrent(); - - inputs.steerFacing = Rotation2d.fromRotations(steerRelativeEncoder.getPosition() / STEER_GEAR_RATIO) - .minus(steerRelativePositionEncoderOffset); - inputs.steerVelocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond(steerRelativeEncoder.getVelocity() / STEER_GEAR_RATIO); - - inputs.steerMotorAppliedVolts = steerSparkMax.getAppliedOutput() * steerSparkMax.getBusVoltage(); - inputs.steerMotorCurrentAmps = steerSparkMax.getOutputCurrent(); - - inputs.odometryDriveWheelRevolutions = drivePositionInput.stream() - .mapToDouble(value -> Units.rotationsToRadians(value) / DRIVE_GEAR_RATIO) - .toArray(); - drivePositionInput.clear(); - inputs.odometrySteerPositions = steerRelativeEncoderPositionUngeared.stream() - .map(value -> - Rotation2d.fromRotations(value / STEER_GEAR_RATIO).minus(steerRelativePositionEncoderOffset)) - .toArray(Rotation2d[]::new); - steerRelativeEncoderPositionUngeared.clear(); - } - - private Rotation2d steerRelativePositionEncoderOffset = new Rotation2d(); - - @Override - public void calibrate() { - final Rotation2d steerActualFacing = Rotation2d.fromRotations( - turnAbsoluteEncoder.getVoltage() / RobotController.getVoltage5V()) - .minus(absoluteEncoderOffset); - final Rotation2d relativeEncoderReportedFacing = - Rotation2d.fromRotations(steerRelativeEncoder.getPosition() / STEER_GEAR_RATIO); - /* reported - offset = actual, so offset = reported - actual */ - steerRelativePositionEncoderOffset = relativeEncoderReportedFacing.minus(steerActualFacing); - } - - @Override - public void setDriveVoltage(double volts) { - driveSparkMax.set(volts / RobotController.getBatteryVoltage()); - } - - @Override - public void setSteerPowerPercent(double powerPercent) { - steerSparkMax.set(powerPercent); - } - - @Override - public void setDriveBrake(boolean enable) { - driveSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); - } - - @Override - public void setSteerBrake(boolean enable) { - steerSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); - } -} +//// Original Source: +//// https://github.com/Mechanical-Advantage/AdvantageKit/tree/main/example_projects/advanced_swerve_drive/src/main, +//// Copyright 2021-2024 FRC 6328 +//// Modified by 5516 Iron Maple https://github.com/Shenzhen-Robotics-Alliance/ +// +// package frc.robot.subsystems.drive.IO; +// +// import static frc.robot.constants.DriveTrainConstants.*; +// +// import com.revrobotics.CANSparkBase.IdleMode; +// import com.revrobotics.CANSparkLowLevel.MotorType; +// import com.revrobotics.CANSparkLowLevel.PeriodicFrame; +// import com.revrobotics.CANSparkMax; +// import com.revrobotics.RelativeEncoder; +// import edu.wpi.first.math.geometry.Rotation2d; +// import edu.wpi.first.math.util.Units; +// import edu.wpi.first.wpilibj.AnalogInput; +// import edu.wpi.first.wpilibj.RobotController; +// import java.util.Queue; +// +/// ** +// * Module IO implementation for SparkMax drive motor controller, SparkMax turn motor controller (NEO or NEO 550), and +// * analog absolute encoder connected to the RIO +// * +// *

NOTE: This implementation should be used as a starting point and adapted to different hardware configurations +// * (e.g. If using a CANcoder, copy from "ModuleIOTalonFX") +// * +// *

To calibrate the absolute encoder offsets, point the modules straight (such that forward motion on the drive +// motor +// * will propel the robot forward) and copy the reported values from the absolute encoders using AdvantageScope. These +// * values are logged under "/Drive/ModuleX/TurnAbsolutePositionRad" +// */ +// public class ModuleIOSpark implements ModuleIO { +// // Gear ratios for SDS MK4i L2, adjust as necessary +// private static final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); +// private static final double STEER_GEAR_RATIO = 150.0 / 7.0; +// +// private final CANSparkMax driveSparkMax; +// private final CANSparkMax steerSparkMax; +// +// private final RelativeEncoder driveEncoder; +// private final RelativeEncoder steerRelativeEncoder; +// private final AnalogInput turnAbsoluteEncoder; +// private final Queue drivePositionInput; +// private final Queue steerRelativeEncoderPositionUngeared; +// +// private final boolean isTurnMotorInverted = true; +// private final Rotation2d absoluteEncoderOffset; +// +// public ModuleIOSpark(int index) { +// switch (index) { +// case 0 -> { +// driveSparkMax = new CANSparkMax(1, MotorType.kBrushless); +// steerSparkMax = new CANSparkMax(2, MotorType.kBrushless); +// turnAbsoluteEncoder = new AnalogInput(0); +// absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED +// } +// case 1 -> { +// driveSparkMax = new CANSparkMax(3, MotorType.kBrushless); +// steerSparkMax = new CANSparkMax(4, MotorType.kBrushless); +// turnAbsoluteEncoder = new AnalogInput(1); +// absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED +// } +// case 2 -> { +// driveSparkMax = new CANSparkMax(5, MotorType.kBrushless); +// steerSparkMax = new CANSparkMax(6, MotorType.kBrushless); +// turnAbsoluteEncoder = new AnalogInput(2); +// absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED +// } +// case 3 -> { +// driveSparkMax = new CANSparkMax(7, MotorType.kBrushless); +// steerSparkMax = new CANSparkMax(8, MotorType.kBrushless); +// turnAbsoluteEncoder = new AnalogInput(3); +// absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED +// } +// default -> throw new RuntimeException("Invalid module index"); +// } +// +// driveSparkMax.restoreFactoryDefaults(); +// steerSparkMax.restoreFactoryDefaults(); +// +// driveSparkMax.setCANTimeout(250); +// steerSparkMax.setCANTimeout(250); +// +// driveEncoder = driveSparkMax.getEncoder(); +// steerRelativeEncoder = steerSparkMax.getEncoder(); +// +// steerSparkMax.setInverted(isTurnMotorInverted); +// driveSparkMax.setSmartCurrentLimit(40); +// steerSparkMax.setSmartCurrentLimit(30); +// driveSparkMax.enableVoltageCompensation(12.0); +// steerSparkMax.enableVoltageCompensation(12.0); +// +// driveEncoder.setPosition(0.0); +// driveEncoder.setMeasurementPeriod(10); +// driveEncoder.setAverageDepth(2); +// +// steerRelativeEncoder.setPosition(0.0); +// steerRelativeEncoder.setMeasurementPeriod(10); +// steerRelativeEncoder.setAverageDepth(2); +// +// driveSparkMax.setCANTimeout(0); +// steerSparkMax.setCANTimeout(0); +// +// driveSparkMax.setPeriodicFramePeriod(PeriodicFrame.kStatus2, (int) (1000.0 / ODOMETRY_FREQUENCY)); +// steerSparkMax.setPeriodicFramePeriod(PeriodicFrame.kStatus2, (int) (1000.0 / ODOMETRY_FREQUENCY)); +// this.drivePositionInput = OdometryThread.registerInput(driveEncoder::getPosition); +// this.steerRelativeEncoderPositionUngeared = OdometryThread.registerInput(steerRelativeEncoder::getPosition); +// +// driveSparkMax.burnFlash(); +// steerSparkMax.burnFlash(); +// } +// +// @Override +// public void updateInputs(ModuleIOInputs inputs) { +// inputs.driveWheelFinalRevolutions = driveEncoder.getPosition() / DRIVE_GEAR_RATIO; +// final double RPM_TO_REVOLUTIONS_PER_SECOND = 1.0 / 60.0; +// inputs.driveWheelFinalVelocityRevolutionsPerSec = +// driveEncoder.getVelocity() / DRIVE_GEAR_RATIO * RPM_TO_REVOLUTIONS_PER_SECOND; +// +// inputs.driveMotorAppliedVolts = driveSparkMax.getAppliedOutput() * driveSparkMax.getBusVoltage(); +// inputs.driveMotorCurrentAmps = driveSparkMax.getOutputCurrent(); +// +// inputs.steerFacing = Rotation2d.fromRotations(steerRelativeEncoder.getPosition() / STEER_GEAR_RATIO) +// .minus(steerRelativePositionEncoderOffset); +// inputs.steerVelocityRadPerSec = +// Units.rotationsPerMinuteToRadiansPerSecond(steerRelativeEncoder.getVelocity() / STEER_GEAR_RATIO); +// +// inputs.steerMotorAppliedVolts = steerSparkMax.getAppliedOutput() * steerSparkMax.getBusVoltage(); +// inputs.steerMotorCurrentAmps = steerSparkMax.getOutputCurrent(); +// +// inputs.odometryDriveWheelRevolutions = drivePositionInput.stream() +// .mapToDouble(value -> Units.rotationsToRadians(value) / DRIVE_GEAR_RATIO) +// .toArray(); +// drivePositionInput.clear(); +// inputs.odometrySteerPositions = steerRelativeEncoderPositionUngeared.stream() +// .map(value -> +// Rotation2d.fromRotations(value / STEER_GEAR_RATIO).minus(steerRelativePositionEncoderOffset)) +// .toArray(Rotation2d[]::new); +// steerRelativeEncoderPositionUngeared.clear(); +// } +// +// private Rotation2d steerRelativePositionEncoderOffset = new Rotation2d(); +// +// @Override +// public void calibrate() { +// final Rotation2d steerActualFacing = Rotation2d.fromRotations( +// turnAbsoluteEncoder.getVoltage() / RobotController.getVoltage5V()) +// .minus(absoluteEncoderOffset); +// final Rotation2d relativeEncoderReportedFacing = +// Rotation2d.fromRotations(steerRelativeEncoder.getPosition() / STEER_GEAR_RATIO); +// /* reported - offset = actual, so offset = reported - actual */ +// steerRelativePositionEncoderOffset = relativeEncoderReportedFacing.minus(steerActualFacing); +// } +// +// @Override +// public void setDriveVoltage(double volts) { +// driveSparkMax.set(volts / RobotController.getBatteryVoltage()); +// } +// +// @Override +// public void setSteerPowerPercent(double powerPercent) { +// steerSparkMax.set(powerPercent); +// } +// +// @Override +// public void setDriveBrake(boolean enable) { +// driveSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); +// } +// +// @Override +// public void setSteerBrake(boolean enable) { +// steerSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); +// } +// } diff --git a/src/main/java/frc/robot/subsystems/drive/IO/ModuleIOTalon.java b/src/main/java/frc/robot/subsystems/drive/IO/ModuleIOTalon.java index 6ef7c71..55ad96e 100644 --- a/src/main/java/frc/robot/subsystems/drive/IO/ModuleIOTalon.java +++ b/src/main/java/frc/robot/subsystems/drive/IO/ModuleIOTalon.java @@ -7,143 +7,222 @@ import static edu.wpi.first.units.Units.*; import static frc.robot.constants.DriveTrainConstants.*; +import static frc.robot.utils.PhoenixUtil.tryUntilOk; import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.controls.DutyCycleOut; -import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.*; 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.swerve.SwerveDrivetrainConstants; +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.Robot; +import frc.robot.constants.TunerConstants; import java.util.Queue; public class ModuleIOTalon implements ModuleIO { private final String name; + + // Module Constants + private final SwerveModuleConstants moduleConstants; + + // Hardware objects private final TalonFX driveTalon; private final TalonFX steerTalon; private final CANcoder cancoder; - private final Queue driveEncoderUngearedPosition; - private final StatusSignal driveEncoderUngearedVelocity; - private final StatusSignal driveMotorAppliedVoltage; - private final StatusSignal driveMotorCurrent; - - private final Queue steerEncoderAbsolutePosition; - private final StatusSignal steerEncoderVelocity; - private final StatusSignal steerMotorAppliedVolts; - private final StatusSignal steerMotorCurrent; - - private final BaseStatusSignal[] periodicallyRefreshedSignals; - - private final double DRIVE_GEAR_RATIO; - - public ModuleIOTalon( - SwerveDrivetrainConstants drivetrainConstants, SwerveModuleConstants moduleConstants, String name) { + // Inputs from drive motor + private final StatusSignal driveRotterPosition; + private final Queue driveRotterPositionRotations; + private final StatusSignal driveRotterVelocity; + private final StatusSignal driveAppliedVoltage; + private final StatusSignal driveMotorCurrentDrawn; + + // Inputs from turn motor + private final StatusSignal steerAbsolutePosition; + private final Queue steerAbsolutePositionCache; + private final StatusSignal steerFinalMechanismVelocity; + private final StatusSignal steerAppliedVoltage; + private final StatusSignal steerMotorCurrentDrawn; + + // Connection debouncers + private final Debouncer driveConnectedDebounce = new Debouncer(0.5); + private final Debouncer steerConnectedDebounce = new Debouncer(0.5); + private final Debouncer steerEncoderConnectedDebounce = new Debouncer(0.5); + + public ModuleIOTalon(SwerveModuleConstants moduleConstants, String name) { this.name = name; - driveTalon = new TalonFX(moduleConstants.DriveMotorId, drivetrainConstants.CANBusName); - steerTalon = new TalonFX(moduleConstants.SteerMotorId, drivetrainConstants.CANBusName); - cancoder = new CANcoder(moduleConstants.CANcoderId, drivetrainConstants.CANBusName); + this.moduleConstants = moduleConstants; + driveTalon = new TalonFX(moduleConstants.DriveMotorId, TunerConstants.DrivetrainConstants.CANBusName); + steerTalon = new TalonFX(moduleConstants.SteerMotorId, TunerConstants.DrivetrainConstants.CANBusName); + cancoder = new CANcoder(moduleConstants.CANcoderId, TunerConstants.DrivetrainConstants.CANBusName); + // Configure drive motor var driveConfig = moduleConstants.DriveMotorInitialConfigs; - driveConfig.CurrentLimits.StatorCurrentLimit = DRIVE_CURRENT_LIMIT_ANTI_SLIP.in(Amps); + driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + driveConfig.Slot0 = moduleConstants.DriveMotorGains; + driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = moduleConstants.SlipCurrent; + driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -moduleConstants.SlipCurrent; + driveConfig.CurrentLimits.StatorCurrentLimit = moduleConstants.SlipCurrent; driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; driveConfig.MotorOutput.Inverted = moduleConstants.DriveMotorInverted ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; - driveTalon.getConfigurator().apply(driveConfig); - setDriveBrake(true); - - var steerConfig = moduleConstants.SteerMotorInitialConfigs; - steerConfig.CurrentLimits.StatorCurrentLimit = STEER_CURRENT_LIMIT.in(Amps); - steerConfig.CurrentLimits.StatorCurrentLimitEnable = true; - steerConfig.MotorOutput.Inverted = moduleConstants.SteerMotorInverted + tryUntilOk(5, () -> driveTalon.getConfigurator().apply(driveConfig, 0.25)); + tryUntilOk(5, () -> driveTalon.setPosition(0.0, 0.25)); + + // Configure turn motor + var turnConfig = new TalonFXConfiguration(); + turnConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + turnConfig.Slot0 = moduleConstants.SteerMotorGains; + turnConfig.Feedback.FeedbackRemoteSensorID = moduleConstants.CANcoderId; + turnConfig.Feedback.FeedbackSensorSource = switch (moduleConstants.FeedbackSource) { + case RemoteCANcoder -> FeedbackSensorSourceValue.RemoteCANcoder; + case FusedCANcoder -> FeedbackSensorSourceValue.FusedCANcoder; + case SyncCANcoder -> FeedbackSensorSourceValue.SyncCANcoder;}; + turnConfig.Feedback.RotorToSensorRatio = moduleConstants.SteerMotorGearRatio; + turnConfig.MotionMagic.MotionMagicCruiseVelocity = 100.0 / moduleConstants.SteerMotorGearRatio; + turnConfig.MotionMagic.MotionMagicAcceleration = turnConfig.MotionMagic.MotionMagicCruiseVelocity / 0.100; + turnConfig.MotionMagic.MotionMagicExpo_kV = 0.12 * moduleConstants.SteerMotorGearRatio; + turnConfig.MotionMagic.MotionMagicExpo_kA = 0.1; + turnConfig.ClosedLoopGeneral.ContinuousWrap = true; + turnConfig.MotorOutput.Inverted = moduleConstants.SteerMotorInverted ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; - steerTalon.getConfigurator().apply(steerConfig); - setSteerBrake(true); - - var encoderConfig = moduleConstants.CANcoderInitialConfigs; - encoderConfig.MagnetSensor.MagnetOffset = moduleConstants.CANcoderOffset; - cancoder.getConfigurator().apply(encoderConfig); - - driveEncoderUngearedPosition = OdometryThread.registerSignalInput(driveTalon.getPosition()); - driveEncoderUngearedVelocity = driveTalon.getVelocity(); - driveMotorAppliedVoltage = driveTalon.getMotorVoltage(); - driveMotorCurrent = driveTalon.getStatorCurrent(); - - steerEncoderAbsolutePosition = OdometryThread.registerSignalInput(cancoder.getAbsolutePosition()); - steerEncoderVelocity = cancoder.getVelocity(); - steerMotorAppliedVolts = steerTalon.getMotorVoltage(); - steerMotorCurrent = steerTalon.getStatorCurrent(); - - periodicallyRefreshedSignals = new BaseStatusSignal[] { - driveEncoderUngearedVelocity, driveMotorAppliedVoltage, - driveMotorCurrent, steerEncoderVelocity, - steerMotorAppliedVolts, steerMotorCurrent - }; - - BaseStatusSignal.setUpdateFrequencyForAll(50.0, periodicallyRefreshedSignals); - driveTalon.optimizeBusUtilization(); - steerTalon.optimizeBusUtilization(); - - this.DRIVE_GEAR_RATIO = moduleConstants.DriveMotorGearRatio; + tryUntilOk(5, () -> steerTalon.getConfigurator().apply(turnConfig, 0.25)); + + // Configure CANCoder + var cancoderConfig = moduleConstants.CANcoderInitialConfigs; + cancoderConfig.MagnetSensor.MagnetOffset = moduleConstants.CANcoderOffset; + cancoderConfig.MagnetSensor.SensorDirection = moduleConstants.CANcoderInverted + ? SensorDirectionValue.Clockwise_Positive + : SensorDirectionValue.CounterClockwise_Positive; + cancoder.getConfigurator().apply(cancoderConfig); + + // Create drive status signals + driveRotterPosition = driveTalon.getPosition(); + driveRotterPositionRotations = OdometryThread.registerSignalSignal(driveTalon.getPosition()); + driveRotterVelocity = driveTalon.getVelocity(); + driveAppliedVoltage = driveTalon.getMotorVoltage(); + driveMotorCurrentDrawn = driveTalon.getStatorCurrent(); + + // Create turn status signals + steerAbsolutePosition = cancoder.getAbsolutePosition(); + steerAbsolutePositionCache = OdometryThread.registerSignalSignal(cancoder.getAbsolutePosition()); + steerFinalMechanismVelocity = steerTalon.getVelocity(); + steerAppliedVoltage = steerTalon.getMotorVoltage(); + steerMotorCurrentDrawn = steerTalon.getStatorCurrent(); + + // Configure periodic frames + BaseStatusSignal.setUpdateFrequencyForAll(ODOMETRY_FREQUENCY, driveRotterPosition, steerAbsolutePosition); + BaseStatusSignal.setUpdateFrequencyForAll( + 1 / Robot.defaultPeriodSecs, + driveRotterVelocity, + driveAppliedVoltage, + driveMotorCurrentDrawn, + steerFinalMechanismVelocity, + steerAppliedVoltage, + steerMotorCurrentDrawn); + ParentDevice.optimizeBusUtilizationForAll(driveTalon, steerTalon); } @Override public void updateInputs(ModuleIOInputs inputs) { - inputs.hardwareConnected = - BaseStatusSignal.refreshAll(periodicallyRefreshedSignals).isOK(); - - inputs.odometryDriveWheelRevolutions = driveEncoderUngearedPosition.stream() + // Refresh all signals + var driveStatus = BaseStatusSignal.refreshAll( + driveRotterPosition, driveRotterVelocity, driveAppliedVoltage, driveMotorCurrentDrawn); + var steerStatus = BaseStatusSignal.refreshAll( + steerAbsolutePosition, steerFinalMechanismVelocity, steerAppliedVoltage, steerMotorCurrentDrawn); + var steerEncoderStatus = BaseStatusSignal.refreshAll(steerAbsolutePosition); + inputs.driveMotorConnected = driveConnectedDebounce.calculate(driveStatus.isOK()); + inputs.steerMotorConnected = steerConnectedDebounce.calculate(steerStatus.isOK()); + inputs.steerEncoderConnected = steerEncoderConnectedDebounce.calculate(steerEncoderStatus.isOK()); + + // Fetch high-frequency drive encoder inputs + inputs.odometryDriveWheelRevolutions = driveRotterPositionRotations.stream() .mapToDouble(value -> value.in(Rotations) / DRIVE_GEAR_RATIO) .toArray(); - driveEncoderUngearedPosition.clear(); - if (inputs.odometryDriveWheelRevolutions.length > 0) - inputs.driveWheelFinalRevolutions = - inputs.odometryDriveWheelRevolutions[inputs.odometryDriveWheelRevolutions.length - 1]; + driveRotterPositionRotations.clear(); + // Fetch high-frequency drive encoder inputs inputs.odometrySteerPositions = - steerEncoderAbsolutePosition.stream().map(Rotation2d::new).toArray(Rotation2d[]::new); - steerEncoderAbsolutePosition.clear(); - if (inputs.odometrySteerPositions.length > 0) - inputs.steerFacing = inputs.odometrySteerPositions[inputs.odometrySteerPositions.length - 1]; + steerAbsolutePositionCache.stream().map(Rotation2d::new).toArray(Rotation2d[]::new); + steerAbsolutePositionCache.clear(); + // Fetch low frequency position and velocity signals + inputs.driveWheelFinalRevolutions = driveRotterPosition.getValue().in(Revolutions) / DRIVE_GEAR_RATIO; inputs.driveWheelFinalVelocityRevolutionsPerSec = - driveEncoderUngearedVelocity.getValue().in(RotationsPerSecond) / DRIVE_GEAR_RATIO; - inputs.driveMotorAppliedVolts = driveMotorAppliedVoltage.getValue().in(Volts); - inputs.driveMotorCurrentAmps = driveMotorCurrent.getValue().in(Amps); + driveRotterVelocity.getValue().in(RotationsPerSecond) / DRIVE_GEAR_RATIO; + inputs.steerFacing = new Rotation2d(steerAbsolutePosition.getValue()); + inputs.steerVelocityRadPerSec = steerFinalMechanismVelocity.getValue().in(RadiansPerSecond); + + // Fetch applied voltage and current + inputs.driveMotorAppliedVolts = driveAppliedVoltage.getValue().in(Volts); + inputs.driveMotorCurrentAmps = driveMotorCurrentDrawn.getValue().in(Amps); + inputs.steerMotorAppliedVolts = steerAppliedVoltage.getValue().in(Volts); + inputs.steerMotorCurrentAmps = steerMotorCurrentDrawn.getValue().in(Amps); + } - inputs.steerVelocityRadPerSec = steerEncoderVelocity.getValue().in(RadiansPerSecond); - inputs.steerMotorAppliedVolts = steerMotorAppliedVolts.getValue().in(Volts); - inputs.steerMotorCurrentAmps = steerMotorCurrent.getValue().in(Amps); + @Override + public void setDriveBrake(boolean enable) { + driveTalon.setNeutralMode(enable ? NeutralModeValue.Brake : NeutralModeValue.Coast); } @Override - public void setDriveVoltage(double volts) { - final VoltageOut voltageOut = new VoltageOut(volts).withEnableFOC(false); - driveTalon.setControl(voltageOut); + public void setSteerBrake(boolean enable) { + steerTalon.setNeutralMode(enable ? NeutralModeValue.Brake : NeutralModeValue.Coast); } @Override - public void setSteerPowerPercent(double powerPercent) { - steerTalon.setControl(new DutyCycleOut(powerPercent).withEnableFOC(true)); + public void requestDriveOpenLoop(Voltage output) { + driveTalon.setControl(new VoltageOut(output)); } @Override - public void setDriveBrake(boolean enable) { - driveTalon.setNeutralMode(enable ? NeutralModeValue.Brake : NeutralModeValue.Coast); + public void requestDriveOpenLoop(Current output) { + driveTalon.setControl(new TorqueCurrentFOC(output)); } @Override - public void setSteerBrake(boolean enable) { - steerTalon.setNeutralMode(enable ? NeutralModeValue.Brake : NeutralModeValue.Coast); + public void requestSteerOpenLoop(Voltage output) { + steerTalon.setControl(new VoltageOut(output)); + } + + @Override + public void requestSteerOpenLoop(Current output) { + steerTalon.setControl(new TorqueCurrentFOC(output)); + } + + @Override + public void requestDriveVelocityControl(double desiredWheelVelocityRadPerSec) { + double motorVelocityRotPerSec = + Units.radiansToRotations(desiredWheelVelocityRadPerSec) * moduleConstants.DriveMotorGearRatio; + driveTalon.setControl( + switch (moduleConstants.DriveMotorClosedLoopOutput) { + case Voltage -> new VelocityVoltage(motorVelocityRotPerSec); + case TorqueCurrentFOC -> new VelocityTorqueCurrentFOC(motorVelocityRotPerSec); + }); + } + + @Override + public void requestSteerPositionControl(Rotation2d desiredSteerAbsoluteFacing) { + double steerPosition = desiredSteerAbsoluteFacing.getRotations(); + steerTalon.setControl( + switch (moduleConstants.SteerMotorClosedLoopOutput) { + case Voltage -> new PositionVoltage(steerPosition); + case TorqueCurrentFOC -> new PositionTorqueCurrentFOC(steerPosition); + }); } } diff --git a/src/main/java/frc/robot/subsystems/drive/IO/OdometryThread.java b/src/main/java/frc/robot/subsystems/drive/IO/OdometryThread.java index 87b712a..d607923 100644 --- a/src/main/java/frc/robot/subsystems/drive/IO/OdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/IO/OdometryThread.java @@ -33,7 +33,7 @@ public void cacheInputToQueue() { List registeredInputs = new ArrayList<>(); List registeredStatusSignals = new ArrayList<>(); - static Queue registerSignalInput(StatusSignal signal) { + static Queue registerSignalSignal(StatusSignal signal) { signal.setUpdateFrequency(ODOMETRY_FREQUENCY, ODOMETRY_WAIT_TIMEOUT_SECONDS); registeredStatusSignals.add(signal); return registerInput(signal.asSupplier()); diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveModule.java b/src/main/java/frc/robot/subsystems/drive/SwerveModule.java index 7d1854c..43ddf22 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveModule.java @@ -19,7 +19,6 @@ import frc.robot.subsystems.drive.IO.ModuleIO; import frc.robot.subsystems.drive.IO.ModuleIOInputsAutoLogged; import frc.robot.utils.Alert; -import frc.robot.utils.CustomMaths.SwerveStateProjection; import frc.robot.utils.CustomPIDs.MaplePIDController; import org.littletonrobotics.junction.Logger; @@ -55,7 +54,8 @@ public SwerveModule(ModuleIO io, String name) { public void updateOdometryInputs() { io.updateInputs(inputs); Logger.processInputs("Drive/Module-" + name, inputs); - this.hardwareFaultAlert.setActivated(!inputs.hardwareConnected); + this.hardwareFaultAlert.setActivated( + !(inputs.driveMotorConnected && inputs.steerMotorConnected && inputs.steerEncoderConnected)); } @Override @@ -72,36 +72,26 @@ private void updateOdometryPositions() { } } - private void runSteerCloseLoop() { - turnCloseLoop.setSetpoint(setPoint.angle.getRadians()); - io.setSteerPowerPercent(turnCloseLoop.calculate(getSteerFacing().getRadians())); - } - - private void runDriveControlLoop() { - final double adjustSpeedSetpointMetersPerSec = SwerveStateProjection.project(setPoint, getSteerFacing()); - io.setDriveVoltage(DRIVE_OPEN_LOOP.calculate(adjustSpeedSetpointMetersPerSec) - + driveCloseLoop.calculate(getDriveVelocityMetersPerSec(), adjustSpeedSetpointMetersPerSec)); - } - /** Runs the module with the specified setpoint state. Returns the optimized state. */ - public SwerveModuleState runSetPoint(SwerveModuleState state) { - this.setPoint = SwerveModuleState.optimize(state, getSteerFacing()); + public SwerveModuleState runSetPoint(SwerveModuleState newSetpoint) { + newSetpoint = SwerveModuleState.optimize(newSetpoint, getSteerFacing()); - if (Math.abs(state.speedMetersPerSecond) < 0.01) { - io.setDriveVoltage(0); - io.setSteerPowerPercent(0); - return this.setPoint = new SwerveModuleState(); + if (Math.abs(newSetpoint.speedMetersPerSecond) < 0.01) { + io.stop(); + return this.setPoint = new SwerveModuleState(0, setPoint.angle); } - runDriveControlLoop(); - runSteerCloseLoop(); - return this.setPoint; + double desiredWheelVelocityRadPerSec = + newSetpoint.speedMetersPerSecond / DriveTrainConstants.WHEEL_RADIUS.in(Meters); + io.requestDriveVelocityControl(desiredWheelVelocityRadPerSec); + io.requestSteerPositionControl(newSetpoint.angle); + + return this.setPoint = newSetpoint; } @Override public void onDisable() { - io.setSteerPowerPercent(0); - io.setDriveVoltage(0); + io.stop(); } /** Returns the current turn angle of the module. */ diff --git a/src/main/java/frc/robot/utils/PhoenixUtil.java b/src/main/java/frc/robot/utils/PhoenixUtil.java new file mode 100644 index 0000000..3873cf0 --- /dev/null +++ b/src/main/java/frc/robot/utils/PhoenixUtil.java @@ -0,0 +1,27 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.utils; + +import com.ctre.phoenix6.StatusCode; +import java.util.function.Supplier; + +public class PhoenixUtil { + /** Attempts to run the command until no error is produced. */ + public static void tryUntilOk(int maxAttempts, Supplier command) { + for (int i = 0; i < maxAttempts; i++) { + var error = command.get(); + if (error.isOK()) break; + } + } +}