feat: add an arm hardware method for getting pivot motor velocity

Signed-off-by: Dasun Abeykoon <[email protected]>

Author: dabeycorn

src/main/java/org/frc6423/robot/subsystems/superstructure/arm/ArmIO.java

@@ -25,6 +25,12 @@ public interface ArmIO extends AutoCloseable {
   @Logged(name = "Pivot Motor Setpoint Angle (Radians)")
   public double getSetpointAngleRads();
 
+  /**
+   * @return pivot motor velocity in Radians/Second
+   */
+  @Logged(name = "Pivot Motor Velocity (Radians Per Second)")
+  public double getVelocityRadsPerSec();
+
   /**
    * @return stator current of pivot motor in amps
    */

src/main/java/org/frc6423/robot/subsystems/superstructure/arm/ArmIONone.java

@@ -23,6 +23,11 @@ public double getSetpointAngleRads() {
     return 0.0;
   }
 
+  @Override
+  public double getVelocityRadsPerSec() {
+    return 0.0;
+  }
+
   @Override
   public double getStatorCurrentAmps() {
     return 0.0;

src/main/java/org/frc6423/robot/subsystems/superstructure/arm/ArmIOReal.java

@@ -35,7 +35,7 @@ public class ArmIOReal implements ArmIO {
   private final VoltageOut voltReq = new VoltageOut(0.0).withEnableFOC(true);
   private final MotionMagicTorqueCurrentFOC poseReq = new MotionMagicTorqueCurrentFOC(0.0);
 
-  private final BaseStatusSignal poseSig, currentSig;
+  private final BaseStatusSignal poseSig, velSig, currentSig;
 
   public ArmIOReal() {
     conf.Audio.BeepOnBoot = true;
@@ -72,12 +72,13 @@ public ArmIOReal() {
     tryUntilOk(() -> motor.getConfigurator().apply(conf), 10, MOTOR_ID);
 
     poseSig = motor.getPosition();
+    velSig = motor.getVelocity();
     currentSig = motor.getStatorCurrent();
   }
 
   @Override
   public void periodic() {
-    BaseStatusSignal.refreshAll(poseSig, currentSig);
+    BaseStatusSignal.refreshAll(poseSig, velSig, currentSig);
   }
 
   @Override
@@ -90,6 +91,11 @@ public double getSetpointAngleRads() {
     return poseReq.getPositionMeasure().in(Radians);
   }
 
+  @Override
+  public double getVelocityRadsPerSec() {
+    return velSig.getValueAsDouble();
+  }
+
   @Override
   public double getStatorCurrentAmps() {
     return currentSig.getValueAsDouble();

src/main/java/org/frc6423/robot/subsystems/superstructure/arm/ArmIOSim.java

@@ -64,6 +64,11 @@ public double getSetpointAngleRads() {
     return pivotFeedback.getSetpoint().position;
   }
 
+  @Override
+  public double getVelocityRadsPerSec() {
+    return pivotSim.getVelocityRadPerSec();
+  }
+
   @Override
   public double getStatorCurrentAmps() {
     return pivotSim.getCurrentDrawAmps();