refactor: Remove wrist.

src/main/java/frc/robot/RobotMechanisms.java

@@ -113,25 +113,16 @@ public Mechanism2d getMechanism() {
 
   public void updateSuperstructure(SuperstructureState state) {
     Rotation2d shoulderRotation = Rotation2d.fromRotations(state.shoulderAngleRotations().position);
-    Rotation2d wristRotation = Rotation2d.fromRotations(state.wristAngleRotations().position);
 
     shoulder.setAngle(shoulderRotation);
 
-    // Offset the rendered wrist rotation so that zero degrees is perpendicular to the shoulder,
-    // rather than parallel with the shoulder
-    Rotation2d offsetWristRotation = wristRotation.plus(Rotation2d.fromDegrees(90));
-    Rotation2d shooterRotation = offsetWristRotation;
-    Rotation2d serializerRotation = offsetWristRotation.plus(Rotation2d.fromDegrees(180));
-
-    flywheel.setAngle(shooterRotation);
     flywheel.setColor(
         new Color8Bit(
             flywheelColor.sample(
                 Math.abs(state.flywheelVelocityRotationsPerSecond()),
                 0,
                 FlywheelConstants.MAXIMUM_TANGENTIAL_SPEED)));
 
-    serializer.setAngle(serializerRotation);
     serializer.setColor(
         new Color8Bit(
             serializerColor.sample(

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

@@ -7,45 +7,25 @@
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import frc.lib.Subsystem;
 import frc.lib.Telemetry;
-import frc.robot.arm.LimitSwitchIO.LimitSwitchIOValues;
 import frc.robot.arm.ShoulderMotorIO.ShoulderMotorIOValues;
-import frc.robot.arm.WristMotorIO.WristMotorIOValues;
 
 /** Subsystem class for the arm subsystem. */
 public class Arm extends Subsystem {
 
   /** Instance variable for the arm subsystem singleton. */
   private static Arm instance = null;
 
-  /** Limit switch. */
-  private final LimitSwitchIO limitSwitch;
-
-  /** Limit switch values. */
-  private final LimitSwitchIOValues limitSwitchValues = new LimitSwitchIOValues();
-
   /** Shoulder motor. */
   private final ShoulderMotorIO shoulderMotor;
 
   /** Shoulder motor values. */
   private final ShoulderMotorIOValues shoulderMotorValues = new ShoulderMotorIOValues();
 
-  /** Wrist motor. */
-  private final WristMotorIO wristMotor;
-
-  /** Wrist motor values. */
-  private final WristMotorIOValues wristMotorValues = new WristMotorIOValues();
-
   /** Creates a new instance of the arm subsystem. */
   private Arm() {
-    limitSwitch = ArmFactory.createLimitSwitch();
     shoulderMotor = ArmFactory.createShoulderMotor();
-    wristMotor = ArmFactory.createWristMotor();
 
-    limitSwitch.configure();
     shoulderMotor.configure();
-    wristMotor.configure();
-
-    // setPosition(ArmState.INIT);
   }
 
   /**
@@ -63,29 +43,20 @@ public static Arm getInstance() {
 
   @Override
   public void periodic() {
-    limitSwitch.update(limitSwitchValues);
     shoulderMotor.update(shoulderMotorValues);
-    wristMotor.update(wristMotorValues);
   }
 
   @Override
   public void addToShuffleboard(ShuffleboardTab tab) {
-    ShuffleboardLayout limitSwitch = Telemetry.addColumn(tab, "Limit Switch");
-
-    limitSwitch.addBoolean("Is Pressed?", () -> limitSwitchValues.isPressed);
-
     ShuffleboardLayout position = Telemetry.addColumn(tab, "Position");
 
     position.addDouble(
         "Shoulder Position (deg)",
         () -> Units.rotationsToDegrees(shoulderMotorValues.positionRotations));
-    position.addDouble(
-        "Wrist Position (deg)", () -> Units.rotationsToDegrees(wristMotorValues.positionRotations));
 
     ShuffleboardLayout voltages = Telemetry.addColumn(tab, "Voltages");
 
     voltages.addDouble("Shoulder Voltage (V)", () -> shoulderMotorValues.inputVoltage);
-    voltages.addDouble("Wrist Voltage (V)", () -> wristMotorValues.inputVoltage);
 
     ShuffleboardLayout derivatives = Telemetry.addColumn(tab, "Derivatives");
 
@@ -94,36 +65,18 @@ public void addToShuffleboard(ShuffleboardTab tab) {
     derivatives.addDouble(
         "Shoulder Acceleration (rpsps)",
         () -> shoulderMotorValues.accelerationRotationsPerSecondPerSecond);
-    derivatives.addDouble(
-        "Wrist Velocity (rps)", () -> wristMotorValues.velocityRotationsPerSecond);
-    derivatives.addDouble(
-        "Wrist Acceleration (rpsps)",
-        () -> wristMotorValues.accelerationRotationsPerSecondPerSecond);
   }
 
   public State getMeasuredShoulderState() {
     return new TrapezoidProfile.State(
         shoulderMotorValues.positionRotations, shoulderMotorValues.velocityRotationsPerSecond);
   }
 
-  public State getMeasuredWristState() {
-    return new TrapezoidProfile.State(
-        wristMotorValues.positionRotations, wristMotorValues.velocityRotationsPerSecond);
-  }
-
   public void setShoulderPosition(double positionRotations) {
     shoulderMotor.setPosition(positionRotations);
   }
 
-  public void setWristPosition(double positionRotations) {
-    wristMotor.setPosition(positionRotations);
-  }
-
   public void setShoulderSetpoint(double positionRotations, double velocityRotationsPerSecond) {
     shoulderMotor.setSetpoint(positionRotations, velocityRotationsPerSecond);
   }
-
-  public void setWristSetpoint(double positionRotations, double velocityRotationsPerSecond) {
-    wristMotor.setSetpoint(positionRotations, velocityRotationsPerSecond);
-  }
 }

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

@@ -20,18 +20,4 @@ public static class ShoulderMotorConstants {
             DCMotor.getNEO(1), // motor
             1);
   }
-
-  /** Constants for the wrist motor. */
-  public static class WristMotorConstants {
-    /** Joint constants for the wrist joint. */
-    public static final JointConstants JOINT_CONSTANTS =
-        new JointConstants(
-            Units.lbsToKilograms(8.016), // massKg
-            Units.inchesToMeters(5.135), // lengthMeters
-            Units.inchesToMeters(3.47629), // radiusMeters
-            0.02835,
-            20.454545,
-            DCMotor.getNEO(1), // motor
-            1);
-  }
 }

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

@@ -7,13 +7,6 @@
 /** Helper class for creating hardware for the arm subsystem. */
 public class ArmFactory {
 
-  public static LimitSwitchIO createLimitSwitch() {
-    if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.ARM))
-      return new LimitSwitchIODigital();
-
-    return new LimitSwitchIOSim();
-  }
-
   /**
    * Creates a shoulder motor.
    *
@@ -25,16 +18,4 @@ public static ShoulderMotorIO createShoulderMotor() {
 
     return new ShoulderMotorIOSim();
   }
-
-  /**
-   * Creates an elbow motor.
-   *
-   * @return an elbow motor.
-   */
-  public static WristMotorIO createWristMotor() {
-    if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.ARM))
-      return new WristMotorIOSparkMax();
-
-    return new WristMotorIOSim();
-  }
 }