better current limits & hardware faults detection

Shenzhen-Robotics-Alliance · Dec 25, 2024 · b6e2bd1 · b6e2bd1
1 parent e8f92cd
commit b6e2bd1
Show file tree

Hide file tree

Showing 8 changed files with 73 additions and 20 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -288,7 +288,7 @@ public void configureButtonBindings() {
         drive.setDefaultCommand(joystickDrive);
 
         /* lock chassis with x-formation */
-        driverXBox.x().whileTrue(Commands.run(drive::lockChassisWithXFormation, drive));
+        driverXBox.x().whileTrue(drive.lockChassisWithXFormation());
 
         /* reset gyro heading manually (in case the vision does not work) */
         driverXBox

diff --git a/src/main/java/frc/robot/constants/DriveTrainConstants.java b/src/main/java/frc/robot/constants/DriveTrainConstants.java
@@ -37,10 +37,10 @@ public class DriveTrainConstants {
     public static final MomentOfInertia STEER_INERTIA = KilogramSquareMeters.of(0.025);
 
     /* adjust current limit */
-    public static final Current DRIVE_CURRENT_LIMIT_ANTI_SLIP = Amps.of(50);
+    public static final Current DRIVE_ANTI_SLIP_TORQUE_CURRENT_LIMIT = Amps.of(50);
     public static final Current DRIVE_OVER_CURRENT_PROTECTION = Amps.of(120);
     public static final Time DRIVE_OVERHEAT_PROTECTION_TIME = Seconds.of(1.5);
-    public static final Current DRIVE_OVERHEAT_PROTECTION_CURRENT = Amps.of(70);
+    public static final Current DRIVE_OVERHEAT_PROTECTION = Amps.of(70);
     public static final Current STEER_CURRENT_LIMIT = Amps.of(20);
 
     /** translations of the modules to the robot center, in FL, FR, BL, BR */
@@ -65,7 +65,7 @@ public class DriveTrainConstants {
 
     /* force = torque / distance */
     public static final Force MAX_PROPELLING_FORCE = NewtonMeters.of(
-                    DRIVE_MOTOR.getTorque(DRIVE_CURRENT_LIMIT_ANTI_SLIP.in(Amps)) * DRIVE_GEAR_RATIO)
+                    DRIVE_MOTOR.getTorque(DRIVE_ANTI_SLIP_TORQUE_CURRENT_LIMIT.in(Amps)) * DRIVE_GEAR_RATIO)
             .div(WHEEL_RADIUS);
 
     /* floor_speed = wheel_angular_velocity * wheel_radius */

diff --git a/src/main/java/frc/robot/subsystems/drive/HolonomicDriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/HolonomicDriveSubsystem.java
@@ -130,7 +130,7 @@ default void configHolonomicPathPlannerAutoBuilder() {
                                 DriveTrainConstants.CHASSIS_MAX_VELOCITY,
                                 DriveTrainConstants.WHEEL_COEFFICIENT_OF_FRICTION,
                                 DriveTrainConstants.DRIVE_MOTOR.withReduction(DriveTrainConstants.DRIVE_GEAR_RATIO),
-                                DriveTrainConstants.DRIVE_CURRENT_LIMIT_ANTI_SLIP,
+                                DriveTrainConstants.DRIVE_ANTI_SLIP_TORQUE_CURRENT_LIMIT,
                                 1),
                         DriveTrainConstants.MODULE_TRANSLATIONS),
                 FieldMirroringUtils::isSidePresentedAsRed,

diff --git a/src/main/java/frc/robot/subsystems/drive/IO/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/IO/ModuleIO.java
@@ -86,8 +86,8 @@ default void requestDriveVelocityControl(double desiredWheelVelocityRadPerSec) {
     default void requestSteerPositionControl(Rotation2d desiredSteerAbsoluteFacing) {}
 
     /** Enable or disable brake mode on the drive motor. */
-    default void setDriveBrake(boolean enable) {}
+    default void setDriveBrake(boolean enableDriveBrake) {}
 
     /** Enable or disable brake mode on the turn motor. */
-    default void setSteerBrake(boolean enable) {}
+    default void setSteerBrake(boolean enableSteerBrake) {}
 }
diff --git a/src/main/java/frc/robot/subsystems/drive/IO/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/IO/ModuleIOSim.java
@@ -46,7 +46,7 @@ public ModuleIOSim(SwerveModuleSimulation moduleSimulation) {
         this.moduleSimulation = moduleSimulation;
         this.driveMotor = moduleSimulation
                 .useGenericMotorControllerForDrive()
-                .withCurrentLimit(DriveTrainConstants.DRIVE_CURRENT_LIMIT_ANTI_SLIP);
+                .withCurrentLimit(DriveTrainConstants.DRIVE_ANTI_SLIP_TORQUE_CURRENT_LIMIT);
         this.steerMotor = moduleSimulation
                 .useGenericControllerForSteer()
                 .withCurrentLimit(DriveTrainConstants.STEER_CURRENT_LIMIT);

diff --git a/src/main/java/frc/robot/subsystems/drive/IO/ModuleIOTalon.java b/src/main/java/frc/robot/subsystems/drive/IO/ModuleIOTalon.java
@@ -77,6 +77,10 @@ public ModuleIOTalon(SwerveModuleConstants moduleConstants, String name) {
         driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -moduleConstants.SlipCurrent;
         driveConfig.CurrentLimits.StatorCurrentLimit = moduleConstants.SlipCurrent;
         driveConfig.CurrentLimits.StatorCurrentLimitEnable = true;
+        driveConfig.CurrentLimits.SupplyCurrentLimit = DRIVE_OVER_CURRENT_PROTECTION.in(Amps);
+        driveConfig.CurrentLimits.SupplyCurrentLowerTime = DRIVE_OVERHEAT_PROTECTION_TIME.in(Seconds);
+        driveConfig.CurrentLimits.SupplyCurrentLowerLimit = DRIVE_OVERHEAT_PROTECTION.in(Amps);
+
         driveConfig.MotorOutput.Inverted = moduleConstants.DriveMotorInverted
                 ? InvertedValue.Clockwise_Positive
                 : InvertedValue.CounterClockwise_Positive;
@@ -175,14 +179,22 @@ public void updateInputs(ModuleIOInputs inputs) {
         inputs.steerMotorCurrentAmps = steerMotorCurrentDrawn.getValue().in(Amps);
     }
 
+    boolean driveBrakeEnabled = true;
+
     @Override
-    public void setDriveBrake(boolean enable) {
-        driveTalon.setNeutralMode(enable ? NeutralModeValue.Brake : NeutralModeValue.Coast);
+    public void setDriveBrake(boolean enableDriveBrake) {
+        if (driveBrakeEnabled == enableDriveBrake) return;
+        driveTalon.setNeutralMode(enableDriveBrake ? NeutralModeValue.Brake : NeutralModeValue.Coast);
+        this.driveBrakeEnabled = enableDriveBrake;
     }
 
+    boolean steerBrakeEnabled = true;
+
     @Override
-    public void setSteerBrake(boolean enable) {
-        steerTalon.setNeutralMode(enable ? NeutralModeValue.Brake : NeutralModeValue.Coast);
+    public void setSteerBrake(boolean enableSteerBrake) {
+        if (this.steerBrakeEnabled == enableSteerBrake) return;
+        steerTalon.setNeutralMode(enableSteerBrake ? NeutralModeValue.Brake : NeutralModeValue.Coast);
+        this.steerBrakeEnabled = enableSteerBrake;
     }
 
     @Override

diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java b/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java
@@ -20,6 +20,9 @@
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.FunctionalCommand;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.constants.DriveControlLoops;
 import frc.robot.constants.FieldConstants;
 import frc.robot.subsystems.MapleSubsystem;
@@ -224,11 +227,23 @@ public void stop() {
      * Locks the chassis and turns the modules to an X formation to resist movement. The lock will be cancelled the next
      * time a nonzero velocity is requested.
      */
-    public void lockChassisWithXFormation() {
+    public Command lockChassisWithXFormation() {
         Rotation2d[] swerveHeadings = new Rotation2d[swerveModules.length];
         for (int i = 0; i < swerveHeadings.length; i++) swerveHeadings[i] = MODULE_TRANSLATIONS[i].getAngle();
-        DRIVE_KINEMATICS.resetHeadings(swerveHeadings);
-        HolonomicDriveSubsystem.super.stop();
+        return new FunctionalCommand(
+                () -> DRIVE_KINEMATICS.resetHeadings(swerveHeadings),
+                () -> {
+                    for (int i = 0; i < swerveModules.length; i++)
+                        swerveModules[i].runSetPoint(new SwerveModuleState(0, swerveHeadings[i]));
+                },
+                (interrupted) -> {},
+                () -> false,
+                this);
+    }
+
+    /** Turns the motor brakes on */
+    public void setMotorBrake(boolean motorBrakeEnabled) {
+        for (SwerveModule module : swerveModules) module.setMotorBrake(motorBrakeEnabled);
     }
 
     /** Returns the module states (turn angles and drive velocities) for all the modules. */
@@ -324,4 +339,12 @@ private void startDashboardDisplay() {
                     null);
         });
     }
+
+    private boolean hasHardwareFaults() {
+        if (!gyroInputs.connected) return true;
+        for (SwerveModule module : swerveModules) if (module.hasHardwareFaults()) return true;
+        return false;
+    }
+
+    public final Trigger hardwareFaultsDetected = new Trigger(this::hasHardwareFaults);
 }
diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveModule.java b/src/main/java/frc/robot/subsystems/drive/SwerveModule.java
@@ -31,14 +31,21 @@ public class SwerveModule extends MapleSubsystem {
     private SwerveModuleState setPoint;
     private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {};
 
-    private final Alert hardwareFaultAlert;
+    private final Alert driveMotorHardwareFault, steerMotorHardwareFault, steerEncoderHardwareFault;
 
     public SwerveModule(ModuleIO io, String name) {
         super("Module-" + name);
         this.io = io;
         this.name = name;
-        this.hardwareFaultAlert = new Alert("Module-" + name + " Hardware Fault", Alert.AlertType.ERROR);
-        this.hardwareFaultAlert.setActivated(false);
+        this.driveMotorHardwareFault =
+                new Alert("Module-" + name + " Drive Motor Hardware Fault Detected", Alert.AlertType.ERROR);
+        this.steerMotorHardwareFault =
+                new Alert("Module-" + name + " Steer Motor Hardware Fault Detected", Alert.AlertType.ERROR);
+        this.steerEncoderHardwareFault =
+                new Alert("Module-" + name + " Steer Encoder Hardware Fault Detected", Alert.AlertType.ERROR);
+        this.driveMotorHardwareFault.setActivated(false);
+        this.steerMotorHardwareFault.setActivated(false);
+        this.steerEncoderHardwareFault.setActivated(false);
 
         turnCloseLoop = new MaplePIDController(STEER_CLOSE_LOOP);
         driveCloseLoop = new MaplePIDController(DRIVE_CLOSE_LOOP);
@@ -54,13 +61,15 @@ public SwerveModule(ModuleIO io, String name) {
     public void updateOdometryInputs() {
         io.updateInputs(inputs);
         Logger.processInputs("Drive/Module-" + name, inputs);
-        this.hardwareFaultAlert.setActivated(
-                !(inputs.driveMotorConnected && inputs.steerMotorConnected && inputs.steerEncoderConnected));
     }
 
     @Override
     public void periodic(double dt, boolean enabled) {
         updateOdometryPositions();
+
+        this.driveMotorHardwareFault.setActivated(!inputs.driveMotorConnected);
+        this.steerMotorHardwareFault.setActivated(!inputs.steerMotorConnected);
+        this.steerEncoderHardwareFault.setActivated(!inputs.steerEncoderConnected);
     }
 
     private void updateOdometryPositions() {
@@ -94,6 +103,11 @@ public void onDisable() {
         io.stop();
     }
 
+    public void setMotorBrake(boolean motorBrakeEnabled) {
+        io.setDriveBrake(motorBrakeEnabled);
+        io.setSteerBrake(motorBrakeEnabled);
+    }
+
     /** Returns the current turn angle of the module. */
     public Rotation2d getSteerFacing() {
         return inputs.steerFacing;
@@ -131,4 +145,8 @@ public SwerveModuleState getMeasuredState() {
     public SwerveModulePosition[] getOdometryPositions() {
         return odometryPositions;
     }
+
+    public boolean hasHardwareFaults() {
+        return !(inputs.driveMotorConnected && inputs.steerMotorConnected && inputs.steerEncoderConnected);
+    }
 }