Merge branch 'week2' of https://github.com/team4909/2024-Crescendo in…

…to week2

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,7 +1,11 @@
 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;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.filter.Debouncer.DebounceType;
 import edu.wpi.first.math.geometry.Pose3d;
@@ -14,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;
@@ -28,12 +33,13 @@ public class Arm extends SubsystemBase {
   private final double kSDeadband = 0.05;
   private final ArmIO m_io;
   private final ArmIOInputsAutoLogged m_inputs = new ArmIOInputsAutoLogged();
-  private final ArmModel m_armModel = new ArmModel();
   private final ArmVisualizer m_goalVisualizer, m_setpointVisualizer, m_measuredVisualizer;
   private ProfiledPIDController m_elbowController =
       new ProfiledPIDController(0.0, 0.0, 0.0, new TrapezoidProfile.Constraints(0.0, 0.0));
   private ProfiledPIDController m_wristController =
       new ProfiledPIDController(0.0, 0.0, 0.0, new TrapezoidProfile.Constraints(0.0, 0.0));
+  private final ArmFeedforward m_elbowFeedForward = new ArmFeedforward(0.0, 0.15, 2.31);
+  private final ArmFeedforward m_wristFeedForward = new ArmFeedforward(0.0, 0.51, 0.8);
 
   private static final LoggedTunableNumber elbowkP = new LoggedTunableNumber("Arm/Elbow/kP");
   private static final LoggedTunableNumber elbowkD = new LoggedTunableNumber("Arm/Elbow/kD");
@@ -60,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) {
@@ -102,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());
   }
 
@@ -190,12 +215,10 @@ private void runSetpoint(double elbowDelay, double wristDelay) {
     } else
       wristFeedbackVolts =
           m_deadbandkS.apply(m_wristController.calculate(m_inputs.wristPositionRad), wristkS.get());
-    final Vector<N2> feedforwardVolts =
-        m_armModel.feedforward(
-            VecBuilder.fill(elbowNextPosition, wristNextPosition),
-            VecBuilder.fill(elbowNextVelocity, wristNextVelocity));
-    double elbowFeedForwardVolts = feedforwardVolts.get(0, 0);
-    double wristFeedForwardVolts = feedforwardVolts.get(1, 0);
+    double elbowFeedForwardVolts =
+        m_elbowFeedForward.calculate(elbowNextPosition, elbowNextVelocity);
+    double wristFeedForwardVolts =
+        m_wristFeedForward.calculate(wristNextPosition, wristNextVelocity);
     Logger.recordOutput("Arm/Elbow Feed Forward", elbowFeedForwardVolts);
     Logger.recordOutput("Arm/Wrist Feed Forward", wristFeedForwardVolts);
     m_io.setElbowVoltage(elbowFeedbackVolts + elbowFeedForwardVolts);
@@ -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,8 +2,6 @@
 
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.Vector;
-import edu.wpi.first.math.controller.ArmFeedforward;
-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;
@@ -12,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;
@@ -25,8 +20,6 @@ public class ArmModel {
   private final double kElbowMassKg = Units.lbsToKilograms(7.0);
   private final double kElbowMoiKgMetersSq =
       SingleJointedArmSim.estimateMOI(kElbowLengthMeters, kElbowMassKg);
-  private final double elbowkG = 0.15;
-  private final double elbowkV = 2.31;
 
   public static final double kWristGearboxReduction = 3.0 * 5.0;
   public static final double kWristChainReduction = 36.0 / 22.0;
@@ -38,15 +31,9 @@ public class ArmModel {
   private final double kWristMassKg = Units.lbsToKilograms(11.5);
   private final double kWristMoiKgMetersSq =
       SingleJointedArmSim.estimateMOI(kWristLengthMeters, kWristMassKg);
-  private final double wristkG = 0.51;
-  private final double wristkV = 0.8;
-
-  private final ArmFeedforward m_elbowFeedForward, m_wristFeedForward;
   private final SingleJointedArmSim m_elbowSim, m_wristSim;
 
   public ArmModel() {
-    m_elbowFeedForward = new ArmFeedforward(0.0, elbowkG, elbowkV, 0.0);
-    m_wristFeedForward = new ArmFeedforward(0.0, wristkG, wristkV, 0.0);
     m_elbowSim =
         new SingleJointedArmSim(
             kElbowGearbox,
@@ -69,16 +56,6 @@ public ArmModel() {
             0.0);
   }
 
-  public Vector<N2> feedforward(Vector<N2> position) {
-    return feedforward(position, VecBuilder.fill(0.0, 0.0));
-  }
-
-  public Vector<N2> feedforward(Vector<N2> position, Vector<N2> velocity) {
-    return VecBuilder.fill(
-        m_elbowFeedForward.calculate(position.get(0, 0), velocity.get(0, 0)),
-        m_wristFeedForward.calculate(position.get(0, 0), velocity.get(1, 0)));
-  }
-
   /**
    * Adjusts the simulated state of the arm based on applied voltages.
    *

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));
                 },