refactor(swerve): Use generic velocity controller.

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

@@ -1,6 +1,5 @@
 package frc.lib.controller;
 
-import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import frc.lib.Telemetry;
@@ -37,13 +36,10 @@ public static void addToShuffleboard(
       ShuffleboardTab tab, String name, PositionControllerIOValues values) {
     ShuffleboardLayout positionController = Telemetry.addColumn(tab, name);
 
+    positionController.addDouble("Position (rot)", () -> values.positionRotations);
+    positionController.addDouble("Velocity (rps)", () -> values.velocityRotationsPerSecond);
     positionController.addDouble(
-        "Position (deg)", () -> Units.rotationsToDegrees(values.positionRotations));
-    positionController.addDouble(
-        "Velocity (dps)", () -> Units.rotationsToDegrees(values.velocityRotationsPerSecond));
-    positionController.addDouble(
-        "Acceleration (dpsps)",
-        () -> Units.rotationsToDegrees(values.accelerationRotationsPerSecondPerSecond));
+        "Acceleration (rpsps)", () -> values.accelerationRotationsPerSecondPerSecond);
     positionController.addDouble("Voltage (V)", () -> values.motorVolts);
     positionController.addDouble("Current (A)", () -> values.motorAmps);
   }
@@ -63,7 +59,7 @@ public static void addToShuffleboard(
   public void update(PositionControllerIOValues values);
 
   /**
-   * Sets the mechanism position.
+   * Sets the position controller position.
    *
    * @param positionRotations
    */

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

@@ -1,6 +1,5 @@
 package frc.lib.controller;
 
-import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import frc.lib.Telemetry;
@@ -10,6 +9,9 @@ public interface VelocityControllerIO {
 
   /** Velocity controller values. */
   public static class VelocityControllerIOValues {
+    /** Position in rotations. */
+    public double positionRotations = 0.0;
+
     /** Velocity in rotations per second. */
     public double velocityRotationsPerSecond = 0.0;
 
@@ -34,17 +36,10 @@ public static void addToShuffleboard(
       ShuffleboardTab tab, String name, VelocityControllerIOValues values) {
     ShuffleboardLayout velocityController = Telemetry.addColumn(tab, name);
 
-    velocityController.addDouble(
-        "Velocity (dps)", () -> Units.rotationsToDegrees(values.velocityRotationsPerSecond));
-    velocityController.addDouble(
-        "Acceleration (dpsps)",
-        () -> Units.rotationsToDegrees(values.accelerationRotationsPerSecondPerSecond));
+    velocityController.addDouble("Position (rot)", () -> values.positionRotations);
     velocityController.addDouble("Velocity (rps)", () -> values.velocityRotationsPerSecond);
     velocityController.addDouble(
         "Acceleration (rpsps)", () -> values.accelerationRotationsPerSecondPerSecond);
-    velocityController.addDouble("Velocity (rpm)", () -> values.velocityRotationsPerSecond * 60);
-    velocityController.addDouble(
-        "Acceleration (rpmpm)", () -> values.accelerationRotationsPerSecondPerSecond * 60);
     velocityController.addDouble("Voltage (V)", () -> values.motorVolts);
     velocityController.addDouble("Current (A)", () -> values.motorAmps);
   }
@@ -63,6 +58,13 @@ public static void addToShuffleboard(
    */
   public void update(VelocityControllerIOValues values);
 
+  /**
+   * Sets the velocity controller position.
+   *
+   * @param positionRotations
+   */
+  public void setPosition(double positionRotations);
+
   /**
    * Sets the velocity setpoint.
    *

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

@@ -1,18 +1,30 @@
 package frc.lib.controller;
 
+import frc.robot.RobotConstants;
+
 /** Simulated velocity controller. */
 public class VelocityControllerIOSim implements VelocityControllerIO {
 
+  private double positionRotations = 0.0;
+
   private double velocityRotationsPerSecond = 0.0;
 
   @Override
   public void configure() {}
 
   @Override
   public void update(VelocityControllerIOValues values) {
+    positionRotations += velocityRotationsPerSecond * RobotConstants.PERIODIC_DURATION;
+
+    values.positionRotations = positionRotations;
     values.velocityRotationsPerSecond = velocityRotationsPerSecond;
   }
 
+  @Override
+  public void setPosition(double positionRotations) {
+    this.positionRotations = positionRotations;
+  }
+
   @Override
   public void setSetpoint(double velocityRotationsPerSecond) {
     this.velocityRotationsPerSecond = velocityRotationsPerSecond;

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

@@ -15,7 +15,7 @@ public abstract class VelocityControllerIOTalonFX implements VelocityControllerI
 
   protected final TalonFX motor;
 
-  protected final StatusSignal<Double> velocity, acceleration, volts, amps;
+  protected final StatusSignal<Double> position, velocity, acceleration, volts, amps;
 
   /**
    * Creates a new velocity controller using TalonFX.
@@ -27,6 +27,7 @@ protected VelocityControllerIOTalonFX(CAN can, MechanismConfig config) {
 
     motor = new TalonFX(can.id(), can.bus());
 
+    position = motor.getPosition();
     velocity = motor.getVelocity();
     acceleration = motor.getAcceleration();
     volts = motor.getMotorVoltage();
@@ -35,7 +36,7 @@ protected VelocityControllerIOTalonFX(CAN can, MechanismConfig config) {
 
   @Override
   public void configure() {
-    BaseStatusSignal.setUpdateFrequencyForAll(100.0, velocity, acceleration, volts, amps);
+    BaseStatusSignal.setUpdateFrequencyForAll(100.0, position, velocity, acceleration, volts, amps);
 
     ParentDevice.optimizeBusUtilizationForAll(motor);
 
@@ -45,14 +46,20 @@ public void configure() {
 
   @Override
   public void update(VelocityControllerIOValues values) {
-    BaseStatusSignal.refreshAll(velocity, acceleration, volts, amps);
+    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) {
+    motor.setPosition(positionRotations);
+  }
+
   @Override
   public abstract void setSetpoint(double velocityRotationsPerSecond);
 }

src/main/java/frc/robot/Robot.java

@@ -3,24 +3,15 @@
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.button.Trigger;
-import frc.robot.swerve.Swerve;
 
 public class Robot extends TimedRobot {
   private Command autonomousCommand;
 
   private RobotContainer robotContainer;
-  private Swerve swerve;
 
   @Override
   public void robotInit() {
     robotContainer = RobotContainer.getInstance();
-    swerve = Swerve.getInstance();
-
-    new Trigger(this::isDisabled)
-        .debounce(RobotConstants.DISABLE_COAST_DELAY)
-        .onTrue(Commands.runOnce(() -> swerve.setBrake(false), swerve).ignoringDisable(true));
   }
 
   @Override
@@ -39,8 +30,6 @@ public void disabledExit() {}
 
   @Override
   public void autonomousInit() {
-    swerve.setBrake(true);
-
     autonomousCommand = robotContainer.getAutonomousCommand();
 
     if (autonomousCommand != null) {
@@ -56,8 +45,6 @@ public void autonomousExit() {}
 
   @Override
   public void teleopInit() {
-    swerve.setBrake(true);
-
     if (autonomousCommand != null) {
       autonomousCommand.cancel();
     }

src/main/java/frc/robot/RobotConstants.java

@@ -16,12 +16,6 @@ public class RobotConstants {
   /** Voltage of the robot's battery, */
   public static final double BATTERY_VOLTAGE = 12.0;
 
-  /**
-   * Duration between the robot being disabled and the swerve subsystem is allowed to coast in
-   * seconds.
-   */
-  public static final double DISABLE_COAST_DELAY = 3.0;
-
   /** Distance from the frame perimeter to the origin in meters. */
   public static final double FRAME_PERIMETER_TO_ORIGIN_DISTANCE = Units.inchesToMeters(14);