From fb1a685dafdb3085cffe32eab17fe8a699e6701a Mon Sep 17 00:00:00 2001 From: DylanTaylor29 <132412179+DylanTaylor29@users.noreply.github.com> Date: Thu, 12 Feb 2026 19:24:51 -0500 Subject: [PATCH 1/2] added real kicker and spindexer --- .../subsystems/feeder/KickerIOSpark.java | 86 +++++++++++++++++++ .../subsystems/feeder/SpindexerIOSpark.java | 83 ++++++++++++++++++ 2 files changed, 169 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/feeder/KickerIOSpark.java create mode 100644 src/main/java/frc/robot/subsystems/feeder/SpindexerIOSpark.java diff --git a/src/main/java/frc/robot/subsystems/feeder/KickerIOSpark.java b/src/main/java/frc/robot/subsystems/feeder/KickerIOSpark.java new file mode 100644 index 0000000..8d1abd4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/feeder/KickerIOSpark.java @@ -0,0 +1,86 @@ +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.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.LinearSystemId; +import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import frc.robot.Constants.CANBusPorts.CAN2; +import frc.robot.Constants.MotorConstants.NEOVortexConstants; +import frc.robot.util.SparkOdometryThread; +import frc.robot.util.SparkOdometryThread.SparkInputs; +import frc.robot.Constants.RobotConstants; +import frc.robot.Robot; + +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); + } +} diff --git a/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSpark.java b/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSpark.java new file mode 100644 index 0000000..53bc506 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSpark.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.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.util.SparkOdometryThread; +import frc.robot.util.SparkOdometryThread.SparkInputs; +import frc.robot.Constants.RobotConstants; + +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); + } +} From 3f05bb1628203cc6ab7e7df5732596bb6b4218f4 Mon Sep 17 00:00:00 2001 From: DylanTaylor29 <132412179+DylanTaylor29@users.noreply.github.com> Date: Thu, 12 Feb 2026 19:27:41 -0500 Subject: [PATCH 2/2] formatted --- .../java/frc/robot/subsystems/feeder/KickerIOSpark.java | 9 +++------ .../frc/robot/subsystems/feeder/SpindexerIOSpark.java | 5 +++-- 2 files changed, 6 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/feeder/KickerIOSpark.java b/src/main/java/frc/robot/subsystems/feeder/KickerIOSpark.java index 8d1abd4..690ce8b 100644 --- a/src/main/java/frc/robot/subsystems/feeder/KickerIOSpark.java +++ b/src/main/java/frc/robot/subsystems/feeder/KickerIOSpark.java @@ -7,7 +7,6 @@ import com.revrobotics.PersistMode; import com.revrobotics.RelativeEncoder; 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; @@ -16,15 +15,12 @@ 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.LinearSystemId; import edu.wpi.first.units.measure.LinearVelocity; -import edu.wpi.first.wpilibj.simulation.DCMotorSim; 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; -import frc.robot.Constants.RobotConstants; -import frc.robot.Robot; public class KickerIOSpark implements KickerIO { @@ -68,7 +64,8 @@ public void updateInputs(KickerIOInputs inputs) { @Override public void setOpenLoop(double output) { - flex.setVoltage(output);; + flex.setVoltage(output); + ; } @Override diff --git a/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSpark.java b/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSpark.java index 53bc506..acdff43 100644 --- a/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSpark.java +++ b/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSpark.java @@ -18,9 +18,9 @@ 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; -import frc.robot.Constants.RobotConstants; public class SpindexerIOSpark implements SpindexerIO { @@ -65,7 +65,8 @@ public void updateInputs(SpindexerIOInputs inputs) { @Override public void setOpenLoop(double output) { - flex.setVoltage(output);; + flex.setVoltage(output); + ; } @Override