Switch intake rollers to Spark/NEO Vortex

build.gradle

@@ -155,6 +155,8 @@ wpi.java.configureTestTasks(test)
 // Configure string concat to always inline compile
 tasks.withType(JavaCompile) {
     options.compilerArgs.add '-XDstringConcat=inline'
+    // Ensure Java compiler treats source files as UTF-8 so Unicode comments/characters compile
+    options.encoding = 'UTF-8'
 }
 
 // Create version file

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

@@ -72,8 +72,8 @@
 import frc.robot.subsystems.intake.IntakeConstants;
 import frc.robot.subsystems.intake.IntakeConstants.RollerConstants;
 import frc.robot.subsystems.intake.RollerIO;
-import frc.robot.subsystems.intake.RollerIOSimTalonFX;
-import frc.robot.subsystems.intake.RollerIOTalonFX;
+import frc.robot.subsystems.intake.RollerIOSimSpark;
+import frc.robot.subsystems.intake.RollerIOSpark;
 import frc.robot.subsystems.launcher.FlywheelIO;
 import frc.robot.subsystems.launcher.FlywheelIOSimTalonFX;
 import frc.robot.subsystems.launcher.FlywheelIOTalonFX;
@@ -198,8 +198,8 @@ public Robot() {
         if (FeatureFlags.kHopperEnabled) hopper = new Hopper(new HopperIOReal());
         intake =
             new Intake(
-                new RollerIOTalonFX(RollerConstants.upperRollerConfig),
-                new RollerIOTalonFX(RollerConstants.lowerRollerConfig),
+                new RollerIOSpark(RollerConstants.upperRollerConfig),
+                new RollerIOSpark(RollerConstants.lowerRollerConfig),
                 new IntakeArmIOReal());
         feeder = new Feeder(new SpindexerIOSpark(), new KickerIOSpark());
         compressor = new LoggedCompressor(PneumaticsModuleType.REVPH, "Compressor");
@@ -245,8 +245,8 @@ public Robot() {
         var intakeArmIOSim = new IntakeArmIOSim();
         intake =
             new Intake(
-                new RollerIOSimTalonFX(RollerConstants.upperRollerConfig),
-                new RollerIOSimTalonFX(RollerConstants.lowerRollerConfig),
+                new RollerIOSimSpark(RollerConstants.upperRollerConfig),
+                new RollerIOSimSpark(RollerConstants.lowerRollerConfig),
                 intakeArmIOSim);
         pneumaticsSimulator =
             new PneumaticsSimulator(intakeArmIOSim.intakeArmPneumatic, new REVPHSim(1));

src/main/java/frc/robot/subsystems/intake/IntakeConstants.java

@@ -3,13 +3,8 @@
 import static edu.wpi.first.units.Units.*;
 
 import com.ctre.phoenix6.CANBus;
-import com.ctre.phoenix6.configs.Slot0Configs;
-import com.ctre.phoenix6.configs.Slot1Configs;
-import edu.wpi.first.math.system.plant.DCMotor;
-import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.Distance;
 import frc.robot.Constants.CANBusPorts.CAN2;
-import frc.robot.Constants.MotorConstants.KrakenX60Constants;
 
 public class IntakeConstants {
   /** Time (seconds) to wait after resolving an intake/hopper interlock before proceeding. */
@@ -20,18 +15,13 @@ public static class RollerConstants {
 
     // motor controller
     public static final double motorReduction = 1.0;
-    public static final AngularVelocity maxAngularVelocity =
-        KrakenX60Constants.kFreeSpeed.div(motorReduction);
-    public static final Slot0Configs velocityVoltageGains =
-        new Slot0Configs().withKP(0.11).withKI(0.0).withKD(0.0).withKS(0.1).withKV(0.12);
-    public static final Slot1Configs velocityTorqueCurrentGains =
-        new Slot1Configs().withKP(5).withKI(0.0).withKD(0.0).withKS(2.5);
-
+    public static final int numMotors = 1;
     public static final double maxAcceleration = 4000.0;
     public static final double maxJerk = 40000.0;
 
-    // simulation
-    public static final DCMotor gearbox = DCMotor.getKrakenX60(2);
+    // roller constants
+    public static final double encoderPositionFactor = 2.0 * Math.PI / motorReduction; // Meters
+    public static final double encoderVelocityFactor = encoderPositionFactor / 60.0; // Meters/sec
 
     // configs
     public static final RollerConfig upperRollerConfig =

src/main/java/frc/robot/subsystems/intake/RollerIOSimSpark.java

@@ -0,0 +1,103 @@
+package frc.robot.subsystems.intake;
+
+import static edu.wpi.first.units.Units.*;
+import static frc.robot.subsystems.intake.IntakeConstants.RollerConstants.*;
+
+import com.revrobotics.PersistMode;
+import com.revrobotics.ResetMode;
+import com.revrobotics.sim.SparkFlexSim;
+import com.revrobotics.spark.ClosedLoopSlot;
+import com.revrobotics.spark.FeedbackSensor;
+import com.revrobotics.spark.SparkBase.ControlType;
+import com.revrobotics.spark.SparkClosedLoopController;
+import com.revrobotics.spark.SparkFlex;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.config.SparkFlexConfig;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.units.measure.LinearVelocity;
+import edu.wpi.first.units.measure.Voltage;
+import edu.wpi.first.wpilibj.simulation.DCMotorSim;
+import edu.wpi.first.wpilibj.simulation.RoboRioSim;
+import frc.robot.Constants.MotorConstants.NEOVortexConstants;
+import frc.robot.Constants.RobotConstants;
+import frc.robot.Robot;
+import frc.robot.subsystems.intake.IntakeConstants.RollerConfig;
+
+public class RollerIOSimSpark implements RollerIO {
+  private static final double KICKER_MOI_KG_M2 = 0.00052;
+  private static final double KP = 0.11;
+  private static final double KD = 0.0;
+  private static final LinearVelocity maxTangentialVelocity =
+      MetersPerSecond.of(
+          NEOVortexConstants.kFreeSpeed.in(RadiansPerSecond)
+              * rollerRadius.in(Meters)
+              / motorReduction);
+  private static final DCMotor gearbox = DCMotor.getNeoVortex(numMotors);
+
+  private final DCMotorSim rollerSim;
+
+  private final SparkFlex flex;
+  private final SparkClosedLoopController controller;
+  private final SparkFlexSim flexSim;
+
+  public RollerIOSimSpark(RollerConfig rollerConfig) {
+    flex = new SparkFlex(rollerConfig.port, MotorType.kBrushless);
+    controller = flex.getClosedLoopController();
+
+    var config = new SparkFlexConfig();
+    config
+        .inverted(rollerConfig.inverted)
+        .idleMode(IdleMode.kBrake)
+        .smartCurrentLimit(NEOVortexConstants.kDefaultSupplyCurrentLimit)
+        .voltageCompensation(RobotConstants.kNominalVoltage);
+
+    config
+        .encoder
+        .positionConversionFactor(encoderPositionFactor)
+        .velocityConversionFactor(encoderVelocityFactor);
+
+    config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(KP, 0.0, KD);
+
+    flex.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+    flexSim = new SparkFlexSim(flex, gearbox);
+
+    rollerSim =
+        new DCMotorSim(
+            LinearSystemId.createDCMotorSystem(gearbox, KICKER_MOI_KG_M2, motorReduction), gearbox);
+  }
+
+  @Override
+  public void updateInputs(RollerIOInputs inputs) {
+    // Update simulation state
+    double busVoltage = RoboRioSim.getVInVoltage();
+    rollerSim.setInput(flexSim.getAppliedOutput() * busVoltage);
+    rollerSim.update(Robot.defaultPeriodSecs);
+    flexSim.iterate(rollerSim.getAngularVelocityRadPerSec(), busVoltage, Robot.defaultPeriodSecs);
+
+    // Update inputs
+    inputs.connected = true;
+    inputs.velocityMetersPerSec = flexSim.getVelocity() * rollerRadius.in(Meters);
+    inputs.appliedVolts = flexSim.getAppliedOutput() * flexSim.getBusVoltage();
+    inputs.currentAmps = Math.abs(flexSim.getMotorCurrent());
+  }
+
+  @Override
+  public void setOpenLoop(Voltage volts) {
+    flexSim.setAppliedOutput(volts.in(Volts) / RobotConstants.kNominalVoltage);
+  }
+
+  @Override
+  public void setVelocity(LinearVelocity tangentialVelocity) {
+    double feedforwardVolts =
+        RobotConstants.kNominalVoltage
+            * tangentialVelocity.in(MetersPerSecond)
+            / maxTangentialVelocity.in(MetersPerSecond);
+    controller.setSetpoint(
+        tangentialVelocity.in(MetersPerSecond) / rollerRadius.in(Meters),
+        ControlType.kVelocity,
+        ClosedLoopSlot.kSlot0,
+        feedforwardVolts);
+  }
+}

src/main/java/frc/robot/subsystems/intake/RollerIOSimTalonFX.java

@@ -6,6 +6,8 @@
 
 import com.ctre.phoenix6.BaseStatusSignal;
 import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.Slot1Configs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.NeutralOut;
 import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC;
@@ -14,6 +16,7 @@
 import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import edu.wpi.first.math.filter.Debouncer;
+import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.system.plant.LinearSystemId;
 import edu.wpi.first.units.measure.AngularAcceleration;
 import edu.wpi.first.units.measure.AngularVelocity;
@@ -26,6 +29,13 @@
 import frc.robot.subsystems.intake.IntakeConstants.RollerConfig;
 
 public class RollerIOSimTalonFX implements RollerIO {
+  private static final double kP = 0.11;
+  private static final double kD = 0.0;
+  private static final Slot0Configs velocityVoltageGains =
+      new Slot0Configs().withKP(kP).withKI(0.0).withKD(kD).withKS(0.1).withKV(0.12);
+  private static final Slot1Configs velocityTorqueCurrentGains =
+      new Slot1Configs().withKP(kP).withKI(0.0).withKD(kD).withKS(2.5);
+  private static final DCMotor gearbox = DCMotor.getKrakenX60(numMotors);
 
   private final DCMotorSim rollerSim;
 

src/main/java/frc/robot/subsystems/intake/RollerIOSpark.java

@@ -0,0 +1,96 @@
+package frc.robot.subsystems.intake;
+
+import static edu.wpi.first.units.Units.*;
+import static frc.robot.subsystems.intake.IntakeConstants.RollerConstants.*;
+import static frc.robot.util.SparkUtil.*;
+
+import com.revrobotics.PersistMode;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.ResetMode;
+import com.revrobotics.spark.ClosedLoopSlot;
+import com.revrobotics.spark.FeedbackSensor;
+import com.revrobotics.spark.SparkBase.ControlType;
+import com.revrobotics.spark.SparkClosedLoopController;
+import com.revrobotics.spark.SparkFlex;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.config.SparkFlexConfig;
+import edu.wpi.first.units.measure.LinearVelocity;
+import edu.wpi.first.units.measure.Voltage;
+import frc.robot.Constants.MotorConstants.NEOVortexConstants;
+import frc.robot.Constants.RobotConstants;
+import frc.robot.subsystems.intake.IntakeConstants.RollerConfig;
+import frc.robot.util.SparkOdometryThread;
+import frc.robot.util.SparkOdometryThread.SparkInputs;
+
+public class RollerIOSpark implements RollerIO {
+  private static final double KP = 0.001;
+  private static final double KD = 0.0;
+  private static final LinearVelocity maxTangentialVelocity =
+      MetersPerSecond.of(
+          NEOVortexConstants.kFreeSpeed.in(RadiansPerSecond)
+              * rollerRadius.in(Meters)
+              / motorReduction);
+
+  private final SparkFlex flex;
+  private final RelativeEncoder encoder;
+  private final SparkClosedLoopController controller;
+  private final SparkInputs sparkInputs;
+
+  public RollerIOSpark(RollerConfig rollerConfig) {
+    flex = new SparkFlex(rollerConfig.port, MotorType.kBrushless);
+    encoder = flex.getEncoder();
+    controller = flex.getClosedLoopController();
+
+    var config = new SparkFlexConfig();
+    config
+        .inverted(rollerConfig.inverted)
+        .idleMode(IdleMode.kBrake)
+        .smartCurrentLimit(NEOVortexConstants.kDefaultSupplyCurrentLimit)
+        .voltageCompensation(RobotConstants.kNominalVoltage);
+
+    config
+        .encoder
+        .positionConversionFactor(encoderPositionFactor)
+        .velocityConversionFactor(encoderVelocityFactor)
+        .uvwAverageDepth(2)
+        .uvwMeasurementPeriod(8);
+
+    config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(KP, 0.0, KD);
+
+    tryUntilOk(
+        flex,
+        5,
+        () ->
+            flex.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters));
+
+    sparkInputs = SparkOdometryThread.getInstance().registerSpark(flex, encoder);
+  }
+
+  @Override
+  public void updateInputs(RollerIOInputs inputs) {
+    inputs.connected = sparkInputs.isConnected();
+    inputs.velocityMetersPerSec = sparkInputs.getVelocity() * rollerRadius.in(Meters);
+    inputs.appliedVolts = sparkInputs.getAppliedVolts();
+    inputs.currentAmps = sparkInputs.getOutputCurrent();
+  }
+
+  @Override
+  public void setOpenLoop(Voltage volts) {
+    flex.setVoltage(volts);
+    ;
+  }
+
+  @Override
+  public void setVelocity(LinearVelocity tangentialVelocity) {
+    double feedforwardVolts =
+        RobotConstants.kNominalVoltage
+            * tangentialVelocity.in(MetersPerSecond)
+            / maxTangentialVelocity.in(MetersPerSecond);
+    controller.setSetpoint(
+        tangentialVelocity.in(MetersPerSecond) / rollerRadius.in(Meters),
+        ControlType.kVelocity,
+        ClosedLoopSlot.kSlot0,
+        feedforwardVolts);
+  }
+}

src/main/java/frc/robot/subsystems/intake/RollerIOTalonFX.java

@@ -6,6 +6,8 @@
 
 import com.ctre.phoenix6.BaseStatusSignal;
 import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.Slot1Configs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.NeutralOut;
 import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC;
@@ -23,6 +25,13 @@
 import frc.robot.subsystems.intake.IntakeConstants.RollerConfig;
 
 public class RollerIOTalonFX implements RollerIO {
+  private static final double kP = 0.11;
+  private static final double kD = 0.0;
+  private static final Slot0Configs velocityVoltageGains =
+      new Slot0Configs().withKP(kP).withKI(0.0).withKD(kD).withKS(0.1).withKV(0.12);
+  private static final Slot1Configs velocityTorqueCurrentGains =
+      new Slot1Configs().withKP(kP).withKI(0.0).withKD(kD).withKS(2.5);
+
   private final TalonFX motor;
   private final TalonFXConfiguration config;
   private final Debouncer connectedDebounce = new Debouncer(0.5, Debouncer.DebounceType.kFalling);