refactor(shooter): Use velocity controller for shooter.

simgui.json

@@ -99,9 +99,6 @@
           "Measurement": {
             "open": true
           },
-          "Setpoint": {
-            "open": true
-          },
           "open": true,
           "setpoint": {
             "open": true

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

@@ -1,5 +1,9 @@
 package frc.lib.controller;
 
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+import frc.lib.Telemetry;
+
 /** Velocity controller interface. */
 public interface VelocityControllerIO {
 
@@ -33,6 +37,15 @@ public static class VelocityControllerIOValues {
         public double motorAmps = 0.0;
     }
 
+    public static void addToShuffleboard(ShuffleboardTab tab, String name, VelocityControllerIOValues values) {
+        ShuffleboardLayout velocityController = Telemetry.addColumn(tab, name);
+
+        velocityController.addDouble("Velocity (rps)", () -> values.velocityRotationsPerSecond);
+        velocityController.addDouble("Acceleration (rpsps)", () -> values.accelerationRotationsPerSecondPerSecond);
+        velocityController.addDouble("Voltage (V)", () -> values.motorVolts);
+        velocityController.addDouble("Current (A)", () -> values.motorAmps);
+    }
+
     /**
      * Configures the velocity controller. 
      * 

src/main/java/frc/robot/intake/Intake.java

@@ -1,9 +1,7 @@
 package frc.robot.intake;
 
-import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import frc.lib.Subsystem;
-import frc.lib.Telemetry;
 import frc.lib.controller.VelocityControllerIO;
 import frc.lib.controller.VelocityControllerIO.VelocityControllerIOValues;
 import frc.robot.intake.IntakeConstants.BackRollerConstants;
@@ -53,16 +51,8 @@ public void periodic() {
 
   @Override
   public void addToShuffleboard(ShuffleboardTab tab) {
-    initializeRollerShuffleboard(tab, "Front Roller", frontRollerValues);
-    initializeRollerShuffleboard(tab, "Back Roller", backRollerValues);
-  }
-
-  public void initializeRollerShuffleboard(ShuffleboardTab tab, String name, VelocityControllerIOValues values) {
-    ShuffleboardLayout roller = Telemetry.addColumn(tab, name);
-
-    roller.addDouble("Velocity (rps)", () -> values.velocityRotationsPerSecond);
-    roller.addDouble("Voltage (V)", () -> values.motorVolts);
-    roller.addDouble("Current (A)", () -> values.motorAmps);
+    VelocityControllerIO.addToShuffleboard(tab, "Front Roller", frontRollerValues);
+    VelocityControllerIO.addToShuffleboard(tab, "Back Roller", backRollerValues);
   }
 
   public double getRollerVelocity() {

src/main/java/frc/robot/shooter/Shooter.java

@@ -1,37 +1,39 @@
 package frc.robot.shooter;
 
-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.shooter.FlywheelMotorIO.FlywheelMotorIOValues;
-import frc.robot.shooter.SerializerMotorIO.SerializerMotorIOValues;
+import frc.lib.controller.VelocityControllerIO;
+import frc.lib.controller.VelocityControllerIO.VelocityControllerIOValues;
+import frc.robot.shooter.ShooterConstants.FlywheelConstants;
+import frc.robot.shooter.ShooterConstants.SerializerConstants;
 
 /** Subsystem class for the shooter subsystem. */
 public class Shooter extends Subsystem {
 
   /** Instance variable for the shooter subsystem singleton. */
   private static Shooter instance = null;
 
-  /** Serializer motor. */
-  private final SerializerMotorIO serializerMotor;
+  /** Serializer. */
+  private final VelocityControllerIO serializer;
 
-  /** Serializer motor values. */
-  private final SerializerMotorIOValues serializerMotorValues = new SerializerMotorIOValues();
+  /** Serializer values. */
+  private final VelocityControllerIOValues serializerValues;
 
-  /** Flywheel motor. */
-  private final FlywheelMotorIO flywheelMotor;
+  /** Flywheel. */
+  private final VelocityControllerIO flywheel;
 
-  /** Flywheel motor values. */
-  private final FlywheelMotorIOValues flywheelMotorValues = new FlywheelMotorIOValues();
+  /** Flywheel values. */
+  private final VelocityControllerIOValues flywheelValues;
 
   /** Creates a new instance of the shooter subsystem. */
   private Shooter() {
-    serializerMotor = ShooterFactory.createSerializerMotor();
-    flywheelMotor = ShooterFactory.createFlywheelMotor();
+    serializer = ShooterFactory.createSerializerMotor();
+    serializerValues = new VelocityControllerIOValues();
+    serializer.configure(SerializerConstants.CONTROLLER_CONSTANTS);
 
-    serializerMotor.configure();
-    flywheelMotor.configure();
+    flywheel = ShooterFactory.createFlywheelMotor();
+    flywheelValues = new VelocityControllerIOValues();
+    flywheel.configure(FlywheelConstants.CONTROLLER_CONSTANTS);
   }
 
   /**
@@ -49,40 +51,27 @@ public static Shooter getInstance() {
 
   @Override
   public void periodic() {
-    serializerMotor.update(serializerMotorValues);
-    flywheelMotor.update(flywheelMotorValues);
+    serializer.update(serializerValues);
+    flywheel.update(flywheelValues);
   }
 
   @Override
   public void addToShuffleboard(ShuffleboardTab tab) {
-    ShuffleboardLayout serializer = Telemetry.addColumn(tab, "Serializer");
-
-    serializer.addDouble(
-        "Serializer Speed (rps)", () -> serializerMotorValues.velocityRotationsPerSecond);
-    serializer.addDouble("Serializer Current (A)", () -> serializerMotorValues.currentAmps);
-
-    ShuffleboardLayout flywheel = Telemetry.addColumn(tab, "Flywheel");
-
-    flywheel.addDouble(
-        "Flywheel Speed (rps)", () -> flywheelMotorValues.velocityRotationsPerSecond);
-    flywheel.addDouble("Flywheel Current (A)", () -> flywheelMotorValues.currentAmps);
+    VelocityControllerIO.addToShuffleboard(tab, "Serializer", serializerValues);
+    VelocityControllerIO.addToShuffleboard(tab, "Flywheel", flywheelValues);
   }
 
   public double getFlywheelVelocity() {
-    flywheelMotor.update(flywheelMotorValues);
-
-    return flywheelMotorValues.velocityRotationsPerSecond;
+    return flywheelValues.velocityRotationsPerSecond;
   }
 
   public double getSerializerVelocity() {
-    serializerMotor.update(serializerMotorValues);
-
-    return serializerMotorValues.velocityRotationsPerSecond;
+    return serializerValues.velocityRotationsPerSecond;
   }
 
   public void setSetpoint(
       double flywheelVelocityRotationsPerSecond, double serializerVelocityRotationsPerSecond) {
-    flywheelMotor.setSetpoint(flywheelVelocityRotationsPerSecond);
-    serializerMotor.setSetpoint(serializerVelocityRotationsPerSecond);
+    flywheel.setSetpoint(flywheelVelocityRotationsPerSecond);
+    serializer.setSetpoint(serializerVelocityRotationsPerSecond);
   }
 }

src/main/java/frc/robot/shooter/ShooterConstants.java

@@ -1,13 +1,34 @@
 package frc.robot.shooter;
 
 import edu.wpi.first.math.filter.SlewRateLimiter;
+import frc.lib.CAN;
 import frc.lib.MotionProfileCalculator;
+import frc.lib.PIDFConstants;
+import frc.lib.controller.VelocityControllerIO.VelocityControllerIOConstants;
 
 /** Constants for the shooter subsystem. */
 public class ShooterConstants {
 
   /** Constants for the serializer motor used in the shooter subsystem. */
   public static class SerializerConstants {
+    /** Serializer's CAN. */
+    public static final CAN CAN = new CAN(42);
+
+    /** Serializer's PIDF constants. */
+    public static final PIDFConstants PIDF = new PIDFConstants();
+    static {
+      PIDF.kS = 0.14;
+      PIDF.kV = 0.2617;
+    }
+
+    /** Serializer's controller constants. */
+    public static final VelocityControllerIOConstants CONTROLLER_CONSTANTS = new VelocityControllerIOConstants();
+    static {
+      CONTROLLER_CONSTANTS.ccwPositive = false;
+      CONTROLLER_CONSTANTS.neutralBrake = true;
+      CONTROLLER_CONSTANTS.sensorToMechanismRatio = 36.0 / 16.0;
+    }
+
     /** Velocity to apply while intaking in rotations per second. */
     public static final double INTAKE_VELOCITY = 34;
 
@@ -26,6 +47,24 @@ public static class SerializerConstants {
 
   /** Constants for the flywheel motor used in the shooter subsystem. */
   public static class FlywheelConstants {
+    /** Flywheel's CAN. */
+    public static final CAN CAN = new CAN(44);
+
+    /** Flywheel's PIDF constants. */
+    public static final PIDFConstants PIDF = new PIDFConstants();
+    static {
+      PIDF.kS = 0.14;
+      PIDF.kV = 0.2539;
+    }
+
+    /** Flywheel's controller constants. */
+    public static final VelocityControllerIOConstants CONTROLLER_CONSTANTS = new VelocityControllerIOConstants();
+    static {
+      CONTROLLER_CONSTANTS.ccwPositive = false;
+      CONTROLLER_CONSTANTS.neutralBrake = true;
+      CONTROLLER_CONSTANTS.sensorToMechanismRatio = 36.0 / 16.0;
+    }
+
     /** Velocity to apply while shooting into the speaker in rotations per second. */
     public static final double SPEAKER_VELOCITY = 44;
 

src/main/java/frc/robot/shooter/ShooterFactory.java

@@ -1,8 +1,13 @@
 package frc.robot.shooter;
 
+import frc.lib.controller.VelocityControllerIO;
+import frc.lib.controller.VelocityControllerIOSim;
+import frc.lib.controller.VelocityControllerIOTalonFXPIDF;
 import frc.robot.Robot;
 import frc.robot.RobotConstants;
 import frc.robot.RobotConstants.Subsystem;
+import frc.robot.shooter.ShooterConstants.FlywheelConstants;
+import frc.robot.shooter.ShooterConstants.SerializerConstants;
 
 /** Helper class for creating hardware for the shooter subsystem. */
 public class ShooterFactory {
@@ -12,22 +17,24 @@ public class ShooterFactory {
    *
    * @return a serializer motor.
    */
-  public static SerializerMotorIO createSerializerMotor() {
-    if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.SHOOTER))
-      return new SerializerMotorIOTalonFX();
+  public static VelocityControllerIO createSerializerMotor() {
+    if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.SHOOTER)) {
+      return new VelocityControllerIOTalonFXPIDF(SerializerConstants.CAN, SerializerConstants.PIDF);
+    }
 
-    return new SerializerMotorIOSim();
+    return new VelocityControllerIOSim();
   }
 
   /**
    * Creates a flywheel motor.
    *
    * @return a flywheel motor.
    */
-  public static FlywheelMotorIO createFlywheelMotor() {
-    if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.SHOOTER))
-      return new FlywheelMotorIOTalonFX();
+  public static VelocityControllerIO createFlywheelMotor() {
+    if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.SHOOTER)) {
+      return new VelocityControllerIOTalonFXPIDF(FlywheelConstants.CAN, FlywheelConstants.PIDF);
+    }
 
-    return new FlywheelMotorIOSim();
+    return new VelocityControllerIOSim();
   }
 }