refactor(controller): Work on velocity controllers.

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

@@ -17,18 +17,18 @@ public class PIDFConstants {
   /** Feedback controller position tolerance. */
   public double kPositionTolerance = 0.0;
 
-  /** Feedback controller velocity constraint. */
-  public double kVelocityConstraint = 0.0;
-
-  /** Feedback controller acceleration constraint. */
-  public double kAccelerationConstraint = 0.0;
+  /** Feedback controller velocity tolerance. */
+  public double kVelocityTolerance = 0.0;
 
   /** Feedforward controller static gain. */
   public double kS = 0.0;
 
   /** Feedforward controller velocity gain. */
   public double kV = 0.0;
 
+  /** Feedforward controller acceleration gain. */
+  public double kA = 0.0;
+
   /**
    * Creates a Phoenix PIDF configuration using the PIDF constants.
    *

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

@@ -0,0 +1,57 @@
+package frc.lib.controller;
+
+/** Velocity controller interface. */
+public interface VelocityControllerIO {
+
+    /** Velocity controller constants. */
+    public static class VelocityControllerIOConstants {
+        /** Interpret counterclockwise rotation on the motor face as having positive velocity, if set to true. */
+        public boolean ccwPositive = true;
+
+        /** Use the motor to brake the controlled mechanism on neutral output, if set to true. */
+        public boolean neutralBrake = false;
+
+        /** 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. */
+        public double sensorToMechanismRatio = 1.0;
+    }
+
+    /** Velocity controller values. */
+    public static class VelocityControllerIOValues {
+        /** Velocity in rotations per second. */
+        public double velocityRotationsPerSecond = 0.0;
+
+        /** Acceleration in rotations per second per second. */
+        public double accelerationRotationsPerSecondPerSecond = 0.0;
+
+        /** Motor voltage in volts. */
+        public double motorVolts = 0.0;
+
+        /** Motor current in amps. */
+        public double motorAmps = 0.0;
+    }
+
+    /**
+     * Configures the velocity controller. 
+     * 
+     * @param constants
+    */
+    public void configure(VelocityControllerIOConstants constants);
+
+    /**
+     * Updates the velocity controller's values. 
+     * 
+     * @param values
+    */
+    public void update(VelocityControllerIOValues values);
+
+    /**
+     * Sets the velocity setpoint.
+     * 
+     * @param velocityRotationsPerSecond
+     */
+    public void setSetpoint(double velocityRotationsPerSecond);
+
+}

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

@@ -0,0 +1,74 @@
+package frc.lib.controller;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+
+import frc.lib.CAN;
+import frc.lib.Configurator;
+
+/** Velocity controller using TalonFX. */
+public abstract class VelocityControllerIOTalonFX implements VelocityControllerIO {
+
+    protected final TalonFX motor; 
+
+    protected final StatusSignal<Double> velocity, acceleration, volts, amps;
+
+    /**
+     * Creates a new velocity controller using TalonFX.
+     * 
+     * @param can
+     */
+    protected VelocityControllerIOTalonFX(CAN can) {
+        motor = new TalonFX(can.id(), can.bus());
+
+        velocity = motor.getVelocity();
+        acceleration = motor.getAcceleration();
+        volts = motor.getMotorVoltage();
+        amps = motor.getStatorCurrent();
+    }
+
+    @Override
+    public void configure(VelocityControllerIOConstants constants) {
+        BaseStatusSignal.setUpdateFrequencyForAll(100.0, velocity, acceleration, volts, amps);
+
+        motor.optimizeBusUtilization();
+
+        TalonFXConfiguration config = new TalonFXConfiguration();
+
+        config.MotorOutput.Inverted = constants.ccwPositive ? InvertedValue.CounterClockwise_Positive : InvertedValue.Clockwise_Positive;
+        config.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
+        config.CurrentLimits.StatorCurrentLimit = constants.currentLimitAmps * 2.0;
+        config.CurrentLimits.StatorCurrentLimitEnable = true;
+
+        config.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
+        config.CurrentLimits.SupplyCurrentThreshold = constants.currentLimitAmps * 1.5;
+        config.CurrentLimits.SupplyTimeThreshold = 1;
+        config.CurrentLimits.SupplyCurrentLimitEnable = true;
+
+        config.Feedback.SensorToMechanismRatio = constants.sensorToMechanismRatio;
+
+        Configurator.configureTalonFX(motor.getConfigurator(), config);
+    }
+
+    @Override
+    public void update(VelocityControllerIOValues values) {
+        BaseStatusSignal.refreshAll(velocity, acceleration, volts, amps);
+
+        values.velocityRotationsPerSecond = velocity.getValue();
+        values.accelerationRotationsPerSecondPerSecond = acceleration.getValue();
+        values.motorVolts = volts.getValue();
+        values.motorAmps = volts.getValue();
+    }
+
+    @Override
+    public abstract void setSetpoint(double velocityRotationsPerSecond);
+
+}

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

@@ -0,0 +1,47 @@
+package frc.lib.controller;
+
+import com.ctre.phoenix6.controls.VoltageOut;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import frc.lib.CAN;
+import frc.lib.PIDFConstants;
+
+/** Velocity controller using TalonFX and external PIDF. */
+public class VelocityControllerIOTalonFXPIDF extends VelocityControllerIOTalonFX {
+
+    private final SimpleMotorFeedforward feedforward;
+
+    private final PIDController feedback;
+
+    private final VoltageOut voltage;
+
+    /**
+     * Creates a new velocity controller using TalonFX and external PIDF.
+     * 
+     * @param can
+     * @param pidf
+     * @param enableFOC
+     */
+    public VelocityControllerIOTalonFXPIDF(CAN can, PIDFConstants pidf, boolean enableFOC) {
+        super(can);
+
+        feedforward = new SimpleMotorFeedforward(pidf.kS, pidf.kV, pidf.kA);
+
+        feedback = new PIDController(pidf.kP, pidf.kI, pidf.kD);
+
+        voltage = new VoltageOut(0.0).withEnableFOC(enableFOC);
+    }
+
+    @Override
+    public void setSetpoint(double velocityRotationsPerSecond) {
+        double feedforwardVolts = feedforward.calculate(velocityRotationsPerSecond);
+
+        double measuredVelocityRotationsPerSecond = velocity.getValue();
+
+        double feedbackVolts = feedback.calculate(measuredVelocityRotationsPerSecond, velocityRotationsPerSecond);
+
+        motor.setControl(voltage.withOutput(feedforwardVolts + feedbackVolts));
+    }
+
+}