From b6e2bd1e54e9bffa183bcc01d61eacb8ae637f7b Mon Sep 17 00:00:00 2001 From: caTrixLiu Date: Wed, 25 Dec 2024 19:55:50 +0800 Subject: [PATCH] better current limits & hardware faults detection --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../robot/constants/DriveTrainConstants.java | 6 ++-- .../drive/HolonomicDriveSubsystem.java | 2 +- .../robot/subsystems/drive/IO/ModuleIO.java | 4 +-- .../subsystems/drive/IO/ModuleIOSim.java | 2 +- .../subsystems/drive/IO/ModuleIOTalon.java | 20 ++++++++++--- .../robot/subsystems/drive/SwerveDrive.java | 29 +++++++++++++++++-- .../robot/subsystems/drive/SwerveModule.java | 28 ++++++++++++++---- 8 files changed, 73 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3c98b59..369585e 100644 --- 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 index 7d7dbea..11a5f50 100644 --- 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 index 574ecd4..3635b48 100644 --- 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 index 8521f0a..03bcb6a 100644 --- 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 index 2a1c492..83398da 100644 --- 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 index 55ad96e..c65fb97 100644 --- 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 index 811a353..d8bfc52 100644 --- 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 index 43ddf22..712038a 100644 --- 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); + } }