refactor(arm): Use velocity in setting setpoints.

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

@@ -99,6 +99,9 @@ public void setState(ArmState state) {
    * @return the state of the arm.
    */
   public ArmState getState() {
+    shoulderMotor.update(shoulderMotorValues);
+    elbowMotor.update(elbowMotorValues);
+
     return new ArmState(
         Rotation2d.fromRotations(shoulderMotorValues.positionRotations),
         Rotation2d.fromRotations(elbowMotorValues.positionRotations),
@@ -113,7 +116,7 @@ private void setGoal(ArmState goal) {
     this.goal = goal;
     this.setpoint = this.setpoint.nextSetpoint(goal);
 
-    shoulderMotor.setSetpoint(this.setpoint.shoulder().position);
+    shoulderMotor.setSetpoint(this.setpoint.shoulder().position, this.setpoint.shoulder().velocity);
     elbowMotor.setSetpoint(this.setpoint.elbow().position);
     // wristMotor.runSetpoint(state.wrist().getRotations());
   }

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

@@ -59,7 +59,7 @@ public Mechanism2d getMechanism() {
   public void setState(ArmState state) {
     Rotation2d shoulderRotation = Rotation2d.fromRotations(state.shoulder().position);
     Rotation2d elbowRotation = Rotation2d.fromRotations(state.elbow().position);
-    //Rotation2d wristRotation = Rotation2d.fromRotations(state.wrist().position);
+    // Rotation2d wristRotation = Rotation2d.fromRotations(state.wrist().position);
 
     shoulder.setAngle(shoulderRotation);
     elbow.setAngle(elbowRotation.minus(shoulderRotation));

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

@@ -32,7 +32,8 @@ public static class ShoulderMotorIOValues {
   /**
    * Runs the shoulder motor's setpoint.
    *
-   * @param positionRotations the shoulder motor's setpoint.
+   * @param positionRotations the shoulder motor's position setpoint.
+   * @param velocityRotationsPerSecond the shoulder motor's velocity setpoint.
    */
-  public void setSetpoint(double positionRotations);
+  public void setSetpoint(double positionRotations, double velocityRotationsPerSecond);
 }

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

@@ -59,12 +59,13 @@ public void setPosition(double positionRotations) {
   }
 
   @Override
-  public void setSetpoint(double positionRotations) {
+  public void setSetpoint(double positionRotations, double velocityRotationsPerSecond) {
     double measuredPositionRotations = Units.radiansToRotations(singleJointedArmSim.getAngleRads());
 
     double feedbackVolts = feedback.calculate(measuredPositionRotations, positionRotations);
 
-    double feedforwardVolts = feedforward.calculate(measuredPositionRotations, 0.0);
+    double feedforwardVolts =
+        feedforward.calculate(measuredPositionRotations, velocityRotationsPerSecond);
 
     singleJointedArmSim.setInputVoltage(feedbackVolts + feedforwardVolts);
   }

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

@@ -4,9 +4,8 @@
 import com.revrobotics.CANSparkLowLevel.MotorType;
 import com.revrobotics.CANSparkMax;
 import edu.wpi.first.math.controller.ArmFeedforward;
-import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
 import frc.lib.ArmFeedforwardCalculator;
 import frc.lib.Configurator;
 import frc.robot.arm.ArmConstants.ShoulderMotorConstants;
@@ -18,7 +17,7 @@ public class ShoulderMotorIOSparkMax implements ShoulderMotorIO {
   private final CANSparkMax sparkMax;
 
   /** Feedback controller for shoulder position. */
-  private final ProfiledPIDController feedback;
+  private final PIDController feedback;
 
   /** Feedforward controller for shoulder position. */
   private final ArmFeedforward feedforward;
@@ -27,13 +26,7 @@ public class ShoulderMotorIOSparkMax implements ShoulderMotorIO {
   public ShoulderMotorIOSparkMax() {
     sparkMax = new CANSparkMax(ShoulderMotorConstants.CAN.id(), MotorType.kBrushless);
 
-    feedback =
-        new ProfiledPIDController(
-            ShoulderMotorConstants.KP,
-            0,
-            0,
-            new Constraints(
-                ShoulderMotorConstants.MAXIMUM_SPEED, ShoulderMotorConstants.MAXIMUM_ACCELERATION));
+    feedback = new PIDController(ShoulderMotorConstants.KP, 0, 0);
 
     feedforward =
         new ArmFeedforward(
@@ -62,13 +55,13 @@ public void setPosition(double positionRotations) {
   }
 
   @Override
-  public void setSetpoint(double positionRotations) {
+  public void setSetpoint(double positionRotations, double velocityRotationsPerSecond) {
     double measuredPositionRotations = getPositionRotations();
 
     double feedbackVolts = feedback.calculate(measuredPositionRotations, positionRotations);
 
     double feedforwardVolts =
-        feedforward.calculate(measuredPositionRotations, feedback.getSetpoint().velocity);
+        feedforward.calculate(measuredPositionRotations, velocityRotationsPerSecond);
 
     sparkMax.setVoltage(feedbackVolts + feedforwardVolts);
   }