refactor arm code, add sysid, and fix gyro zero

src/main/java/frc/robot/Robot.java

@@ -148,6 +148,26 @@ public Robot() {
         "Shooter SysId (Dynamic Forward)", m_shooter.sysIdDynamic(SysIdRoutine.Direction.kForward));
     m_autoChooser.addOption(
         "Shooter SysId (Dynamic Reverse)", m_shooter.sysIdDynamic(SysIdRoutine.Direction.kReverse));
+    m_autoChooser.addOption(
+        "Elbow SysId (Quasistatic Forward)",
+        m_arm.sysIdElbowQuasistatic(SysIdRoutine.Direction.kForward));
+    m_autoChooser.addOption(
+        "Elbow SysId (Quasistatic Reverse)",
+        m_arm.sysIdElbowQuasistatic(SysIdRoutine.Direction.kReverse));
+    m_autoChooser.addOption(
+        "Elbow SysId (Dynamic Forward)", m_arm.sysIdElbowDynamic(SysIdRoutine.Direction.kForward));
+    m_autoChooser.addOption(
+        "Elbow SysId (Dynamic Reverse)", m_arm.sysIdElbowDynamic(SysIdRoutine.Direction.kReverse));
+    m_autoChooser.addOption(
+        "Wrist SysId (Quasistatic Forward)",
+        m_arm.sysIdWristQuasistatic(SysIdRoutine.Direction.kForward));
+    m_autoChooser.addOption(
+        "Wrist SysId (Quasistatic Reverse)",
+        m_arm.sysIdWristQuasistatic(SysIdRoutine.Direction.kReverse));
+    m_autoChooser.addOption(
+        "Wrist SysId (Dynamic Forward)", m_arm.sysIdWristDynamic(SysIdRoutine.Direction.kForward));
+    m_autoChooser.addOption(
+        "Wrist SysId (Dynamic Reverse)", m_arm.sysIdWristDynamic(SysIdRoutine.Direction.kReverse));
     m_autoChooser.addOption("3 Piece Centerline", autos.centerlineTwoPiece());
     m_drivetrain.setDefaultCommand(
         m_drivetrain.joystickDrive(

src/main/java/frc/robot/arm/Arm.java

@@ -1,5 +1,8 @@
 package frc.robot.arm;
 
+import static edu.wpi.first.units.Units.Seconds;
+import static edu.wpi.first.units.Units.Volts;
+
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.controller.ArmFeedforward;
@@ -15,6 +18,7 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.lib.LoggedTunableNumber;
 import frc.robot.Constants;
 import java.util.Set;
@@ -62,6 +66,7 @@ public class Arm extends SubsystemBase {
 
   private Vector<N2> m_profileInitialAngles;
   public Supplier<Pose3d> wristPoseSupplier;
+  private final SysIdRoutine m_sysIdRoutineElbow, m_sysIdRoutineWrist;
 
   static {
     switch (Constants.kCurrentMode) {
@@ -104,6 +109,24 @@ public Arm(ArmIO io) {
     m_elbowController.setTolerance(Units.degreesToRadians(0.5));
     m_wristController.setTolerance(Units.degreesToRadians(0.5));
     wristPoseSupplier = () -> m_measuredVisualizer.getWristPose();
+    m_sysIdRoutineElbow =
+        new SysIdRoutine(
+            new SysIdRoutine.Config(
+                Volts.of(0.25).per(Seconds.of(1.0)),
+                Volts.of(2.0),
+                null,
+                state -> Logger.recordOutput("Arm/SysIdState", state.toString())),
+            new SysIdRoutine.Mechanism(
+                voltage -> m_io.setElbowVoltage(voltage.in(Volts)), null, this));
+    m_sysIdRoutineWrist =
+        new SysIdRoutine(
+            new SysIdRoutine.Config(
+                Volts.of(0.25).per(Seconds.of(1.0)),
+                Volts.of(2.0),
+                null,
+                state -> Logger.recordOutput("Arm/SysIdState", state.toString())),
+            new SysIdRoutine.Mechanism(
+                voltage -> m_io.setWristVoltage(voltage.in(Volts)), null, this));
     setDefaultCommand(idle());
   }
 
@@ -254,6 +277,42 @@ public Command idleCoast() {
         .withName("Idle Coast");
   }
 
+  public Command sysIdElbowQuasistatic(SysIdRoutine.Direction direction) {
+    return m_sysIdRoutineElbow
+        .quasistatic(direction)
+        .until(
+            () ->
+                m_inputs.elbowPositionRad > (Math.PI / 2)
+                    || m_inputs.elbowPositionRad < ArmSetpoints.kStowed.elbowAngle);
+  }
+
+  public Command sysIdElbowDynamic(SysIdRoutine.Direction direction) {
+    return m_sysIdRoutineElbow
+        .dynamic(direction)
+        .until(
+            () ->
+                m_inputs.elbowPositionRad > (Math.PI / 2)
+                    || m_inputs.elbowPositionRad < ArmSetpoints.kStowed.elbowAngle);
+  }
+
+  public Command sysIdWristQuasistatic(SysIdRoutine.Direction direction) {
+    return m_sysIdRoutineWrist
+        .quasistatic(direction)
+        .until(
+            () ->
+                m_inputs.elbowPositionRad < 0.0
+                    || m_inputs.wristPositionRad > ArmSetpoints.kStowed.wristAngle);
+  }
+
+  public Command sysIdWristDynamic(SysIdRoutine.Direction direction) {
+    return m_sysIdRoutineWrist
+        .dynamic(direction)
+        .until(
+            () ->
+                m_inputs.elbowPositionRad < 0.0
+                    || m_inputs.wristPositionRad > ArmSetpoints.kStowed.wristAngle);
+  }
+
   private boolean getJointsAtGoal() {
     return m_elbowController.atGoal() && m_wristController.atGoal();
   }

src/main/java/frc/robot/arm/ArmModel.java

@@ -2,7 +2,6 @@
 
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.Vector;
-import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.numbers.N2;
 import edu.wpi.first.math.numbers.N4;
 import edu.wpi.first.math.system.plant.DCMotor;
@@ -11,9 +10,6 @@
 
 public class ArmModel {
 
-  public static final Translation2d origin =
-      new Translation2d(Units.inchesToMeters(-9.2), Units.inchesToMeters(20));
-
   public static final double kElbowGearboxReduction = 3.0 * 5.0 * 5.0;
   public static final double kElbowChainReduction = 36.0 / 22.0;
   public static final double kElbowFinalReduction = kElbowGearboxReduction * kElbowChainReduction;

src/main/java/frc/robot/arm/ArmVisualizer.java

@@ -3,6 +3,7 @@
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
@@ -16,6 +17,8 @@ public class ArmVisualizer {
 
   // Value from CAD, angle from horizontal to shoulder.
   private final double kShoulderAngleDegrees = 103.985388;
+  private final Translation2d origin =
+      new Translation2d(Units.inchesToMeters(-9.2), Units.inchesToMeters(20));
   private final String m_logKey;
   private final Mechanism2d m_mechanism;
   private final MechanismRoot2d m_mechanismRoot;
@@ -25,15 +28,11 @@ public class ArmVisualizer {
   public ArmVisualizer(String logKey, double ligamentWidth, Color color) {
     m_logKey = logKey;
     m_mechanism = new Mechanism2d(4, 3, new Color8Bit(Color.kGray));
-    m_mechanismRoot = m_mechanism.getRoot("Arm", 2 + ArmModel.origin.getX(), 0);
+    m_mechanismRoot = m_mechanism.getRoot("Arm", 2 + origin.getX(), 0);
     m_fixedShoulderLigament =
         m_mechanismRoot.append(
             new MechanismLigament2d(
-                "Shoulder",
-                ArmModel.origin.getY(),
-                kShoulderAngleDegrees,
-                6,
-                new Color8Bit(Color.kBlack)));
+                "Shoulder", origin.getY(), kShoulderAngleDegrees, 6, new Color8Bit(Color.kBlack)));
     m_elbowLigament =
         m_fixedShoulderLigament.append(
             new MechanismLigament2d(
@@ -50,11 +49,7 @@ public void update(double elbowAngle, double wristAngle) {
     Logger.recordOutput("Mechanism2d/" + m_logKey, m_mechanism);
 
     m_elbowPose =
-        new Pose3d(
-            ArmModel.origin.getX(),
-            0.0,
-            ArmModel.origin.getY(),
-            new Rotation3d(0.0, -elbowAngle, 0.0));
+        new Pose3d(origin.getX(), 0.0, origin.getY(), new Rotation3d(0.0, -elbowAngle, 0.0));
     m_wristPose =
         m_elbowPose.transformBy(
             new Transform3d(

src/main/java/frc/robot/drivetrain/Drivetrain.java

@@ -212,7 +212,13 @@ public Command joystickDrive(
   }
 
   public Command zeroGyro() {
-    return Commands.runOnce(() -> m_imuIO.setGyroAngle(0.0)).ignoringDisable(true);
+    return Commands.runOnce(
+            () ->
+                resetPose.accept(
+                    PoseEstimation.getInstance()
+                        .getPose()
+                        .rotateBy(m_imuInputs.yawPosition.unaryMinus())))
+        .ignoringDisable(true);
   }
 
   public Command sysIdDriveQuasistatic(SysIdRoutine.Direction direction) {

src/main/java/frc/robot/shooter/Shooter.java

@@ -53,9 +53,9 @@ public Shooter(ShooterIO io) {
                 null,
                 null,
                 null,
-                (state) -> Logger.recordOutput("Shooter/SysIdState", state.toString())),
+                state -> Logger.recordOutput("Shooter/SysIdState", state.toString())),
             new SysIdRoutine.Mechanism(
-                (voltage) -> {
+                voltage -> {
                   m_io.setTopRollerVoltage(voltage.in(Volts));
                   m_io.setBottomRollerVoltage(voltage.in(Volts));
                 },