feat(arm): Test hold-in-place command.

simgui.json

@@ -29,6 +29,15 @@
   "NetworkTables": {
     "transitory": {
       "Shuffleboard": {
+        "Arm": {
+          "Position": {
+            "open": true
+          },
+          "Setpoint": {
+            "open": true
+          },
+          "open": true
+        },
         "Odometry": {
           "Velocity": {
             "open": true

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

@@ -1,6 +1,7 @@
 package frc.robot.arm;
 
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -31,12 +32,16 @@ public class Arm extends Subsystem {
   /** Elbow motor values. */
   private final ElbowMotorIOValues elbowMotorValues = new ElbowMotorIOValues();
 
+  private ArmState setpoint;
+
   /** Creates a new instance of the arm subsystem. */
   private Arm() {
     shoulderMotor = ArmFactory.createShoulderMotor();
     elbowMotor = ArmFactory.createElbowMotor();
 
     setState(new ArmState(Rotation2d.fromDegrees(90), new Rotation2d(), new Rotation2d()));
+
+    setpoint = getState();
   }
 
   /**
@@ -61,13 +66,19 @@ public void periodic() {
 
   @Override
   public void addToShuffleboard(ShuffleboardTab tab) {
-    ShuffleboardLayout shoulder = Telemetry.addColumn(tab, "Shoulder");
+    ShuffleboardLayout position = Telemetry.addColumn(tab, "Position");
 
-    shoulder.addDouble("Position (rot)", () -> shoulderMotorValues.positionRotations);
+    position.addDouble(
+        "Shoulder Position (deg)",
+        () -> Units.rotationsToDegrees(shoulderMotorValues.positionRotations));
+    position.addDouble(
+        "Elbow Position (deg)", () -> Units.rotationsToDegrees(elbowMotorValues.positionRotations));
 
-    ShuffleboardLayout elbow = Telemetry.addColumn(tab, "Elbow");
+    ShuffleboardLayout setpoint = Telemetry.addColumn(tab, "Setpoint");
 
-    elbow.addDouble("Position (rot)", () -> elbowMotorValues.positionRotations);
+    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());
   }
 
   public void setState(ArmState state) {
@@ -88,31 +99,41 @@ public ArmState getState() {
         Rotation2d.fromRotations(0));
   }
 
-  private void runSetpoint(ArmState state) {
-    shoulderMotor.runSetpoint(state.shoulder().getRotations());
-    elbowMotor.runSetpoint(state.elbow().getRotations());
+  public ArmState getSetpoint() {
+    return setpoint;
+  }
+
+  private void setSetpoint(ArmState setpoint) {
+    this.setpoint = setpoint;
+
+    shoulderMotor.setSetpoint(setpoint.shoulder().getRotations());
+    elbowMotor.setSetpoint(setpoint.elbow().getRotations());
     // wristMotor.runSetpoint(state.wrist().getRotations());
   }
 
-  public Command runSetpoint(Supplier<ArmState> stateSupplier) {
-    return run(() -> runSetpoint(stateSupplier.get()));
+  public Command runSetpoint(Supplier<ArmState> setpointSupplier) {
+    return run(() -> setSetpoint(setpointSupplier.get()));
   }
 
   public Command hold() {
-    return runSetpoint(this::getState);
+    return runOnce(() -> setSetpoint(getState())).andThen(runSetpoint(this::getSetpoint));
   }
 
   public Command driveShoulderWithJoystick(DoubleSupplier joystickSupplier) {
     return run(() ->
             shoulderMotor.setVoltage(
-                joystickSupplier.getAsDouble() * -1 * Math.abs(ShoulderMotorConstants.MAXIMUM_VOLTAGE)))
+                joystickSupplier.getAsDouble()
+                    * -1
+                    * Math.abs(ShoulderMotorConstants.MAXIMUM_VOLTAGE)))
         .finallyDo(shoulderMotor::stop);
   }
 
   public Command driveElbowWithJoystick(DoubleSupplier joystickSupplier) {
     return run(() ->
             elbowMotor.setVoltage(
-                joystickSupplier.getAsDouble() * -1 * Math.abs(ElbowMotorConstants.MAXIMUM_VOLTAGE)))
+                joystickSupplier.getAsDouble()
+                    * -1
+                    * Math.abs(ElbowMotorConstants.MAXIMUM_VOLTAGE)))
         .finallyDo(shoulderMotor::stop);
   }
 }

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

@@ -25,8 +25,22 @@ public class ArmMechanism {
   private ArmMechanism() {
     mechanism = new Mechanism2d(1, 1);
     root = mechanism.getRoot("arm", 0.15, 0);
-    shoulder = root.append(new MechanismLigament2d("shoulder", ShoulderMotorConstants.SHOULDER_TO_ELBOW_DISTANCE, 90, THICKNESS, new Color8Bit(Color.kOrange)));
-    elbow = shoulder.append(new MechanismLigament2d("elbow", ElbowMotorConstants.ELBOW_TO_WRIST_DISTANCE, 90, THICKNESS, new Color8Bit(Color.kGreen)));
+    shoulder =
+        root.append(
+            new MechanismLigament2d(
+                "shoulder",
+                ShoulderMotorConstants.SHOULDER_TO_ELBOW_DISTANCE,
+                90,
+                THICKNESS,
+                new Color8Bit(Color.kOrange)));
+    elbow =
+        shoulder.append(
+            new MechanismLigament2d(
+                "elbow",
+                ElbowMotorConstants.ELBOW_TO_WRIST_DISTANCE,
+                90,
+                THICKNESS,
+                new Color8Bit(Color.kGreen)));
   }
 
   public static ArmMechanism getInstance() {

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

@@ -33,7 +33,7 @@ public static class ElbowMotorIOValues {
    *
    * @param positionRotations the elbow motor's setpoint.
    */
-  public void runSetpoint(double positionRotations);
+  public void setSetpoint(double positionRotations);
 
   // TODO Remove, only for characterization
   /**

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

@@ -13,7 +13,7 @@ public void update(ElbowMotorIOValues values) {}
   public void setPosition(double positionRotations) {}
 
   @Override
-  public void runSetpoint(double positionRotations) {}
+  public void setSetpoint(double positionRotations) {}
 
   @Override
   public void setVoltage(double volts) {}

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

@@ -34,7 +34,7 @@ public static class ShoulderMotorIOValues {
    *
    * @param positionRotations the shoulder motor's setpoint.
    */
-  public void runSetpoint(double positionRotations);
+  public void setSetpoint(double positionRotations);
 
   // TODO Remove, only for characterization
   /**

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

@@ -1,5 +1,7 @@
 package frc.robot.arm;
 
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
@@ -11,6 +13,10 @@ public class ShoulderMotorIOSim implements ShoulderMotorIO {
 
   private final SingleJointedArmSim singleJointedArmSim;
 
+  private final PIDController feedback;
+
+  private final ArmFeedforward feedforward;
+
   /** Creates a new simulated shoulder motor. */
   public ShoulderMotorIOSim() {
     singleJointedArmSim =
@@ -21,13 +27,18 @@ public ShoulderMotorIOSim() {
             ShoulderMotorConstants.SHOULDER_TO_ELBOW_DISTANCE,
             Units.degreesToRadians(10),
             Units.degreesToRadians(90),
-            false,
+            true,
             Units.degreesToRadians(90));
+
+    feedback = new PIDController(10.0, 0, 0);
+
+    double kG = 0.101859 / Math.cos(Units.degreesToRadians(70.81));
+
+    feedforward = new ArmFeedforward(0, kG, 0);
   }
 
   @Override
-  public void configure() {
-  }
+  public void configure() {}
 
   @Override
   public void update(ShoulderMotorIOValues values) {
@@ -43,13 +54,21 @@ public void setPosition(double positionRotations) {
   }
 
   @Override
-  public void runSetpoint(double positionRotations) {
-    //setPosition(positionRotations);
+  public void setSetpoint(double positionRotations) {
+    double measuredPositionRotations = Units.radiansToRotations(singleJointedArmSim.getAngleRads());
+
+    double feedbackVolts = feedback.calculate(measuredPositionRotations, positionRotations);
+
+    double feedforwardVolts = feedforward.calculate(measuredPositionRotations, 0.0);
+
+    setVoltage(feedbackVolts + feedforwardVolts);
   }
 
   // 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

@@ -47,7 +47,7 @@ public void setPosition(double positionRotations) {
   }
 
   @Override
-  public void runSetpoint(double positionRotations) {
+  public void setSetpoint(double positionRotations) {
     double measuredPositionRotations = getPositionRotations();
 
     double feedbackVolts = feedback.calculate(positionRotations, measuredPositionRotations);