Addes some stuff for testing PID

simgui.json

@@ -218,6 +218,15 @@
                 1.0
               ],
               "id": "NT:/Robot/arm/getVoltage"
+            },
+            {
+              "color": [
+                0.7686275243759155,
+                0.30588236451148987,
+                0.32156863808631897,
+                1.0
+              ],
+              "id": "NT:/Robot/arm/goalAngleRadians"
             }
           ]
         }

src/main/java/org/sciborgs1155/robot/Robot.java

@@ -1,14 +1,13 @@
 package org.sciborgs1155.robot;
 
 import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.Radians;
 import static edu.wpi.first.units.Units.RadiansPerSecond;
 import static edu.wpi.first.units.Units.Second;
 import static edu.wpi.first.units.Units.Seconds;
 import static edu.wpi.first.wpilibj2.command.button.RobotModeTriggers.*;
 import static org.sciborgs1155.robot.Constants.DEADBAND;
 import static org.sciborgs1155.robot.Constants.PERIOD;
-import static org.sciborgs1155.robot.arm.ArmConstants.MAX_ANGLE;
-import static org.sciborgs1155.robot.arm.ArmConstants.MIN_ANGLE;
 import static org.sciborgs1155.robot.drive.DriveConstants.*;
 
 import edu.wpi.first.wpilibj.DataLogManager;
@@ -146,8 +145,8 @@ private void configureBindings() {
         .onTrue(Commands.runOnce(() -> speedMultiplier = Constants.SLOW_SPEED_MULTIPLIER))
         .onFalse(Commands.runOnce(() -> speedMultiplier = Constants.FULL_SPEED_MULTIPLIER));
 
-    operator.a().onTrue(arm.moveArmTo(MAX_ANGLE));
-    operator.b().onTrue(arm.moveArmTo(MIN_ANGLE));
+    operator.a().onTrue(arm.moveArmTo(Radians.of(1.5)));
+    operator.b().onTrue(arm.moveArmTo(Radians.of(-1.5)));
     // TODO: Add any additional bindings.
   }
 

src/main/java/org/sciborgs1155/robot/arm/Arm.java

@@ -1,6 +1,7 @@
 package org.sciborgs1155.robot.arm;
 
 import static edu.wpi.first.units.Units.Centimeters;
+import static edu.wpi.first.units.Units.Degrees;
 import static edu.wpi.first.units.Units.Radians;
 import static org.sciborgs1155.robot.arm.ArmConstants.*;
 
@@ -9,7 +10,6 @@
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
-import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
 import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj.util.Color8Bit;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -30,17 +30,25 @@ public class Arm extends SubsystemBase implements Logged, AutoCloseable {
   private final PIDController feedbackController = new PIDController(kP, kI, kD);
   private final ArmFeedforward feedforwardController = new ArmFeedforward(kS, kG, kV, kA);
 
+  @Log.NT
+  private double goalAngleRadians = 0;
+
   @Log.NT
   private final Mechanism2d armGUICanvas = new Mechanism2d(60, 60);
-  private final MechanismRoot2d armPivotGUI = armGUICanvas.getRoot("ArmPivot", 30, 30);
-  private final MechanismLigament2d armTowerGUI = armPivotGUI.append(new MechanismLigament2d("ArmTower", 30, -90));
-  private final MechanismLigament2d armGUI = armPivotGUI.append(
+  private final MechanismLigament2d armGUI = armGUICanvas.getRoot("Arm", 30, 30).append(
       new MechanismLigament2d(
           "Arm",
-          ARM_LENGTH.in(Centimeters),
+          ARM_LENGTH.in(Centimeters) * 5,
           STARTING_ANGLE.getDegrees(),
           10,
           new Color8Bit(Color.kSkyBlue)));
+  private final MechanismLigament2d goalGUI = armGUICanvas.getRoot("Setpoint", 30, 30).append(
+      new MechanismLigament2d(
+          "Arm",
+          ARM_LENGTH.in(Centimeters) * 5,
+          STARTING_ANGLE.getDegrees(),
+          10,
+          new Color8Bit(Color.kRed)));
 
   /**
    * Returns a new {@link Arm} subsystem, which will have real hardware if the
@@ -79,7 +87,7 @@ public double position() {
    */
   @Log.NT
   public double velocity() {
-    return hardware.velocity(); 
+    return hardware.velocity();
   }
 
   /**
@@ -92,9 +100,16 @@ public double getVoltage() {
 
   /** Moves the arm to a specified goal angle. */
   public Command moveArmTo(Angle goal) {
-    double feedForward = feedforwardController.calculate(hardware.position(), hardware.velocity());
-    double feedBack = feedbackController.calculate(hardware.position(), goal.in(Radians));
-    return run(() -> hardware.setVoltage(feedBack + feedForward))
+    return runOnce(() -> {
+      goalAngleRadians = goal.in(Radians);
+      goalGUI.setAngle(goal.in(Degrees));
+    }).andThen(run(() -> {
+      double feedBack = feedbackController.calculate(hardware.position(), goal.in(Radians));
+      double feedForward = feedforwardController.calculate(hardware.position(), hardware.velocity());
+
+      hardware.setVoltage(feedBack + feedForward);
+    }))
+
         .withName("Moving Arm To: " + goal.toString() + " radians")
         .andThen(Commands.print("Yippee"));
   }

src/main/java/org/sciborgs1155/robot/arm/ArmConstants.java

@@ -11,9 +11,9 @@
 /** Constants for the {@link Arm} subsystem. */
 public class ArmConstants {
   // TODO: not tuned (at all)
-  public static final double kP = 5;
+  public static final double kP = 1;
   public static final double kI = 0;
-  public static final double kD = 0.01;
+  public static final double kD = 0.5;
 
   public static final double kS = 1;
   public static final double kV = 0;
@@ -33,8 +33,8 @@ public class ArmConstants {
   public static final Distance ARM_LENGTH = Centimeters.of(10);
 
   /** Fully extended. */
-  public static final Angle MIN_ANGLE = Radians.of(-3);
+  public static final Angle MIN_ANGLE = Radians.of(-3.14);
 
   /** Fully retracted. */
-  public static final Angle MAX_ANGLE = Radians.of(3);
+  public static final Angle MAX_ANGLE = Radians.of(3.14);
 }

src/main/java/org/sciborgs1155/robot/arm/SimArm.java

@@ -18,15 +18,15 @@ public class SimArm implements ArmIO {
       ARM_LENGTH.in(Meters),
       MIN_ANGLE.in(Radians),
       MAX_ANGLE.in(Radians),
-      true,
+      false,
       STARTING_ANGLE.getRadians());
 
-  private Time previousUpdateTime = Seconds.of(0);
+  private Time previousSimulationUpdateTimestamp = Seconds.of(0);
 
   @Override
   public double position() {
-    simulation.update(Timer.getFPGATimestamp() - previousUpdateTime.in(Seconds));
-    previousUpdateTime = Seconds.of(Timer.getFPGATimestamp());
+    simulation.update(Timer.getFPGATimestamp() - previousSimulationUpdateTimestamp.in(Seconds));
+    previousSimulationUpdateTimestamp = Seconds.of(Timer.getFPGATimestamp());
     return simulation.getAngleRads();
   }