refactor(swerve): Use generic position controller for azimuth.

src/main/java/frc/lib/config/FeedbackControllerConfig.java

@@ -14,13 +14,14 @@ public class FeedbackControllerConfig {
   /** Feedback controller derivative gain. */
   private double kD = 0.0;
 
+  /** Feedback controller continuous input. */
+  private boolean continuousInput = false;
+
   /** Feedback controller position tolerance. */
-  private double kPositionTolerance = 0.0;
+  private double positionTolerance = 0.0;
 
   /** Feedback controller velocity tolerance. */
-  private double kVelocityTolerance = 0.0;
-
-  // TODO Add velocity / acceleration constraints, profiled PID controller?
+  private double velocityTolerance = 0.0;
 
   /**
    * Modifies this controller config's proportional gain.
@@ -56,38 +57,36 @@ public FeedbackControllerConfig withDerivativeGain(double kD) {
   }
 
   /**
-   * Modifies this controller config's position tolerance.
+   * Modifies this controller config's continuous input.
    *
-   * @param kPositionTolerance the position tolerance.
+   * @param continuousInput the continuous input.
    * @return this controller config.
    */
-  public FeedbackControllerConfig withPositionTolerance(double kPositionTolerance) {
-    this.kPositionTolerance = kPositionTolerance;
+  public FeedbackControllerConfig withContinuousInput(boolean continuousInput) {
+    this.continuousInput = continuousInput;
     return this;
   }
 
   /**
-   * Modifies this controller config's velocity tolerance.
+   * Modifies this controller config's position tolerance.
    *
-   * @param kVelocityTolerance the velocity tolerance.
+   * @param positionTolerance the position tolerance.
    * @return this controller config.
    */
-  public FeedbackControllerConfig withVelocityTolerance(double kVelocityTolerance) {
-    this.kVelocityTolerance = kVelocityTolerance;
+  public FeedbackControllerConfig withPositionTolerance(double positionTolerance) {
+    this.positionTolerance = positionTolerance;
     return this;
   }
 
   /**
-   * Creates a new PID controller using this feedback config.
+   * Modifies this controller config's velocity tolerance.
    *
-   * @return a new PID controller using this feedback config.
+   * @param velocityTolerance the velocity tolerance.
+   * @return this controller config.
    */
-  public PIDController createPIDController() {
-    PIDController pidController = new PIDController(kP, kI, kD);
-
-    pidController.setTolerance(kPositionTolerance, kVelocityTolerance);
-
-    return pidController;
+  public FeedbackControllerConfig withVelocityTolerance(double velocityTolerance) {
+    this.velocityTolerance = velocityTolerance;
+    return this;
   }
 
   /**
@@ -117,21 +116,47 @@ public double kD() {
     return kD;
   }
 
+  /**
+   * Returns the feedback controller continuous input.
+   *
+   * @return the feedback controller continuous input.
+   */
+  public boolean continuousInput() {
+    return continuousInput;
+  }
+
   /**
    * Returns the feedback controller position tolerance.
    *
    * @return the feedback controller position tolerance.
    */
-  public double kPositionTolerance() {
-    return kPositionTolerance;
+  public double positionTolerance() {
+    return positionTolerance;
   }
 
   /**
    * Returns the feedback controller velocity tolerance.
    *
    * @return the feedback controller velocity tolerance.
    */
-  public double kVelocityTolerance() {
-    return kVelocityTolerance;
+  public double velocityTolerance() {
+    return velocityTolerance;
+  }
+
+  /**
+   * Creates a new PID controller using this feedback config.
+   *
+   * @return a new PID controller using this feedback config.
+   */
+  public PIDController createPIDController() {
+    final PIDController pidController = new PIDController(kP, kI, kD);
+
+    pidController.setTolerance(positionTolerance, velocityTolerance);
+
+    if (continuousInput) {
+      pidController.enableContinuousInput(-0.5, 0.5);
+    }
+
+    return pidController;
   }
 }

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

@@ -0,0 +1,106 @@
+package frc.lib.controller;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+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 edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import frc.lib.CAN;
+import frc.lib.config.Configurator;
+import frc.lib.config.MechanismConfig;
+
+/** Creates a new position controller using a steer TalonFX and azimuth CANcoder. */
+public class PositionControllerIOTalonFXSteer implements PositionControllerIO {
+
+  private final MechanismConfig config;
+
+  private final TalonFX steer;
+
+  private final CANcoder azimuth;
+
+  private final StatusSignal<Double> position, velocity, acceleration, volts, amps;
+
+  private final SimpleMotorFeedforward feedforward;
+
+  private final PIDController feedback;
+
+  private final VoltageOut voltage;
+
+  public PositionControllerIOTalonFXSteer(
+      CAN steerCAN, CAN encoderCAN, MechanismConfig config, boolean enableFOC) {
+    this.config = config;
+
+    steer = new TalonFX(steerCAN.id(), steerCAN.bus());
+
+    azimuth = new CANcoder(encoderCAN.id(), encoderCAN.bus());
+
+    // TODO Use steer position after seeding with azimuth
+    position = azimuth.getAbsolutePosition();
+
+    velocity = steer.getVelocity();
+    acceleration = steer.getAcceleration();
+
+    volts = steer.getMotorVoltage();
+    amps = steer.getStatorCurrent();
+
+    feedforward = config.feedforwardControllerConfig().createSimpleMotorFeedforward();
+
+    feedback = config.feedbackControllerConfig().createPIDController();
+
+    voltage = new VoltageOut(0.0).withEnableFOC(enableFOC);
+  }
+
+  @Override
+  public void configure() {
+    BaseStatusSignal.setUpdateFrequencyForAll(100.0, position, velocity, acceleration, volts, amps);
+
+    ParentDevice.optimizeBusUtilizationForAll(steer, azimuth);
+
+    Configurator.configureTalonFX(
+        steer.getConfigurator(), config.motorConfig().createTalonFXConfig());
+
+    Configurator.configureCANcoder(
+        azimuth.getConfigurator(), config.absoluteEncoderConfig().createCANcoderConfig());
+  }
+
+  @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) {}
+
+  @Override
+  public void setSetpoint(double positionRotations, double velocityRotationsPerSecond) {
+    double measuredPositionRotations = position.getValue();
+
+    // double feedforwardVolts = feedforward.calculate(velocityRotationsPerSecond);
+    double feedforwardVolts = calculateFeedforward(measuredPositionRotations, positionRotations);
+
+    double feedbackVolts = feedback.calculate(measuredPositionRotations, positionRotations);
+
+    steer.setControl(voltage.withOutput(feedforwardVolts + feedbackVolts));
+  }
+
+  private double calculateFeedforward(double measurementRotations, double setpointRotations) {
+    if (feedback.atSetpoint() == false) {
+      if (measurementRotations > setpointRotations) {
+        return feedforward.ks;
+      } else {
+        return -feedforward.ks;
+      }
+    }
+
+    return 0.0;
+  }
+}