Create a stub for sys id work

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

@@ -16,6 +16,7 @@
 import edu.wpi.first.wpilibj2.command.RepeatCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
 import frc.robot.Constants.Gamepads;
 import frc.robot.commands.DriveCommand;
 import frc.robot.commands.DriverPullupIntake;
@@ -46,6 +47,7 @@
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.LedSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
+import frc.robot.sysid.SysIdRunner;
 
 enum AutoOption {
   driveForward,
@@ -65,6 +67,7 @@ enum AutoOption {
   sideLongAmpBlue,
   sideLongAmpRed,
   pathPlannerExample,
+  sysIdSequence
 }
 
 /**
@@ -140,6 +143,7 @@ public RobotContainer() {
     m_autoChooser.addOption("Side Long Source Blue", AutoOption.sideLongSourceBlue);
     m_autoChooser.addOption("Side Long Amp Blue", AutoOption.sideLongAmpBlue);
     m_autoChooser.addOption("Side Long Amp Red", AutoOption.sideLongAmpRed);
+    m_autoChooser.addOption("SysId", AutoOption.sysIdSequence);
     dashboard.add("Auto Routine", m_autoChooser).withSize(2, 1).withPosition(8, 0);
 
 
@@ -320,6 +324,7 @@ public Command getAutonomousCommand() {
       case sideLongAmpBlue -> new sideLongAmpBlue(m_drivebase, m_shooter, m_intake, m_led, waittime.getDouble(0));
       case sideLongAmpRed -> new sideLongAmpRed(m_drivebase, m_shooter, m_intake, m_led, waittime.getDouble(0));
       case pathPlannerExample -> new PathPlannerAuto("New Auto");
+      case sysIdSequence -> new SysIdRunner(m_drivebase).sequence();
     };
   }
 }

src/main/java/frc/robot/subsystems/DrivebaseSubsystem.java

@@ -4,6 +4,7 @@
 import edu.wpi.first.math.filter.SlewRateLimiter;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.SPI;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
@@ -27,9 +28,14 @@
 import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
 import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
 import edu.wpi.first.networktables.GenericEntry;
-
+import edu.wpi.first.units.Distance;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.MutableMeasure;
+import edu.wpi.first.units.Units;
+import edu.wpi.first.units.Velocity;
+import edu.wpi.first.units.Voltage;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
-
+import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog;
 import frc.robot.Constants;
 import frc.robot.Constants.Drive;
 import frc.robot.sim.DrivebaseSim;
@@ -463,4 +469,29 @@ public void simulationPeriodic() {
 
     m_drivebaseSim.update(period);
   }
+
+  public void sysIdDrive(Measure<Voltage> voltage) {
+    // FIXME: Unclear whether we should set the speed control
+    // or the voltage control. For simulator, the voltage control
+    // does not work.
+    if (m_isSimulation) {
+      // WARNING: Drive safety disabled when running sysid in simulator to avoid
+      // the check stopping the motors prematurely.
+      m_drive.setSafetyEnabled(false);
+      m_leftDriveMotorF.set(voltage.magnitude() / RobotController.getBatteryVoltage());
+      m_rightDriveMotorF.set(voltage.magnitude() / RobotController.getBatteryVoltage());
+    } else {
+      m_leftDriveMotorF.setVoltage(voltage.magnitude());
+      m_rightDriveMotorF.setVoltage(voltage.magnitude());
+    }
+  }
+
+  public void sysIdUpdateMeasures(boolean isRight, MutableMeasure<Voltage> appliedVoltage,
+      MutableMeasure<Distance> distance, MutableMeasure<Velocity<Distance>> velocity) {
+    appliedVoltage.mut_replace(
+        (isRight ? m_rightDriveMotorF : m_leftDriveMotorF).get() * RobotController.getBatteryVoltage(), Units.Volts);
+    var encoder = isRight ? m_rightDriveEncoder : m_leftDriveEncoder;
+    distance.mut_replace(encoder.getPosition(), Units.Meters);
+    velocity.mut_replace(encoder.getVelocity(), Units.MetersPerSecond);
+  }
 }

src/main/java/frc/robot/sysid/SysIdRunner.java

@@ -0,0 +1,55 @@
+package frc.robot.sysid;
+
+import edu.wpi.first.units.Distance;
+import edu.wpi.first.units.MutableMeasure;
+import edu.wpi.first.units.Units;
+import edu.wpi.first.units.Velocity;
+import edu.wpi.first.units.Voltage;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
+import frc.robot.subsystems.DrivebaseSubsystem;
+
+public class SysIdRunner {
+  private SysIdRoutine m_routine;
+  private final MutableMeasure<Voltage> m_appliedVoltage = MutableMeasure.zero(Units.Volts);
+  private final MutableMeasure<Distance> m_distance = MutableMeasure.zero(Units.Meters);
+  private final MutableMeasure<Velocity<Distance>> m_velocity = MutableMeasure.zero(Units.MetersPerSecond);
+
+  public SysIdRunner(DrivebaseSubsystem drivebase) {
+    // Default config is 1 volt/sec ramp rate and 7 volt step voltage.
+    var config = new SysIdRoutine.Config();
+    var mechanism = new SysIdRoutine.Mechanism(voltage -> {
+      drivebase.sysIdDrive(voltage);
+    }, log -> {
+      drivebase.sysIdUpdateMeasures(false, m_appliedVoltage, m_distance, m_velocity);
+      log.motor("drive-left")
+          .voltage(m_appliedVoltage)
+          .linearPosition(m_distance)
+          .linearVelocity(m_velocity);
+      drivebase.sysIdUpdateMeasures(true, m_appliedVoltage, m_distance, m_velocity);
+      log.motor("drive-right")
+          .voltage(m_appliedVoltage)
+          .linearPosition(m_distance)
+          .linearVelocity(m_velocity);
+    }, drivebase);
+    m_routine = new SysIdRoutine(config, mechanism);
+  }
+
+  public Command quasistatic(SysIdRoutine.Direction direction) {
+    return m_routine.quasistatic(direction);
+  }
+
+  public Command dynamic(SysIdRoutine.Direction direction) {
+    return m_routine.dynamic(direction);
+  }
+
+  public Command sequence() {
+    return new SequentialCommandGroup(
+        this.quasistatic(Direction.kForward),
+        this.quasistatic(Direction.kReverse),
+        this.dynamic(Direction.kForward),
+        this.dynamic(Direction.kReverse));
+  }
+}