feat(arm): Add shoulder stops.

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

@@ -1,5 +1,6 @@
 package frc.robot.arm;
 
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
 import frc.lib.CAN;
 
@@ -16,6 +17,12 @@ public static class ShoulderMotorConstants {
     /** Maximum voltage that can be applied to the shoulder motor. */
     public static final double MAXIMUM_VOLTAGE = 2.0;
 
+    /** Minumum angle of the shoulder joint. */
+    public static final Rotation2d MINIMUM_ANGLE = Rotation2d.fromDegrees(20);
+
+    /** Maximum angle of the shoulder joint. */
+    public static final Rotation2d MAXIMUM_ANGLE = Rotation2d.fromDegrees(90);
+
     /** Shoulder pivot to elbow pivot distance in meters. */
     public static final double SHOULDER_TO_ELBOW_DISTANCE = Units.inchesToMeters(16.775);
   }

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

@@ -25,8 +25,8 @@ public ShoulderMotorIOSim() {
             ShoulderMotorConstants.GEARING,
             0.1,
             ShoulderMotorConstants.SHOULDER_TO_ELBOW_DISTANCE,
-            Units.degreesToRadians(10),
-            Units.degreesToRadians(90),
+            ShoulderMotorConstants.MINIMUM_ANGLE.getRadians(),
+            ShoulderMotorConstants.MAXIMUM_ANGLE.getRadians(),
             true,
             Units.degreesToRadians(90));
 
@@ -67,8 +67,6 @@ public void setSetpoint(double positionRotations) {
   // TODO Remove, only for characterization
   @Override
   public void setVoltage(double volts) {
-    System.out.println(volts);
-
     singleJointedArmSim.setInputVoltage(volts);
   }
 

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

@@ -67,6 +67,14 @@ public void setVoltage(double volts) {
         MathUtil.clamp(
             volts, -ShoulderMotorConstants.MAXIMUM_VOLTAGE, ShoulderMotorConstants.MAXIMUM_VOLTAGE);
 
+    if (volts > 0.0
+        && getPositionRotations() > ShoulderMotorConstants.MAXIMUM_ANGLE.getRotations()) {
+      volts = 0.0;
+    } else if (volts < 0.0
+        && getPositionRotations() < ShoulderMotorConstants.MINIMUM_ANGLE.getRotations()) {
+      volts = 0.0;
+    }
+
     sparkMax.setVoltage(volts);
   }