refactor(arm): Move motion profiling to outside of joints.

simgui-ds.json

@@ -99,6 +99,7 @@
   "robotJoysticks": [
     {},
     {
+      "guid": "78696e70757401000000000000000000",
       "useGamepad": true
     }
   ]

simgui.json

@@ -30,6 +30,9 @@
     "transitory": {
       "Shuffleboard": {
         "Arm": {
+          "Goal": {
+            "open": true
+          },
           "Position": {
             "open": true
           },

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

@@ -31,7 +31,7 @@ public class Arm extends Subsystem {
   /** Elbow motor values. */
   private final ElbowMotorIOValues elbowMotorValues = new ElbowMotorIOValues();
 
-  private ArmState setpoint;
+  private ArmState goal, setpoint;
 
   /** Creates a new instance of the arm subsystem. */
   private Arm() {
@@ -40,6 +40,7 @@ private Arm() {
 
     setState(new ArmState(Rotation2d.fromDegrees(90), new Rotation2d(), new Rotation2d()));
 
+    goal = getState();
     setpoint = getState();
   }
 
@@ -75,14 +76,20 @@ public void addToShuffleboard(ShuffleboardTab tab) {
 
     ShuffleboardLayout setpoint = Telemetry.addColumn(tab, "Setpoint");
 
-    setpoint.addDouble("Shoulder Setpoint (deg)", () -> this.setpoint.shoulder().getDegrees());
-    setpoint.addDouble("Elbow Setpoint (deg)", () -> this.setpoint.elbow().getDegrees());
-    setpoint.addDouble("Wrist Setpoint (deg)", () -> this.setpoint.wrist().getDegrees());
+    setpoint.addDouble("Shoulder Setpoint (deg)", () -> this.goal.shoulder().position);
+    setpoint.addDouble("Elbow Setpoint (deg)", () -> this.goal.elbow().position);
+    setpoint.addDouble("Wrist Setpoint (deg)", () -> this.goal.wrist().position);
+
+    ShuffleboardLayout goal = Telemetry.addColumn(tab, "Goal");
+
+    goal.addDouble("Shoulder Setpoint (deg)", () -> this.goal.shoulder().position);
+    goal.addDouble("Elbow Setpoint (deg)", () -> this.goal.elbow().position);
+    goal.addDouble("Wrist Setpoint (deg)", () -> this.goal.wrist().position);
   }
 
   public void setState(ArmState state) {
-    shoulderMotor.setPosition(state.shoulder().getRotations());
-    elbowMotor.setPosition(state.elbow().getRotations());
+    shoulderMotor.setPosition(state.shoulder().position);
+    elbowMotor.setPosition(state.elbow().position);
     // wristMotor.setPosition(state.wrist().getRotations());
   }
 
@@ -98,24 +105,25 @@ public ArmState getState() {
         Rotation2d.fromRotations(0));
   }
 
-  public ArmState getSetpoint() {
-    return setpoint;
+  public ArmState getGoal() {
+    return goal;
   }
 
-  private void setSetpoint(ArmState setpoint) {
-    this.setpoint = setpoint;
+  private void setGoal(ArmState goal) {
+    this.goal = goal;
+    this.setpoint = this.setpoint.nextSetpoint(goal);
 
-    shoulderMotor.setSetpoint(setpoint.shoulder().getRotations());
-    elbowMotor.setSetpoint(setpoint.elbow().getRotations());
+    shoulderMotor.setSetpoint(this.setpoint.shoulder().position);
+    elbowMotor.setSetpoint(this.setpoint.elbow().position);
     // wristMotor.runSetpoint(state.wrist().getRotations());
   }
 
   public Command runSetpoint(Supplier<ArmState> setpointSupplier) {
-    return run(() -> setSetpoint(setpointSupplier.get()));
+    return run(() -> setGoal(setpointSupplier.get()));
   }
 
   public Command hold() {
-    return runOnce(() -> setSetpoint(getState())).andThen(runSetpoint(this::getSetpoint));
+    return runOnce(() -> setGoal(getState())).andThen(runSetpoint(this::getGoal));
   }
 
   public Command driveElbowWithJoystick(DoubleSupplier joystickSupplier) {

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

@@ -1,6 +1,7 @@
 package frc.robot.arm;
 
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.util.Units;
 import frc.lib.CAN;
 import frc.lib.MotionProfileCalculator;
@@ -36,6 +37,13 @@ public static class ShoulderMotorConstants {
     /** Maximum acceleration of the shoulder joint in rotations per second per second. */
     public static final double MAXIMUM_ACCELERATION =
         MotionProfileCalculator.calculateAcceleration(MAXIMUM_SPEED, 0.3);
+
+    /** Maximum speed and acceleration of the shoulder joint. */
+    public static final TrapezoidProfile.Constraints CONSTRAINTS =
+        new TrapezoidProfile.Constraints(MAXIMUM_SPEED, MAXIMUM_ACCELERATION);
+
+    /** Motion profile of the shoulder joint using constraints. */
+    public static final TrapezoidProfile MOTION_PROFILE = new TrapezoidProfile(CONSTRAINTS);
   }
 
   /** Constants for the elbow motor. */

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

@@ -1,5 +1,6 @@
 package frc.robot.arm;
 
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
@@ -56,7 +57,11 @@ public Mechanism2d getMechanism() {
   }
 
   public void setState(ArmState state) {
-    shoulder.setAngle(state.shoulder());
-    elbow.setAngle(state.elbow().minus(state.shoulder()));
+    Rotation2d shoulderRotation = Rotation2d.fromRotations(state.shoulder().position);
+    Rotation2d elbowRotation = Rotation2d.fromRotations(state.elbow().position);
+    //Rotation2d wristRotation = Rotation2d.fromRotations(state.wrist().position);
+
+    shoulder.setAngle(shoulderRotation);
+    elbow.setAngle(elbowRotation.minus(shoulderRotation));
   }
 }

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

@@ -1,51 +1,66 @@
 package frc.robot.arm;
 
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
+import frc.robot.RobotConstants;
+import frc.robot.arm.ArmConstants.ShoulderMotorConstants;
 import java.util.Objects;
 
 /** State of the arm. */
-public record ArmState(Rotation2d shoulder, Rotation2d elbow, Rotation2d wrist) {
+public record ArmState(State shoulder, State elbow, State wrist) {
 
   /**
    * Creates an arm state.
    *
-   * @param shoulder the shoulder's rotation.
-   * @param elbow the elbow's rotation.
-   * @param wrist the wrist's rotation.
+   * @param shoulder the shoulder's state.
+   * @param elbow the elbow's state.
+   * @param wrist the wrist's state.
    */
   public ArmState {
     Objects.requireNonNull(shoulder);
     Objects.requireNonNull(elbow);
     Objects.requireNonNull(wrist);
   }
 
+  /**
+   * Creates an arm state.
+   *
+   * @param shoulder the shoulder's rotation.
+   * @param elbow the elbow's rotation.
+   * @param wrist the wrist's rotation.
+   */
+  public ArmState(Rotation2d shoulder, Rotation2d elbow, Rotation2d wrist) {
+    this(
+        new State(shoulder.getRotations(), 0),
+        new State(elbow.getRotations(), 0),
+        new State(wrist.getRotations(), 0));
+  }
+
   /**
    * Copies this arm state with a new shoulder rotation.
    *
    * @param newShoulder the new shoulder rotation.
    * @return a copy of this arm state with a new shoulder rotation.
    */
   public ArmState withShoulder(Rotation2d newShoulder) {
-    return new ArmState(newShoulder, elbow, wrist);
+    return withShoulder(new State(newShoulder.getRotations(), 0));
   }
 
   /**
-   * Copies this arm state with a new elbow rotation.
+   * Copies this arm state with a new shoulder state.
    *
-   * @param newElbow the new elbow rotation.
-   * @return a copy of this arm state with a new elbow rotation.
+   * @param newShoulder the new shoulder state.
+   * @return a copy of this arm state with a new shoulder rotation.
    */
-  public ArmState withElbow(Rotation2d newElbow) {
-    return new ArmState(shoulder, newElbow, wrist);
+  public ArmState withShoulder(State newShoulder) {
+    return new ArmState(newShoulder, elbow, wrist);
   }
 
-  /**
-   * Copies this arm state with a new wrist rotation.
-   *
-   * @param newWrist the new wrist rotation.
-   * @return a copy of this arm state with a new wrist rotation.
-   */
-  public ArmState withWrist(Rotation2d newWrist) {
-    return new ArmState(shoulder, elbow, newWrist);
+  public ArmState nextSetpoint(ArmState goal) {
+    State nextShoulderState =
+        ShoulderMotorConstants.MOTION_PROFILE.calculate(
+            RobotConstants.PERIODIC_DURATION, this.shoulder, goal.shoulder);
+
+    return withShoulder(nextShoulderState);
   }
 }

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

@@ -1,10 +1,9 @@
 package frc.robot.arm;
 
 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.system.plant.DCMotor;
-import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
 import frc.lib.ArmFeedforwardCalculator;
@@ -16,7 +15,7 @@ public class ShoulderMotorIOSim implements ShoulderMotorIO {
 
   private final SingleJointedArmSim singleJointedArmSim;
 
-  private final ProfiledPIDController feedback;
+  private final PIDController feedback;
 
   private final ArmFeedforward feedforward;
 
@@ -33,13 +32,7 @@ public ShoulderMotorIOSim() {
             true,
             Units.degreesToRadians(90));
 
-    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(