added system identification for drivetrain

src/main/java/frc/robot/RobotContainer.java

@@ -17,6 +17,7 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.autos.*;
 import frc.robot.commands.drive.*;
 import frc.robot.constants.*;
@@ -226,9 +227,17 @@ private LoggedDashboardChooser<Auto> buildAutoChooser() {
         return autoSendableChooser;
     }
 
-    private static SendableChooser<Supplier<Command>> buildTestsChooser() {
+    private SendableChooser<Supplier<Command>> buildTestsChooser() {
         final SendableChooser<Supplier<Command>> testsChooser = new SendableChooser<>();
         testsChooser.setDefaultOption("None", Commands::none);
+        testsChooser.addOption(
+                "Drive SysId- Quasistatic - Forward", () -> drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
+        testsChooser.addOption(
+                "Drive SysId- Quasistatic - Reverse", () -> drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
+        testsChooser.addOption(
+                "Drive SysId- Dynamic - Forward", () -> drive.sysIdDynamic(SysIdRoutine.Direction.kForward));
+        testsChooser.addOption(
+                "Drive SysId- Dynamic - Reverse", () -> drive.sysIdDynamic(SysIdRoutine.Direction.kReverse));
         // TODO add your tests here (system identification and etc.)
         return testsChooser;
     }

src/main/java/frc/robot/subsystems/drive/IO/ModuleIO.java

@@ -5,8 +5,6 @@
 
 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;
@@ -38,11 +36,6 @@ 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
      *

src/main/java/frc/robot/subsystems/drive/SwerveDrive.java

@@ -19,10 +19,12 @@
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.units.measure.Voltage;
 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 edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.Robot;
 import frc.robot.constants.FieldConstants;
 import frc.robot.subsystems.MapleSubsystem;
@@ -353,4 +355,23 @@ private boolean hasHardwareFaults() {
     }
 
     public final Trigger hardwareFaultsDetected = new Trigger(this::hasHardwareFaults);
+
+    private void runCharacterization(Voltage voltage) {
+        SwerveModuleState[] moduleStates = DRIVE_KINEMATICS.toSwerveModuleStates(new ChassisSpeeds(0, 0, 1));
+        for (int i = 0; i < swerveModules.length; i++)
+            swerveModules[i].runVoltageCharacterization(moduleStates[i].angle, voltage);
+    }
+
+    private final SysIdRoutine sysId = new SysIdRoutine(
+            new SysIdRoutine.Config(
+                    null, null, null, (state) -> Logger.recordOutput("Drive/SysIdState", state.toString())),
+            new SysIdRoutine.Mechanism(this::runCharacterization, null, this));
+
+    public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
+        return sysId.quasistatic(direction);
+    }
+
+    public Command sysIdDynamic(SysIdRoutine.Direction direction) {
+        return sysId.dynamic(direction);
+    }
 }

src/main/java/frc/robot/subsystems/drive/SwerveModule.java

@@ -13,6 +13,7 @@
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.units.measure.Current;
 import edu.wpi.first.units.measure.Force;
 import edu.wpi.first.units.measure.Voltage;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
@@ -79,10 +80,10 @@ private void updateOdometryPositions() {
     /** Runs the module with the specified setpoint state. Returns the optimized state. */
     public SwerveModuleState runSetPoint(
             SwerveModuleState newSetpoint, Force robotRelativeFeedforwardForceX, Force robotRelativeFeedforwardForceY) {
-        newSetpoint = SwerveModuleState.optimize(newSetpoint, getSteerFacing());
+        newSetpoint = SwerveModuleState.optimize(newSetpoint, setPoint.angle);
 
         if (Math.abs(newSetpoint.speedMetersPerSecond) < 0.01) {
-            io.stop();
+            stop();
             return this.setPoint = new SwerveModuleState(0, setPoint.angle);
         }
 
@@ -104,12 +105,21 @@ public SwerveModuleState runSetPoint(
 
     @Override
     public void onDisable() {
-        io.stop();
+        stop();
     }
 
-    public void setMotorBrake(boolean motorBrakeEnabled) {
-        io.setDriveBrake(motorBrakeEnabled);
-        io.setSteerBrake(motorBrakeEnabled);
+    public void stop() {
+        io.requestDriveOpenLoop(Volts.zero());
+        io.requestSteerOpenLoop(Volts.zero());
+    }
+
+    private boolean brakeEnabled = true;
+
+    public void setMotorBrake(boolean enableMotorBrake) {
+        if (brakeEnabled == enableMotorBrake) return;
+        io.setDriveBrake(enableMotorBrake);
+        io.setSteerBrake(enableMotorBrake);
+        this.brakeEnabled = enableMotorBrake;
     }
 
     /** Returns the current turn angle of the module. */
@@ -153,4 +163,16 @@ public SwerveModulePosition[] getOdometryPositions() {
     public boolean hasHardwareFaults() {
         return !(inputs.driveMotorConnected && inputs.steerMotorConnected && inputs.steerEncoderConnected);
     }
+
+    public void runVoltageCharacterization(Rotation2d steerFacing, Voltage driveVoltageOut) {
+        setMotorBrake(true);
+        io.requestSteerPositionControl(steerFacing);
+        io.requestDriveOpenLoop(driveVoltageOut);
+    }
+
+    public void runCurrentCharacterization(Rotation2d steerFacing, Current driveCurrentOut) {
+        setMotorBrake(true);
+        io.requestSteerPositionControl(steerFacing);
+        io.requestDriveOpenLoop(driveCurrentOut);
+    }
 }