From 732553d6e4baca7c75fca6618e468b38db8c9e57 Mon Sep 17 00:00:00 2001 From: BenGamer3 Date: Tue, 14 Apr 2026 18:12:23 -0400 Subject: [PATCH 1/6] added .java files --- .../subsystems/intake/RollerIOSimSpark.java | 94 +++++++++++++++++++ .../subsystems/intake/RollerIOSpark.java | 91 ++++++++++++++++++ 2 files changed, 185 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/intake/RollerIOSimSpark.java create mode 100644 src/main/java/frc/robot/subsystems/intake/RollerIOSpark.java diff --git a/src/main/java/frc/robot/subsystems/intake/RollerIOSimSpark.java b/src/main/java/frc/robot/subsystems/intake/RollerIOSimSpark.java new file mode 100644 index 0000000..a6b59df --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/RollerIOSimSpark.java @@ -0,0 +1,94 @@ +package frc.robot.subsystems.intake; + +import static edu.wpi.first.units.Units.*; +import static frc.robot.subsystems.feeder.FeederConstants.KickerConstants.*; + +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.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.CANBusPorts.CAN2; +import frc.robot.Constants.MotorConstants.NEOVortexConstants; +import frc.robot.Constants.RobotConstants; +import frc.robot.Robot; + +public class RollerIOSimSpark implements RollerIO { + private static final double KICKER_MOI_KG_M2 = 0.00052; + + private final DCMotorSim kickerSim; + + private final SparkFlex flex; + private final SparkClosedLoopController controller; + private final SparkFlexSim flexSim; + + public RollerIOSimSpark() { + flex = new SparkFlex(CAN2.kicker, MotorType.kBrushless); + 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); + flexSim = new SparkFlexSim(flex, gearbox); + + kickerSim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem(gearbox, KICKER_MOI_KG_M2, motorReduction), gearbox); + } + + @Override + public void updateInputs(RollerIOInputs inputs) { + // Update simulation state + double busVoltage = RoboRioSim.getVInVoltage(); + kickerSim.setInput(flexSim.getAppliedOutput() * busVoltage); + kickerSim.update(Robot.defaultPeriodSecs); + flexSim.iterate(kickerSim.getAngularVelocityRadPerSec(), busVoltage, Robot.defaultPeriodSecs); + + // Update inputs + inputs.connected = true; + inputs.velocityMetersPerSec = flexSim.getVelocity() * radius.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) / radius.in(Meters), + ControlType.kVelocity, + ClosedLoopSlot.kSlot0, + feedforwardVolts); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/RollerIOSpark.java b/src/main/java/frc/robot/subsystems/intake/RollerIOSpark.java new file mode 100644 index 0000000..5d3d210 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/RollerIOSpark.java @@ -0,0 +1,91 @@ +package frc.robot.subsystems.intake; + +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 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.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 RollerIOSpark implements RollerIO { + + private final SparkFlex flex; + private final RelativeEncoder encoder; + private final SparkClosedLoopController controller; + private final SparkInputs sparkInputs; + + public RollerIOSpark() { + 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) + .uvwAverageDepth(2) + .uvwMeasurementPeriod(8); + + config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(kPSim, 0.0, kDSim); + + 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() * radius.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) / radius.in(Meters), + ControlType.kVelocity, + ClosedLoopSlot.kSlot0, + feedforwardVolts); + } +} From 885b8f0b87f94a9cc07408e2c4f7e9ffe068b31f Mon Sep 17 00:00:00 2001 From: BenGamer3 Date: Tue, 14 Apr 2026 18:44:06 -0400 Subject: [PATCH 2/6] deleated old feeder and changed to intake --- .../subsystems/intake/IntakeConstants.java | 17 +++++++++++++++-- .../subsystems/intake/RollerIOSpark.java | 19 +++++++++++-------- 2 files changed, 26 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index f8e53da..cfab838 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -8,8 +8,10 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearVelocity; import frc.robot.Constants.CANBusPorts.CAN2; import frc.robot.Constants.MotorConstants.KrakenX60Constants; +import frc.robot.Constants.MotorConstants.NEOVortexConstants; public class IntakeConstants { /** Time (seconds) to wait after resolving an intake/hopper interlock before proceeding. */ @@ -22,14 +24,25 @@ public static class RollerConstants { public static final double motorReduction = 1.0; public static final AngularVelocity maxAngularVelocity = KrakenX60Constants.kFreeSpeed.div(motorReduction); + public static final double kP = 0.11; + public static final double kD = 0.0; public static final Slot0Configs velocityVoltageGains = - new Slot0Configs().withKP(0.11).withKI(0.0).withKD(0.0).withKS(0.1).withKV(0.12); + new Slot0Configs().withKP(kP).withKI(0.0).withKD(kD).withKS(0.1).withKV(0.12); public static final Slot1Configs velocityTorqueCurrentGains = - new Slot1Configs().withKP(5).withKI(0.0).withKD(0.0).withKS(2.5); + new Slot1Configs().withKP(kP).withKI(0.0).withKD(kD).withKS(2.5); public static final double maxAcceleration = 4000.0; public static final double maxJerk = 40000.0; + // roller constants + public static final double encoderPositionFactor = 2.0 * Math.PI / motorReduction; // Meters + public static final double encoderVelocityFactor = encoderPositionFactor / 60.0; // Meters/sec + public static final LinearVelocity maxTangentialVelocity = + MetersPerSecond.of( + NEOVortexConstants.kFreeSpeed.in(RadiansPerSecond) + * rollerRadius.in(Meters) + / motorReduction); + // simulation public static final DCMotor gearbox = DCMotor.getKrakenX60(2); diff --git a/src/main/java/frc/robot/subsystems/intake/RollerIOSpark.java b/src/main/java/frc/robot/subsystems/intake/RollerIOSpark.java index 5d3d210..0776e01 100644 --- a/src/main/java/frc/robot/subsystems/intake/RollerIOSpark.java +++ b/src/main/java/frc/robot/subsystems/intake/RollerIOSpark.java @@ -2,7 +2,8 @@ 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 static frc.robot.subsystems.intake.IntakeConstants.RollerConfig; +import static frc.robot.subsystems.intake.IntakeConstants.RollerConstants.*; import static frc.robot.util.SparkUtil.*; import com.revrobotics.PersistMode; @@ -18,7 +19,6 @@ import com.revrobotics.spark.config.SparkFlexConfig; import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.units.measure.Voltage; -import frc.robot.Constants.CANBusPorts.CAN2; import frc.robot.Constants.MotorConstants.NEOVortexConstants; import frc.robot.Constants.RobotConstants; import frc.robot.util.SparkOdometryThread; @@ -31,14 +31,17 @@ public class RollerIOSpark implements RollerIO { private final SparkClosedLoopController controller; private final SparkInputs sparkInputs; - public RollerIOSpark() { - flex = new SparkFlex(CAN2.kicker, MotorType.kBrushless); + private final double KP = 0.11; + private final double KD = 0.0; + + public RollerIOSpark(RollerConfig rollerConfig) { + flex = new SparkFlex(rollerConfig.port, MotorType.kBrushless); encoder = flex.getEncoder(); controller = flex.getClosedLoopController(); var config = new SparkFlexConfig(); config - .inverted(false) + .inverted(rollerConfig.inverted) .idleMode(IdleMode.kBrake) .smartCurrentLimit(NEOVortexConstants.kDefaultSupplyCurrentLimit) .voltageCompensation(RobotConstants.kNominalVoltage); @@ -50,7 +53,7 @@ public RollerIOSpark() { .uvwAverageDepth(2) .uvwMeasurementPeriod(8); - config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(kPSim, 0.0, kDSim); + config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(KP, 0.0, KD); tryUntilOk( flex, @@ -65,7 +68,7 @@ public RollerIOSpark() { public void updateInputs(RollerIOInputs inputs) { inputs.connected = sparkInputs.isConnected(); - inputs.velocityMetersPerSec = sparkInputs.getVelocity() * radius.in(Meters); + inputs.velocityMetersPerSec = sparkInputs.getVelocity() * rollerRadius.in(Meters); inputs.appliedVolts = sparkInputs.getAppliedVolts(); inputs.currentAmps = sparkInputs.getOutputCurrent(); } @@ -83,7 +86,7 @@ public void setVelocity(LinearVelocity tangentialVelocity) { * tangentialVelocity.in(MetersPerSecond) / maxTangentialVelocity.in(MetersPerSecond); controller.setSetpoint( - tangentialVelocity.in(MetersPerSecond) / radius.in(Meters), + tangentialVelocity.in(MetersPerSecond) / rollerRadius.in(Meters), ControlType.kVelocity, ClosedLoopSlot.kSlot0, feedforwardVolts); From d436cfe9d5212b4d517865e9cda1038598613632 Mon Sep 17 00:00:00 2001 From: BenGamer3 Date: Thu, 16 Apr 2026 18:34:13 -0400 Subject: [PATCH 3/6] made it so the files on my computer could be read --- build.gradle | 2 ++ 1 file changed, 2 insertions(+) diff --git a/build.gradle b/build.gradle index 8620dfd..bf72faa 100644 --- a/build.gradle +++ b/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 From 412452605f6ff4fb3a5dfc489a4f93fa945db212 Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Tue, 21 Apr 2026 18:25:58 -0400 Subject: [PATCH 4/6] Switch intake rollers to Spark/NEO Vortex, move gains into IO classes - Robot.java: use RollerIOSpark instead of RollerIOTalonFX for both rollers - IntakeConstants: remove Kraken/TalonFX-specific gains and DCMotor gearbox; add numMotors constant - RollerIOSpark/SimSpark: make PID gains and maxTangentialVelocity static; fix RollerIOSimSpark to accept RollerConfig and use intake constants instead of kicker constants - RollerIOTalonFX/SimTalonFX: move Slot0/Slot1 gain configs into each class Co-Authored-By: Claude Sonnet 4.6 --- src/main/java/frc/robot/Robot.java | 6 ++-- .../subsystems/intake/IntakeConstants.java | 25 +------------ .../subsystems/intake/RollerIOSimSpark.java | 35 ++++++++++++------- .../subsystems/intake/RollerIOSimTalonFX.java | 10 ++++++ .../subsystems/intake/RollerIOSpark.java | 16 +++++---- .../subsystems/intake/RollerIOTalonFX.java | 9 +++++ 6 files changed, 54 insertions(+), 47 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a1801a1..e4c163f 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -73,7 +73,7 @@ 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.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"); diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index cfab838..8b89b3e 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -3,15 +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 edu.wpi.first.units.measure.LinearVelocity; import frc.robot.Constants.CANBusPorts.CAN2; -import frc.robot.Constants.MotorConstants.KrakenX60Constants; -import frc.robot.Constants.MotorConstants.NEOVortexConstants; public class IntakeConstants { /** Time (seconds) to wait after resolving an intake/hopper interlock before proceeding. */ @@ -22,29 +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 double kP = 0.11; - public static final double kD = 0.0; - public static final Slot0Configs velocityVoltageGains = - new Slot0Configs().withKP(kP).withKI(0.0).withKD(kD).withKS(0.1).withKV(0.12); - public static final Slot1Configs velocityTorqueCurrentGains = - new Slot1Configs().withKP(kP).withKI(0.0).withKD(kD).withKS(2.5); - + public static final int numMotors = 1; public static final double maxAcceleration = 4000.0; public static final double maxJerk = 40000.0; // roller constants public static final double encoderPositionFactor = 2.0 * Math.PI / motorReduction; // Meters public static final double encoderVelocityFactor = encoderPositionFactor / 60.0; // Meters/sec - public static final LinearVelocity maxTangentialVelocity = - MetersPerSecond.of( - NEOVortexConstants.kFreeSpeed.in(RadiansPerSecond) - * rollerRadius.in(Meters) - / motorReduction); - - // simulation - public static final DCMotor gearbox = DCMotor.getKrakenX60(2); // configs public static final RollerConfig upperRollerConfig = diff --git a/src/main/java/frc/robot/subsystems/intake/RollerIOSimSpark.java b/src/main/java/frc/robot/subsystems/intake/RollerIOSimSpark.java index a6b59df..ef4e044 100644 --- a/src/main/java/frc/robot/subsystems/intake/RollerIOSimSpark.java +++ b/src/main/java/frc/robot/subsystems/intake/RollerIOSimSpark.java @@ -1,7 +1,7 @@ package frc.robot.subsystems.intake; import static edu.wpi.first.units.Units.*; -import static frc.robot.subsystems.feeder.FeederConstants.KickerConstants.*; +import static frc.robot.subsystems.intake.IntakeConstants.RollerConstants.*; import com.revrobotics.PersistMode; import com.revrobotics.ResetMode; @@ -14,32 +14,41 @@ 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.CANBusPorts.CAN2; 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 kickerSim; + private final DCMotorSim rollerSim; private final SparkFlex flex; private final SparkClosedLoopController controller; private final SparkFlexSim flexSim; - public RollerIOSimSpark() { - flex = new SparkFlex(CAN2.kicker, MotorType.kBrushless); + public RollerIOSimSpark(RollerConfig rollerConfig) { + flex = new SparkFlex(rollerConfig.port, MotorType.kBrushless); controller = flex.getClosedLoopController(); var config = new SparkFlexConfig(); config - .inverted(false) + .inverted(rollerConfig.inverted) .idleMode(IdleMode.kBrake) .smartCurrentLimit(NEOVortexConstants.kDefaultSupplyCurrentLimit) .voltageCompensation(RobotConstants.kNominalVoltage); @@ -49,12 +58,12 @@ public RollerIOSimSpark() { .positionConversionFactor(encoderPositionFactor) .velocityConversionFactor(encoderVelocityFactor); - config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(kPSim, 0.0, 0.0); + config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(KP, 0.0, KD); flex.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); flexSim = new SparkFlexSim(flex, gearbox); - kickerSim = + rollerSim = new DCMotorSim( LinearSystemId.createDCMotorSystem(gearbox, KICKER_MOI_KG_M2, motorReduction), gearbox); } @@ -63,13 +72,13 @@ public RollerIOSimSpark() { public void updateInputs(RollerIOInputs inputs) { // Update simulation state double busVoltage = RoboRioSim.getVInVoltage(); - kickerSim.setInput(flexSim.getAppliedOutput() * busVoltage); - kickerSim.update(Robot.defaultPeriodSecs); - flexSim.iterate(kickerSim.getAngularVelocityRadPerSec(), busVoltage, Robot.defaultPeriodSecs); + 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() * radius.in(Meters); + inputs.velocityMetersPerSec = flexSim.getVelocity() * rollerRadius.in(Meters); inputs.appliedVolts = flexSim.getAppliedOutput() * flexSim.getBusVoltage(); inputs.currentAmps = Math.abs(flexSim.getMotorCurrent()); } @@ -86,7 +95,7 @@ public void setVelocity(LinearVelocity tangentialVelocity) { * tangentialVelocity.in(MetersPerSecond) / maxTangentialVelocity.in(MetersPerSecond); controller.setSetpoint( - tangentialVelocity.in(MetersPerSecond) / radius.in(Meters), + tangentialVelocity.in(MetersPerSecond) / rollerRadius.in(Meters), ControlType.kVelocity, ClosedLoopSlot.kSlot0, feedforwardVolts); diff --git a/src/main/java/frc/robot/subsystems/intake/RollerIOSimTalonFX.java b/src/main/java/frc/robot/subsystems/intake/RollerIOSimTalonFX.java index 1739699..87096d0 100644 --- a/src/main/java/frc/robot/subsystems/intake/RollerIOSimTalonFX.java +++ b/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; diff --git a/src/main/java/frc/robot/subsystems/intake/RollerIOSpark.java b/src/main/java/frc/robot/subsystems/intake/RollerIOSpark.java index 0776e01..48357e1 100644 --- a/src/main/java/frc/robot/subsystems/intake/RollerIOSpark.java +++ b/src/main/java/frc/robot/subsystems/intake/RollerIOSpark.java @@ -1,8 +1,6 @@ package frc.robot.subsystems.intake; -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static frc.robot.subsystems.intake.IntakeConstants.RollerConfig; +import static edu.wpi.first.units.Units.*; import static frc.robot.subsystems.intake.IntakeConstants.RollerConstants.*; import static frc.robot.util.SparkUtil.*; @@ -21,19 +19,24 @@ 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; - private final double KP = 0.11; - private final double KD = 0.0; - public RollerIOSpark(RollerConfig rollerConfig) { flex = new SparkFlex(rollerConfig.port, MotorType.kBrushless); encoder = flex.getEncoder(); @@ -66,7 +69,6 @@ public RollerIOSpark(RollerConfig rollerConfig) { @Override public void updateInputs(RollerIOInputs inputs) { - inputs.connected = sparkInputs.isConnected(); inputs.velocityMetersPerSec = sparkInputs.getVelocity() * rollerRadius.in(Meters); inputs.appliedVolts = sparkInputs.getAppliedVolts(); diff --git a/src/main/java/frc/robot/subsystems/intake/RollerIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/RollerIOTalonFX.java index 7619a05..05cc765 100644 --- a/src/main/java/frc/robot/subsystems/intake/RollerIOTalonFX.java +++ b/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); From a70e7904ae70987f0ac9718463693f29d17d05aa Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Tue, 21 Apr 2026 18:27:05 -0400 Subject: [PATCH 5/6] use spark in sim --- src/main/java/frc/robot/Robot.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e4c163f..3ecd6d4 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -72,6 +72,7 @@ import frc.robot.subsystems.intake.IntakeConstants; import frc.robot.subsystems.intake.IntakeConstants.RollerConstants; import frc.robot.subsystems.intake.RollerIO; +import frc.robot.subsystems.intake.RollerIOSimSpark; import frc.robot.subsystems.intake.RollerIOSimTalonFX; import frc.robot.subsystems.intake.RollerIOSpark; import frc.robot.subsystems.launcher.FlywheelIO; @@ -245,8 +246,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)); From 476d071a6d5e9ae2d7691744ac5f6a81a1ac680d Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Tue, 21 Apr 2026 18:28:17 -0400 Subject: [PATCH 6/6] spotless --- src/main/java/frc/robot/Robot.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 3ecd6d4..3a232e6 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -73,7 +73,6 @@ import frc.robot.subsystems.intake.IntakeConstants.RollerConstants; import frc.robot.subsystems.intake.RollerIO; import frc.robot.subsystems.intake.RollerIOSimSpark; -import frc.robot.subsystems.intake.RollerIOSimTalonFX; import frc.robot.subsystems.intake.RollerIOSpark; import frc.robot.subsystems.launcher.FlywheelIO; import frc.robot.subsystems.launcher.FlywheelIOSimTalonFX;