added real kicker and spindexer

src/main/java/frc/robot/subsystems/feeder/KickerIOSpark.java

@@ -0,0 +1,83 @@
+package frc.robot.subsystems.feeder;
+
+import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.MetersPerSecond;
+import static frc.robot.subsystems.feeder.FeederConstants.KickerConstants.*;
+
+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 frc.robot.Constants.CANBusPorts.CAN2;
+import frc.robot.Constants.MotorConstants.NEOVortexConstants;
+import frc.robot.Constants.RobotConstants;
+import frc.robot.util.SparkOdometryThread;
+import frc.robot.util.SparkOdometryThread.SparkInputs;
+
+public class KickerIOSpark implements KickerIO {
+
+  private final SparkFlex flex;
+  private final RelativeEncoder encoder;
+  private final SparkClosedLoopController controller;
+  private final SparkInputs sparkInputs;
+
+  public KickerIOSpark() {
+    flex = new SparkFlex(CAN2.kicker, MotorType.kBrushless);
+    encoder = flex.getEncoder();
+    controller = flex.getClosedLoopController();
+
+    var config = new SparkFlexConfig();
+    config
+        .inverted(false)
+        .idleMode(IdleMode.kBrake)
+        .smartCurrentLimit(NEOVortexConstants.kDefaultSupplyCurrentLimit)
+        .voltageCompensation(RobotConstants.kNominalVoltage);
+
+    config
+        .encoder
+        .positionConversionFactor(encoderPositionFactor)
+        .velocityConversionFactor(encoderVelocityFactor);
+
+    config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(kPSim, 0.0, 0.0);
+
+    flex.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+
+    sparkInputs = SparkOdometryThread.getInstance().registerSpark(flex, encoder);
+  }
+
+  @Override
+  public void updateInputs(KickerIOInputs inputs) {
+    // Update inputs
+    inputs.connected = true;
+    inputs.velocityMetersPerSec = sparkInputs.getVelocity();
+    inputs.appliedVolts = sparkInputs.getAppliedVolts();
+    inputs.currentAmps = sparkInputs.getOutputCurrent();
+  }
+
+  @Override
+  public void setOpenLoop(double output) {
+    flex.setVoltage(output);
+    ;
+  }
+
+  @Override
+  public void setVelocity(LinearVelocity tangentialVelocity) {
+    double feedforwardVolts =
+        RobotConstants.kNominalVoltage
+            * tangentialVelocity.in(MetersPerSecond)
+            / maxTangentialVelocity.in(MetersPerSecond);
+    controller.setSetpoint(
+        tangentialVelocity.in(MetersPerSecond) / radius.in(Meters),
+        ControlType.kVelocity,
+        ClosedLoopSlot.kSlot0,
+        feedforwardVolts);
+  }
+}

src/main/java/frc/robot/subsystems/feeder/SpindexerIOSpark.java

@@ -0,0 +1,84 @@
+package frc.robot.subsystems.feeder;
+
+import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.MetersPerSecond;
+import static frc.robot.subsystems.feeder.FeederConstants.SpindexerConstants.*;
+
+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 frc.robot.Constants.CANBusPorts.CAN2;
+import frc.robot.Constants.MotorConstants.NEOVortexConstants;
+import frc.robot.Constants.RobotConstants;
+import frc.robot.util.SparkOdometryThread;
+import frc.robot.util.SparkOdometryThread.SparkInputs;
+
+public class SpindexerIOSpark implements SpindexerIO {
+
+  private final SparkFlex flex;
+  private final RelativeEncoder encoder;
+  private final SparkClosedLoopController controller;
+  private final SparkInputs sparkInputs;
+
+  public SpindexerIOSpark() {
+    flex = new SparkFlex(CAN2.spindexer, MotorType.kBrushless);
+    encoder = flex.getEncoder();
+    controller = flex.getClosedLoopController();
+
+    var config = new SparkFlexConfig();
+    config
+        .inverted(false)
+        .idleMode(IdleMode.kBrake)
+        .smartCurrentLimit(NEOVortexConstants.kDefaultSupplyCurrentLimit)
+        .voltageCompensation(RobotConstants.kNominalVoltage);
+
+    config
+        .encoder
+        .positionConversionFactor(encoderPositionFactor)
+        .velocityConversionFactor(encoderVelocityFactor);
+
+    config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(kPSim, 0.0, 0.0);
+
+    flex.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+
+    sparkInputs = SparkOdometryThread.getInstance().registerSpark(flex, encoder);
+  }
+
+  @Override
+  public void updateInputs(SpindexerIOInputs inputs) {
+
+    // Update inputs
+    inputs.connected = true;
+    inputs.velocityMetersPerSec = sparkInputs.getVelocity();
+    inputs.appliedVolts = sparkInputs.getAppliedVolts();
+    inputs.currentAmps = sparkInputs.getOutputCurrent();
+  }
+
+  @Override
+  public void setOpenLoop(double output) {
+    flex.setVoltage(output);
+    ;
+  }
+
+  @Override
+  public void setVelocity(LinearVelocity tangentialVelocity) {
+    double feedforwardVolts =
+        RobotConstants.kNominalVoltage
+            * tangentialVelocity.in(MetersPerSecond)
+            / maxTangentialVelocity.in(MetersPerSecond);
+    controller.setSetpoint(
+        tangentialVelocity.in(MetersPerSecond) / radius.in(Meters),
+        ControlType.kVelocity,
+        ClosedLoopSlot.kSlot0,
+        feedforwardVolts);
+  }
+}