refactor(arm): Use position controller for arm.

simgui.json

@@ -189,7 +189,7 @@
             0.0,
             0.8500000238418579
           ],
-          "height": 0,
+          "height": 338,
           "series": [
             {
               "color": [

src/main/java/frc/lib/PIDFConstants.java

@@ -23,6 +23,9 @@ public class PIDFConstants {
   /** Feedforward controller static gain. */
   public double kS = 0.0;
 
+  /** Feedforward controller gravity gain. */
+  public double kG = 0.0;
+
   /** Feedforward controller velocity gain. */
   public double kV = 0.0;
 
@@ -41,7 +44,9 @@ public Slot0Configs asSlot0Configs() {
     slot0Configs.kI = kI;
     slot0Configs.kD = kD;
     slot0Configs.kS = kS;
+    slot0Configs.kG = kG;
     slot0Configs.kV = kV;
+    slot0Configs.kA = kA;
 
     return slot0Configs;
   }

src/main/java/frc/lib/controller/PositionControllerIO.java

@@ -18,8 +18,11 @@ public static class PositionControllerIOConstants {
         /** Maximum amount of current, in amps, to provide to the motor. */
         public double currentLimitAmps = 40.0;
 
-        /** Ratio between the velocity sensor and the controlled mechanism. */
+        /** Ratio between the position sensor and the controlled mechanism. */
         public double sensorToMechanismRatio = 1.0;
+
+        /** Offset between absolute encoder reading and controlled mechanism position in rotations. */
+        public double absoluteEncoderOffsetRotations = 0.0;
     }
 
     /** Position controller values. */
@@ -71,6 +74,13 @@ public static void addToShuffleboard(ShuffleboardTab tab, String name, PositionC
      */
     public void update(PositionControllerIOValues values);
 
+    /**
+     * Sets the mechanism position.
+     * 
+     * @param positionRotations
+     */
+    public void setPosition(double positionRotations);
+
     /**
      * Sets the position setpoint.
      * 

src/main/java/frc/lib/controller/PositionControllerIOSim.java

@@ -16,6 +16,11 @@ public void update(PositionControllerIOValues values) {
         values.velocityRotationsPerSecond = velocityRotationsPerSecond;
     }
 
+    @Override
+    public void setPosition(double positionRotations) {
+        this.positionRotations = positionRotations;
+    }
+
     @Override
     public void setSetpoint(double positionRotations, double velocityRotationsPerSecond) {
         this.positionRotations = positionRotations;

src/main/java/frc/lib/controller/PositionControllerIOTalonFX2.java

@@ -0,0 +1,134 @@
+package frc.lib.controller;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.Follower;
+import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.hardware.ParentDevice;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.ctre.phoenix6.signals.SensorDirectionValue;
+
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.util.Units;
+import frc.lib.CAN;
+import frc.lib.Configurator;
+import frc.lib.PIDFConstants;
+
+/** Position controller using two TalonFXs and a CANcoder and an external PIDF for an arm.  */
+public class PositionControllerIOTalonFX2 implements PositionControllerIO {
+
+    private final TalonFX leaderMotor, followerMotor;
+
+    private final CANcoder encoder;
+
+    private final StatusSignal<Double> position, velocity, acceleration, volts, amps;
+
+    private final ArmFeedforward feedforward;
+
+    private final PIDController feedback;
+
+    private final VoltageOut voltage;
+
+    /**
+     * Creates a new position controller using two TalonFXs and a CANcoder and an external PIDF for an arm.
+     * 
+     * @param leaderCAN
+     * @param followerCAN
+     * @param encoderCAN
+     * @param pidf
+     * @param enableFOC
+     * @param invertFollower
+     */
+    public PositionControllerIOTalonFX2(CAN leaderCAN, CAN followerCAN, CAN encoderCAN, PIDFConstants pidf, boolean enableFOC, boolean invertFollower) {
+        leaderMotor = new TalonFX(leaderCAN.id(), leaderCAN.bus());
+        followerMotor = new TalonFX(followerCAN.id(), followerCAN.bus());
+
+        encoder = new CANcoder(encoderCAN.id(), encoderCAN.bus());
+
+        position = encoder.getAbsolutePosition();
+
+        velocity = leaderMotor.getVelocity();
+        acceleration = leaderMotor.getAcceleration();
+
+        volts = leaderMotor.getMotorVoltage();
+        amps = leaderMotor.getStatorCurrent();
+
+        feedforward = new ArmFeedforward(pidf.kS, pidf.kG, pidf.kV, pidf.kA);
+
+        feedback = new PIDController(pidf.kP, pidf.kI, pidf.kD);
+
+        followerMotor.setControl(new Follower(leaderMotor.getDeviceID(), invertFollower));
+
+        voltage = new VoltageOut(0.0).withEnableFOC(enableFOC);
+    }
+
+    @Override
+    public void configure(PositionControllerIOConstants constants) {
+        BaseStatusSignal.setUpdateFrequencyForAll(100.0, position, velocity, acceleration, volts, amps);
+
+        ParentDevice.optimizeBusUtilizationForAll(leaderMotor, followerMotor, encoder);
+
+        TalonFXConfiguration motorConfig = new TalonFXConfiguration();
+
+        motorConfig.MotorOutput.Inverted = constants.ccwPositive ? InvertedValue.CounterClockwise_Positive : InvertedValue.Clockwise_Positive;
+        motorConfig.MotorOutput.NeutralMode = constants.neutralBrake ? NeutralModeValue.Brake : NeutralModeValue.Coast;
+
+        // Stator current is a measure of the current inside of the motor and is typically higher than supply (breaker) current
+        motorConfig.CurrentLimits.StatorCurrentLimit = constants.currentLimitAmps * 2.0;
+        motorConfig.CurrentLimits.StatorCurrentLimitEnable = true;
+
+        motorConfig.CurrentLimits.SupplyCurrentLimit = constants.currentLimitAmps;
+        // Allow higher current spikes (150%) for a brief duration (one second)
+        // REV 40A auto-resetting breakers typically trip when current exceeds 300% for one second
+        motorConfig.CurrentLimits.SupplyCurrentThreshold = constants.currentLimitAmps * 1.5;
+        motorConfig.CurrentLimits.SupplyTimeThreshold = 1;
+        motorConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
+
+        motorConfig.Feedback.SensorToMechanismRatio = constants.sensorToMechanismRatio;
+
+        Configurator.configureTalonFX(leaderMotor.getConfigurator(), motorConfig);
+        Configurator.configureTalonFX(followerMotor.getConfigurator(), motorConfig);
+
+        CANcoderConfiguration encoderConfig = new CANcoderConfiguration();
+
+        encoderConfig.MagnetSensor.MagnetOffset = constants.absoluteEncoderOffsetRotations;
+        encoderConfig.MagnetSensor.SensorDirection = constants.ccwPositive ? SensorDirectionValue.CounterClockwise_Positive : SensorDirectionValue.Clockwise_Positive;
+
+        Configurator.configureCANcoder(encoder.getConfigurator(), encoderConfig);
+    }
+
+    @Override
+    public void update(PositionControllerIOValues values) {
+        BaseStatusSignal.refreshAll(position, velocity, acceleration, volts, amps);
+
+        values.positionRotations = position.getValue();
+        values.velocityRotationsPerSecond = velocity.getValue();
+        values.accelerationRotationsPerSecondPerSecond = acceleration.getValue();
+        values.motorVolts = volts.getValue();
+        values.motorAmps = amps.getValue();
+    }
+
+    @Override
+    public void setPosition(double positionRotations) {
+        leaderMotor.setPosition(positionRotations);
+        followerMotor.setPosition(positionRotations);
+    }
+
+    @Override
+    public void setSetpoint(double positionRotations, double velocityRotationsPerSecond) {
+        double measuredPositionRotations = position.getValue();
+
+        double feedforwardVolts = feedforward.calculate(Units.rotationsToRadians(measuredPositionRotations), velocityRotationsPerSecond);
+
+        double feedbackVolts = feedback.calculate(measuredPositionRotations, positionRotations);
+
+        leaderMotor.setControl(voltage.withOutput(feedforwardVolts + feedbackVolts));
+    }
+
+}

src/main/java/frc/lib/controller/VelocityControllerIOTalonFX.java

@@ -3,6 +3,7 @@
 import com.ctre.phoenix6.BaseStatusSignal;
 import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.hardware.ParentDevice;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
@@ -35,7 +36,7 @@ protected VelocityControllerIOTalonFX(CAN can) {
     public void configure(VelocityControllerIOConstants constants) {
         BaseStatusSignal.setUpdateFrequencyForAll(100.0, velocity, acceleration, volts, amps);
 
-        motor.optimizeBusUtilization();
+        ParentDevice.optimizeBusUtilizationForAll(motor);
 
         TalonFXConfiguration config = new TalonFXConfiguration();
 
@@ -65,7 +66,7 @@ public void update(VelocityControllerIOValues values) {
         values.velocityRotationsPerSecond = velocity.getValue();
         values.accelerationRotationsPerSecondPerSecond = acceleration.getValue();
         values.motorVolts = volts.getValue();
-        values.motorAmps = volts.getValue();
+        values.motorAmps = amps.getValue();
     }
 
     @Override

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

@@ -2,30 +2,29 @@
 
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import frc.lib.Subsystem;
-import frc.lib.Telemetry;
-import frc.robot.arm.ShoulderMotorIO.ShoulderMotorIOValues;
+import frc.lib.controller.PositionControllerIO;
+import frc.lib.controller.PositionControllerIO.PositionControllerIOValues;
+import frc.robot.arm.ArmConstants.ShoulderConstants;
 
 /** Subsystem class for the arm subsystem. */
 public class Arm extends Subsystem {
 
   /** Instance variable for the arm subsystem singleton. */
   private static Arm instance = null;
 
-  /** Shoulder motor. */
-  private final ShoulderMotorIO shoulderMotor;
+  /** Shoulder. */
+  private final PositionControllerIO shoulder;
 
-  /** Shoulder motor values. */
-  private final ShoulderMotorIOValues shoulderMotorValues = new ShoulderMotorIOValues();
+  /** Shoulder values. */
+  private final PositionControllerIOValues shoulderValues;
 
   /** Creates a new instance of the arm subsystem. */
   private Arm() {
-    shoulderMotor = ArmFactory.createShoulderMotor();
-
-    shoulderMotor.configure();
+    shoulder = ArmFactory.createShoulder();
+    shoulderValues = new PositionControllerIOValues();
+    shoulder.configure(ShoulderConstants.CONTROLLER_CONSTANTS);
   }
 
   /**
@@ -43,44 +42,28 @@ public static Arm getInstance() {
 
   @Override
   public void periodic() {
-    shoulderMotor.update(shoulderMotorValues);
+    shoulder.update(shoulderValues);
   }
 
   @Override
   public void addToShuffleboard(ShuffleboardTab tab) {
-    ShuffleboardLayout position = Telemetry.addColumn(tab, "Position");
-
-    position.addDouble(
-        "Shoulder Position (deg)",
-        () -> Units.rotationsToDegrees(shoulderMotorValues.positionRotations));
-
-    ShuffleboardLayout voltages = Telemetry.addColumn(tab, "Voltages");
-
-    voltages.addDouble("Shoulder Voltage (V)", () -> shoulderMotorValues.inputVoltage);
-
-    ShuffleboardLayout derivatives = Telemetry.addColumn(tab, "Derivatives");
-
-    derivatives.addDouble(
-        "Shoulder Velocity (rps)", () -> shoulderMotorValues.velocityRotationsPerSecond);
-    derivatives.addDouble(
-        "Shoulder Acceleration (rpsps)",
-        () -> shoulderMotorValues.accelerationRotationsPerSecondPerSecond);
+    PositionControllerIO.addToShuffleboard(tab, "Shoulder", shoulderValues);
   }
 
   public State getMeasuredShoulderState() {
     return new TrapezoidProfile.State(
-        shoulderMotorValues.positionRotations, shoulderMotorValues.velocityRotationsPerSecond);
+        shoulderValues.positionRotations, shoulderValues.velocityRotationsPerSecond);
   }
 
   public void setShoulderPosition(double positionRotations) {
-    shoulderMotor.setPosition(positionRotations);
+    shoulder.setPosition(positionRotations);
   }
 
   public void setSetpoint(double positionRotations, double velocityRotationsPerSecond) {
-    shoulderMotor.setSetpoint(positionRotations, velocityRotationsPerSecond);
+    shoulder.setSetpoint(positionRotations, velocityRotationsPerSecond);
   }
 
   public void setVoltage(double volts) {
-    shoulderMotor.setVoltage(volts);
+    // TODO
   }
 }

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

@@ -1,4 +1,41 @@
 package frc.robot.arm;
 
+import edu.wpi.first.math.util.Units;
+import frc.lib.CAN;
+import frc.lib.PIDFConstants;
+import frc.lib.controller.PositionControllerIO.PositionControllerIOConstants;
+
 /** Constants for the arm subsystem. */
-public class ArmConstants {}
+public class ArmConstants {
+
+    /** Constants for the shoulder. */
+    public static class ShoulderConstants {
+        /** Shoulder's leader CAN. */
+        public static final CAN LEADER_CAN = new CAN(48);
+
+        /** Shoulder's follower CAN. */
+        public static final CAN FOLLOWER_CAN = new CAN(46);
+
+        /** Shoulder's encoder CAN. */
+        public static final CAN ENCODER_CAN = new CAN(52);
+
+        /** Shoulder's PIDF constants. */
+        public static final PIDFConstants PIDF = new PIDFConstants();
+        static {
+            PIDF.kS = 0.14;
+            PIDF.kG = 0.45;
+            PIDF.kV = 4.0;
+            PIDF.kP = 20.0;
+        }
+
+        /** Shoulder's controller constants. */
+        public static final PositionControllerIOConstants CONTROLLER_CONSTANTS = new PositionControllerIOConstants();
+        static {
+            CONTROLLER_CONSTANTS.ccwPositive = false;
+            CONTROLLER_CONSTANTS.neutralBrake = true;
+            CONTROLLER_CONSTANTS.sensorToMechanismRatio = 39.771428571;
+            CONTROLLER_CONSTANTS.absoluteEncoderOffsetRotations = Units.degreesToRotations(-146.77) + Units.degreesToRotations(-27.07);
+        }
+    }
+
+}

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

@@ -1,8 +1,12 @@
 package frc.robot.arm;
 
+import frc.lib.controller.PositionControllerIO;
+import frc.lib.controller.PositionControllerIOSim;
+import frc.lib.controller.PositionControllerIOTalonFX2;
 import frc.robot.Robot;
 import frc.robot.RobotConstants;
 import frc.robot.RobotConstants.Subsystem;
+import frc.robot.arm.ArmConstants.ShoulderConstants;
 
 /** Helper class for creating hardware for the arm subsystem. */
 public class ArmFactory {
@@ -12,10 +16,11 @@ public class ArmFactory {
    *
    * @return a shoulder motor.
    */
-  public static ShoulderMotorIO createShoulderMotor() {
-    if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.ARM))
-      return new ShoulderMotorIOTalonFX();
+  public static PositionControllerIO createShoulder() {
+    if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.ARM)) {
+      return new PositionControllerIOTalonFX2(ShoulderConstants.LEADER_CAN, ShoulderConstants.FOLLOWER_CAN, ShoulderConstants.ENCODER_CAN, ShoulderConstants.PIDF, false, true);
+    }
 
-    return new ShoulderMotorIOSim();
+    return new PositionControllerIOSim();
   }
 }