Arm Interface Cleanup

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

@@ -24,11 +24,7 @@ public default double getTargetAngle() {
   }
 
   // sets of the angle of the arm
-  public default void setAngle(double degrees) {
-    setAngle(degrees, 0);
-  }
-
-  public default void setAngle(double degrees, double velocityDegreesPerSecond) {}
+  public default void setAngle(double degrees) {}
 
   public default boolean isAbsoluteEncoderConnected() {
     return true;

src/main/java/frc/robot/subsystems/arm/ArmIO.java

@@ -5,9 +5,10 @@
 public interface ArmIO {
   @AutoLog
   public static class ArmIOInputs {
-    public double positionRad = 0.0;
-    public double positionDegree = 0.0;
-    public double velocityInDegrees = 0.0;
+    public double positionRads = 0.0;
+    public double positionDegrees = 0.0;
+    public double velocityRadsPerSecond = 0.0;
+    public double velocityDegreesPerSecond = 0.0;
 
     public double absolutePositionRaw;
     public boolean absoluteEncoderConnected = false;

src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java

@@ -96,8 +96,8 @@ public ArmIOSparkMax(int id, boolean inverted) {
   /** Updates the set of loggable inputs. */
   @Override
   public void updateInputs(ArmIOInputs inputs) {
-    inputs.positionRad = getOffsetCorrectedAbsolutePositionInRadians();
-    inputs.positionDegree = Units.radiansToDegrees(inputs.positionRad);
+    inputs.positionRads = getOffsetCorrectedAbsolutePositionInRadians();
+    inputs.positionDegrees = Units.radiansToDegrees(inputs.positionRads);
     inputs.absolutePositionRaw = absoluteEncoder.getAbsolutePosition();
     inputs.absoluteEncoderConnected = isAbsoluteEncoderConnected();
 
@@ -106,7 +106,8 @@ public void updateInputs(ArmIOInputs inputs) {
     inputs.current = motor.getOutputCurrent();
 
     inputs.relativePositionDegrees = relEncoder.getPosition();
-    inputs.velocityInDegrees = relEncoder.getVelocity();
+    inputs.velocityDegreesPerSecond = relEncoder.getVelocity();
+    inputs.velocityRadsPerSecond = Units.degreesToRadians(inputs.velocityDegreesPerSecond);
 
     // Code below allows PID to be tuned using SmartDashboard.  And outputs extra data to
     // SmartDashboard.

src/main/java/frc/robot/subsystems/arm/ArmIOStub.java

@@ -13,12 +13,12 @@ public class ArmIOStub implements ArmIO {
   // The P gain for the PID controller that drives this arm.
   private double targetDegrees = 0;
   private double feedForwardVolts = 0;
-  private double armGearingReduction = 317;
-  private double armLengthInMeters = .5;
   private double minAngleInDegrees = ArmConstants.minAngleInDegrees;
   private double maxAngleInDegrees = ArmConstants.maxAngleInDegrees;
-  private double armMassInKg = 11.3398;
 
+  private double armGearingReduction = 210;
+  private double armLengthInMeters = 0.6096;
+  private double armMassInKg = 13.60777;
   // The arm gearbox represents a gearbox containing two Vex 775pro motors.
   private final DCMotor motorPlant = DCMotor.getNEO(1);
   private double currentVoltage = 0.0;
@@ -49,15 +49,17 @@ public class ArmIOStub implements ArmIO {
   /** Updates the set of loggable inputs. */
   @Override
   public void updateInputs(ArmIOInputs inputs) {
-    inputs.positionRad = arm.getAngleRads();
-    inputs.positionDegree = Units.radiansToDegrees(inputs.positionRad);
-    inputs.velocityInDegrees = Units.radiansToDegrees(arm.getVelocityRadPerSec());
+    inputs.positionRads = arm.getAngleRads();
+    inputs.positionDegrees = Units.radiansToDegrees(inputs.positionRads);
+    inputs.velocityRadsPerSecond = arm.getVelocityRadPerSec();
+    inputs.velocityDegreesPerSecond = Units.radiansToDegrees(inputs.velocityRadsPerSecond);
+    inputs.relativePositionDegrees = inputs.positionDegrees;
     inputs.appliedVolts = currentVoltage;
 
     if (softwarePidEnabled) {
       currentVoltage =
           feedForwardVolts
-              + pid.calculate(inputs.positionDegree, targetDegrees)
+              + pid.calculate(inputs.positionDegrees, targetDegrees)
                   * RobotController.getBatteryVoltage();
     }
 

src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java

@@ -28,7 +28,6 @@ public class ArmSubsystem extends SubsystemBase implements Arm {
   @AutoLogOutput private double targetVoltage;
   @AutoLogOutput private double targetDegrees;
   @AutoLogOutput private double targetRelativeDegrees;
-  @AutoLogOutput private double targetVelocityDegreesPerSecond;
 
   // Create a Mechanism2d display of an Arm with a fixed ArmTower and moving Arm.
   private final double armAngle2dOffset = 0;
@@ -87,12 +86,12 @@ public ArmSubsystem(ArmIO io) {
 
   @Override
   public double getAngle() {
-    return inputs.positionDegree;
+    return inputs.positionDegrees;
   }
 
   @Override
   public double getVelocity() {
-    return inputs.velocityInDegrees;
+    return inputs.velocityDegreesPerSecond;
   }
 
   @Override
@@ -107,7 +106,7 @@ public double getTargetAngle() {
 
   // sets of the angle of the arm
   @Override
-  public void setAngle(double degrees, double velocityDegreesPerSecond) {
+  public void setAngle(double degrees) {
     Logger.recordOutput("Arm/setAngle/requestedAngleDegress", degrees);
     // Don't try to set position if absolute encoder is broken/missing.
     if (isAbsoluteEncoderConnected() == false) {
@@ -132,7 +131,6 @@ public void setAngle(double degrees, double velocityDegreesPerSecond) {
       // The  angle is within the range and is set
       this.targetDegrees = degrees;
     }
-    this.targetVelocityDegreesPerSecond = velocityDegreesPerSecond;
 
     // We instantiate a new object here each time because constants can change when being tuned.
     feedforward = new ArmFeedforward(kS, kG, kV, kA);
@@ -144,8 +142,7 @@ public void setAngle(double degrees, double velocityDegreesPerSecond) {
     double deltaDegrees = this.targetDegrees - getAngle();
     this.targetRelativeDegrees = getRelativeAngle() + deltaDegrees;
 
-    double ff =
-        feedforward.calculate(this.targetRelativeDegrees, this.targetVelocityDegreesPerSecond);
+    double ff = feedforward.calculate(this.targetRelativeDegrees, 0);
 
     Logger.recordOutput("Arm/setAngle/setpointDegrees", this.targetRelativeDegrees);
     Logger.recordOutput("Arm/setAngle/ffVolts", ff);
@@ -239,7 +236,7 @@ public void periodic() {
     }
 
     if (null != arm2d) {
-      arm2d.setAngle(inputs.positionDegree + armAngle2dOffset);
+      arm2d.setAngle(inputs.positionDegrees + armAngle2dOffset);
     }
   }
 
@@ -248,7 +245,7 @@ private boolean isLimitHigh() {
     if (isAbsoluteEncoderConnected() == false) {
       return true;
     }
-    if (inputs.positionDegree >= positionDegreeMax) {
+    if (inputs.positionDegrees >= positionDegreeMax) {
       inputs.limitHigh = true;
     } else {
       inputs.limitHigh = false;
@@ -261,7 +258,7 @@ private boolean isLimitLow() {
     if (isAbsoluteEncoderConnected() == false) {
       return true;
     }
-    if (inputs.positionDegree <= positionDegreeMin) {
+    if (inputs.positionDegrees <= positionDegreeMin) {
       inputs.limitLow = true;
 
     } else {
@@ -304,7 +301,7 @@ public void add2dSim(Mechanism2d mech2d) {
             new MechanismLigament2d(
                 "Arm",
                 30,
-                inputs.positionDegree + armAngle2dOffset,
+                inputs.positionDegrees + armAngle2dOffset,
                 6,
                 new Color8Bit(Color.kYellow)));
   }