mechanism2d wrist visualizer

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

@@ -14,26 +14,30 @@
 public class ArmVisualizer {
   private final Mechanism2d mechanism;
   private final MechanismLigament2d arm;
+  private final MechanismLigament2d wrist;
   private final String key;
 
   public ArmVisualizer(String key, Color color) {
     this.key = key;
     mechanism = new Mechanism2d(3.0, 3.0, new Color8Bit(Color.kWhite));
     MechanismRoot2d root = mechanism.getRoot("pivot", 1.0, 0.4);
-    arm = new MechanismLigament2d("arm", ArmConstants.ARM_LENGTH_METERS, 20.0, 6, new Color8Bit(color));
+    arm = new MechanismLigament2d("arm", ArmConstants.ARM_LENGTH_METERS, 0.0, 6, new Color8Bit(color));
+    wrist = new MechanismLigament2d("wrist", ArmConstants.WRIST_LENGTH_METERS, 45.0, 5, new Color8Bit(color));
     root.append(arm);
+    arm.append(wrist);
   }
 
   /** Update arm visualizer with current arm angle */
-  public void update(double angleDegs) {
+  public void update(double armAngleDegs, double wristAngleDegs) {
     // Log Mechanism2d
-    arm.setAngle(Rotation2d.fromDegrees(angleDegs));
+    arm.setAngle(Rotation2d.fromDegrees(armAngleDegs));
+    wrist.setAngle(Rotation2d.fromDegrees(wristAngleDegs));
     Logger.recordOutput("Arm/Mechanism2d/" + key, mechanism);
 
     // Log 3D poses
     Pose3d pivotArm =
             new Pose3d(ArmConstants.PIVOT_JOINT_TRANSLATION.getX(), 0.0,
-                    ArmConstants.PIVOT_JOINT_TRANSLATION.getY(), new Rotation3d(0.0, -angleDegs, 0.0));
+                    ArmConstants.PIVOT_JOINT_TRANSLATION.getY(), new Rotation3d(0.0, -armAngleDegs, 0.0));
     Logger.recordOutput("Arm/Mechanism3d/" + key, pivotArm);
   }
 }