wip

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

@@ -33,6 +33,16 @@ public VelocityControllerIOTalonFXPIDF(CAN can, PIDFConstants pidf, boolean enab
         voltage = new VoltageOut(0.0).withEnableFOC(enableFOC);
     }
 
+    /**
+     * Creates a new velocity controller using TalonFX and external PIDF.
+     * 
+     * @param can
+     * @param pidf
+     */
+    public VelocityControllerIOTalonFXPIDF(CAN can, PIDFConstants pidf) {
+        this(can, pidf, false);
+    }
+
     @Override
     public void setSetpoint(double velocityRotationsPerSecond) {
         double feedforwardVolts = feedforward.calculate(velocityRotationsPerSecond);

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

@@ -4,25 +4,31 @@
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import frc.lib.Subsystem;
 import frc.lib.Telemetry;
-import frc.robot.intake.RollerMotorIO.RollerMotorIOValues;
+import frc.lib.controller.VelocityControllerIO;
+import frc.lib.controller.VelocityControllerIO.VelocityControllerIOConstants;
+import frc.lib.controller.VelocityControllerIO.VelocityControllerIOValues;
 
 /** Subsystem class for the intake subsystem. */
 public class Intake extends Subsystem {
 
   /** Instance variable for the intake subsystem singleton. */
   private static Intake instance = null;
 
-  /** Roller motor. */
-  private final RollerMotorIO rollerMotor;
+  /** Roller motors. */
+  private final VelocityControllerIO frontRoller, backRoller;
 
   /** Roller motor values. */
-  private final RollerMotorIOValues rollerMotorValues = new RollerMotorIOValues();
+  private final VelocityControllerIOValues frontRollerValues, backRollerValues;
 
   /** Creates a new instance of the intake subsystem. */
   private Intake() {
-    rollerMotor = IntakeFactory.createRollerMotor();
+    frontRoller = IntakeFactory.createFrontRoller();
+    frontRollerValues = new VelocityControllerIOValues();
+    frontRoller.configure(new VelocityControllerIOConstants());
 
-    rollerMotor.configure();
+    backRoller = IntakeFactory.createBackRoller();
+    backRollerValues = new VelocityControllerIOValues();
+    backRoller.configure(new VelocityControllerIOConstants());
   }
 
   /**
@@ -40,24 +46,30 @@ public static Intake getInstance() {
 
   @Override
   public void periodic() {
-    rollerMotor.update(rollerMotorValues);
+    frontRoller.update(frontRollerValues);
+    backRoller.update(backRollerValues);
   }
 
   @Override
   public void addToShuffleboard(ShuffleboardTab tab) {
-    ShuffleboardLayout roller = Telemetry.addColumn(tab, "Roller");
+    initializeRollerShuffleboard(tab, "Front Roller", frontRollerValues);
+    initializeRollerShuffleboard(tab, "Back Roller", backRollerValues);
+  }
+
+  public void initializeRollerShuffleboard(ShuffleboardTab tab, String name, VelocityControllerIOValues values) {
+    ShuffleboardLayout roller = Telemetry.addColumn(tab, "Front Roller");
 
-    roller.addDouble("Current (A)", () -> rollerMotorValues.currentAmps);
-    roller.addDouble("Velocity (rps)", () -> rollerMotorValues.velocityRotationsPerSecond);
+    roller.addDouble("Velocity (rps)", () -> values.velocityRotationsPerSecond);
+    roller.addDouble("Voltage (V)", () -> values.motorVolts);
+    roller.addDouble("Current (A)", () -> values.motorAmps);
   }
 
   public double getRollerVelocity() {
-    rollerMotor.update(rollerMotorValues);
-
-    return rollerMotorValues.velocityRotationsPerSecond;
+    return (frontRollerValues.velocityRotationsPerSecond + backRollerValues.velocityRotationsPerSecond) / 2;
   }
 
   public void setSetpoint(double rollerVelocityRotationsPerSecond) {
-    rollerMotor.setSetpoint(rollerVelocityRotationsPerSecond);
+    frontRoller.setSetpoint(rollerVelocityRotationsPerSecond);
+    backRoller.setSetpoint(rollerVelocityRotationsPerSecond);
   }
 }

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

@@ -1,21 +1,28 @@
 package frc.robot.intake;
 
-import frc.robot.Robot;
-import frc.robot.RobotConstants;
-import frc.robot.RobotConstants.Subsystem;
+import frc.lib.CAN;
+import frc.lib.PIDFConstants;
+import frc.lib.controller.VelocityControllerIO;
+import frc.lib.controller.VelocityControllerIOTalonFXPIDF;
 
 /** Helper class for creating hardware for the intake subsystem. */
 public class IntakeFactory {
 
-  /**
-   * Creates a roller motor.
-   *
-   * @return a roller motor.
-   */
-  public static RollerMotorIO createRollerMotor() {
-    if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.INTAKE))
-      return new RollerMotorIOTalonFX();
+  public static VelocityControllerIO createFrontRoller() {
+    PIDFConstants pidf = new PIDFConstants();
 
-    return new RollerMotorIOSim();
+    pidf.kS = 0.13;
+    pidf.kV = 0.1683;
+
+    return new VelocityControllerIOTalonFXPIDF(new CAN(50), pidf);
+  }
+
+  public static VelocityControllerIO createBackRoller() {
+    PIDFConstants pidf = new PIDFConstants();
+
+    pidf.kS = 0.13;
+    pidf.kV = 0.1759;
+
+    return new VelocityControllerIOTalonFXPIDF(new CAN(40), pidf);
   }
 }