From 79cb6d55528949d928c8cdf665f930711c066e00 Mon Sep 17 00:00:00 2001 From: DylanTaylor29 <132412179+DylanTaylor29@users.noreply.github.com> Date: Tue, 10 Feb 2026 19:05:23 -0500 Subject: [PATCH 1/9] renamed files --- src/main/java/frc/robot/Robot.java | 38 +++++++++---------- .../{ModuleIOSim.java => ModuleIOSimWPI.java} | 4 +- ...KickerIOSim.java => KickerIOSimSpark.java} | 4 +- ...xerIOSim.java => SpindexerIOSimSpark.java} | 4 +- ...Sim.java => IntakeRollerIOSimTalonFX.java} | 4 +- ...ywheelIOSim.java => FlywheelIOSimWPI.java} | 4 +- .../{HoodIOSim.java => HoodIOSimSpark.java} | 4 +- ...SimHardwareless.java => HoodIOSimWPI.java} | 4 +- ...TurretIOSim.java => TurretIOSimSpark.java} | 4 +- 9 files changed, 35 insertions(+), 35 deletions(-) rename src/main/java/frc/robot/subsystems/drive/{ModuleIOSim.java => ModuleIOSimWPI.java} (98%) rename src/main/java/frc/robot/subsystems/feeder/{KickerIOSim.java => KickerIOSimSpark.java} (97%) rename src/main/java/frc/robot/subsystems/feeder/{SpindexerIOSim.java => SpindexerIOSimSpark.java} (97%) rename src/main/java/frc/robot/subsystems/intake/{IntakeRollerIOSim.java => IntakeRollerIOSimTalonFX.java} (97%) rename src/main/java/frc/robot/subsystems/launcher/{FlywheelIOSim.java => FlywheelIOSimWPI.java} (96%) rename src/main/java/frc/robot/subsystems/launcher/{HoodIOSim.java => HoodIOSimSpark.java} (98%) rename src/main/java/frc/robot/subsystems/launcher/{HoodIOSimHardwareless.java => HoodIOSimWPI.java} (96%) rename src/main/java/frc/robot/subsystems/launcher/{TurretIOSim.java => TurretIOSimSpark.java} (98%) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d3cc206..2a4eee1 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -32,24 +32,24 @@ import frc.robot.subsystems.drive.GyroIO; import frc.robot.subsystems.drive.GyroIOBoron; import frc.robot.subsystems.drive.ModuleIO; -import frc.robot.subsystems.drive.ModuleIOSim; +import frc.robot.subsystems.drive.ModuleIOSimWPI; import frc.robot.subsystems.drive.ModuleIOTalonFX; import frc.robot.subsystems.feeder.Feeder; import frc.robot.subsystems.feeder.KickerIO; -import frc.robot.subsystems.feeder.KickerIOSim; +import frc.robot.subsystems.feeder.KickerIOSimSpark; import frc.robot.subsystems.feeder.SpindexerIO; -import frc.robot.subsystems.feeder.SpindexerIOSim; +import frc.robot.subsystems.feeder.SpindexerIOSimSpark; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.intake.IntakeRollerIO; -import frc.robot.subsystems.intake.IntakeRollerIOSim; +import frc.robot.subsystems.intake.IntakeRollerIOSimTalonFX; import frc.robot.subsystems.launcher.FlywheelIO; -import frc.robot.subsystems.launcher.FlywheelIOSim; +import frc.robot.subsystems.launcher.FlywheelIOSimWPI; import frc.robot.subsystems.launcher.HoodIO; -import frc.robot.subsystems.launcher.HoodIOSim; -import frc.robot.subsystems.launcher.HoodIOSimHardwareless; +import frc.robot.subsystems.launcher.HoodIOSimSpark; +import frc.robot.subsystems.launcher.HoodIOSimWPI; import frc.robot.subsystems.launcher.Launcher; import frc.robot.subsystems.launcher.TurretIO; -import frc.robot.subsystems.launcher.TurretIOSim; +import frc.robot.subsystems.launcher.TurretIOSimSpark; import frc.robot.subsystems.launcher.TurretIOSpark; import frc.robot.subsystems.vision.Vision; import frc.robot.subsystems.vision.VisionIO; @@ -129,8 +129,8 @@ public Robot() { drive::getPose, drive::getRobotRelativeChassisSpeeds, new TurretIOSpark(), - new FlywheelIOSim(), - new HoodIOSimHardwareless()); + new FlywheelIOSimWPI(), + new HoodIOSimWPI()); intake = new Intake(new IntakeRollerIO() {}); feeder = new Feeder(new SpindexerIO() {}, new KickerIO() {}); break; @@ -143,10 +143,10 @@ public Robot() { drive = new Drive( new GyroIO() {}, - new ModuleIOSim(DriveConstants.FrontLeft), - new ModuleIOSim(DriveConstants.FrontRight), - new ModuleIOSim(DriveConstants.BackLeft), - new ModuleIOSim(DriveConstants.BackRight)); + new ModuleIOSimWPI(DriveConstants.FrontLeft), + new ModuleIOSimWPI(DriveConstants.FrontRight), + new ModuleIOSimWPI(DriveConstants.BackLeft), + new ModuleIOSimWPI(DriveConstants.BackRight)); vision = new Vision( drive::addVisionMeasurement, @@ -163,11 +163,11 @@ public Robot() { new Launcher( drive::getPose, drive::getRobotRelativeChassisSpeeds, - new TurretIOSim(), - new FlywheelIOSim(), - new HoodIOSim()); - feeder = new Feeder(new SpindexerIOSim(), new KickerIOSim()); - intake = new Intake(new IntakeRollerIOSim()); + new TurretIOSimSpark(), + new FlywheelIOSimWPI(), + new HoodIOSimSpark()); + feeder = new Feeder(new SpindexerIOSimSpark(), new KickerIOSimSpark()); + intake = new Intake(new IntakeRollerIOSimTalonFX()); break; case REPLAY: // Replaying a log diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSimWPI.java similarity index 98% rename from src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java rename to src/main/java/frc/robot/subsystems/drive/ModuleIOSimWPI.java index 9331cc5..b190552 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSimWPI.java @@ -26,7 +26,7 @@ import edu.wpi.first.wpilibj.simulation.DCMotorSim; /** Physics sim implementation of module IO. */ -public class ModuleIOSim implements ModuleIO { +public class ModuleIOSimWPI implements ModuleIO { // TunerConstants doesn't support separate sim constants, so they are declared locally private static final double DRIVE_KP = 0.05; private static final double DRIVE_KD = 0.0; @@ -50,7 +50,7 @@ public class ModuleIOSim implements ModuleIO { private double driveAppliedVolts = 0.0; private double turnAppliedVolts = 0.0; - public ModuleIOSim( + public ModuleIOSimWPI( SwerveModuleConstants constants) { // Create drive and turn sim models diff --git a/src/main/java/frc/robot/subsystems/feeder/KickerIOSim.java b/src/main/java/frc/robot/subsystems/feeder/KickerIOSimSpark.java similarity index 97% rename from src/main/java/frc/robot/subsystems/feeder/KickerIOSim.java rename to src/main/java/frc/robot/subsystems/feeder/KickerIOSimSpark.java index b9e8ca1..f633f73 100644 --- a/src/main/java/frc/robot/subsystems/feeder/KickerIOSim.java +++ b/src/main/java/frc/robot/subsystems/feeder/KickerIOSimSpark.java @@ -23,7 +23,7 @@ import frc.robot.Constants.RobotConstants; import frc.robot.Robot; -public class KickerIOSim implements KickerIO { +public class KickerIOSimSpark implements KickerIO { private final DCMotorSim kickerSim; @@ -31,7 +31,7 @@ public class KickerIOSim implements KickerIO { private final SparkClosedLoopController controller; private final SparkFlexSim flexSim; - public KickerIOSim() { + public KickerIOSimSpark() { flex = new SparkFlex(CAN2.kicker, MotorType.kBrushless); controller = flex.getClosedLoopController(); diff --git a/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSim.java b/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSimSpark.java similarity index 97% rename from src/main/java/frc/robot/subsystems/feeder/SpindexerIOSim.java rename to src/main/java/frc/robot/subsystems/feeder/SpindexerIOSimSpark.java index 126dd8b..f8b3298 100644 --- a/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSim.java +++ b/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSimSpark.java @@ -23,7 +23,7 @@ import frc.robot.Constants.RobotConstants; import frc.robot.Robot; -public class SpindexerIOSim implements SpindexerIO { +public class SpindexerIOSimSpark implements SpindexerIO { private final DCMotorSim spindexerSim; @@ -31,7 +31,7 @@ public class SpindexerIOSim implements SpindexerIO { private final SparkClosedLoopController controller; private final SparkFlexSim flexSim; - public SpindexerIOSim() { + public SpindexerIOSimSpark() { flex = new SparkFlex(CAN2.spindexer, MotorType.kBrushless); controller = flex.getClosedLoopController(); diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOSimTalonFX.java similarity index 97% rename from src/main/java/frc/robot/subsystems/intake/IntakeRollerIOSim.java rename to src/main/java/frc/robot/subsystems/intake/IntakeRollerIOSimTalonFX.java index 18f3213..977abea 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOSimTalonFX.java @@ -27,7 +27,7 @@ import frc.robot.Constants.CANBusPorts.CAN2; import frc.robot.Robot; -public class IntakeRollerIOSim implements IntakeRollerIO { +public class IntakeRollerIOSimTalonFX implements IntakeRollerIO { private final DCMotorSim intakeRollerSim; @@ -43,7 +43,7 @@ public class IntakeRollerIOSim implements IntakeRollerIO { private final StatusSignal intakeAppliedVolts; private final StatusSignal intakeCurrent; - public IntakeRollerIOSim() { + public IntakeRollerIOSimTalonFX() { intakeMotor = new TalonFX(CAN2.intakeRoller, CAN2.bus); config = new TalonFXConfiguration(); config.MotorOutput.withInverted(InvertedValue.CounterClockwise_Positive) diff --git a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimWPI.java similarity index 96% rename from src/main/java/frc/robot/subsystems/launcher/FlywheelIOSim.java rename to src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimWPI.java index 1f60aa5..37b79f9 100644 --- a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSim.java +++ b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimWPI.java @@ -11,7 +11,7 @@ import frc.robot.Constants.RobotConstants; import frc.robot.Robot; -public class FlywheelIOSim implements FlywheelIO { +public class FlywheelIOSimWPI implements FlywheelIO { private final DCMotorSim flywheelSim; private boolean closedLoop = false; @@ -19,7 +19,7 @@ public class FlywheelIOSim implements FlywheelIO { private double appliedVolts = 0.0; private double feedforwardVolts = 0.0; - public FlywheelIOSim() { + public FlywheelIOSimWPI() { flywheelSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(gearbox, 0.004, motorReduction), gearbox); } diff --git a/src/main/java/frc/robot/subsystems/launcher/HoodIOSim.java b/src/main/java/frc/robot/subsystems/launcher/HoodIOSimSpark.java similarity index 98% rename from src/main/java/frc/robot/subsystems/launcher/HoodIOSim.java rename to src/main/java/frc/robot/subsystems/launcher/HoodIOSimSpark.java index 05b99ae..5944cc4 100644 --- a/src/main/java/frc/robot/subsystems/launcher/HoodIOSim.java +++ b/src/main/java/frc/robot/subsystems/launcher/HoodIOSimSpark.java @@ -24,7 +24,7 @@ import frc.robot.Constants.RobotConstants; import frc.robot.Robot; -public class HoodIOSim implements HoodIO { +public class HoodIOSimSpark implements HoodIO { private final DCMotorSim hoodSim; @@ -32,7 +32,7 @@ public class HoodIOSim implements HoodIO { private final SparkClosedLoopController controller; private final SparkMaxSim maxSim; - public HoodIOSim() { + public HoodIOSimSpark() { max = new SparkMax(CAN2.hood, MotorType.kBrushless); controller = max.getClosedLoopController(); diff --git a/src/main/java/frc/robot/subsystems/launcher/HoodIOSimHardwareless.java b/src/main/java/frc/robot/subsystems/launcher/HoodIOSimWPI.java similarity index 96% rename from src/main/java/frc/robot/subsystems/launcher/HoodIOSimHardwareless.java rename to src/main/java/frc/robot/subsystems/launcher/HoodIOSimWPI.java index 4543566..918cbd5 100644 --- a/src/main/java/frc/robot/subsystems/launcher/HoodIOSimHardwareless.java +++ b/src/main/java/frc/robot/subsystems/launcher/HoodIOSimWPI.java @@ -12,7 +12,7 @@ import frc.robot.Constants.RobotConstants; import frc.robot.Robot; -public class HoodIOSimHardwareless implements HoodIO { +public class HoodIOSimWPI implements HoodIO { private final DCMotorSim hoodSim; @@ -21,7 +21,7 @@ public class HoodIOSimHardwareless implements HoodIO { private double appliedVolts = 0.0; private double feedforwardVolts = 0.0; - public HoodIOSimHardwareless() { + public HoodIOSimWPI() { hoodSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(gearbox, 0.004, motorReduction), gearbox); diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIOSim.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java similarity index 98% rename from src/main/java/frc/robot/subsystems/launcher/TurretIOSim.java rename to src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java index e906a2f..4cc2f55 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIOSim.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java @@ -25,7 +25,7 @@ import frc.robot.Constants.RobotConstants; import frc.robot.Robot; -public class TurretIOSim implements TurretIO { +public class TurretIOSimSpark implements TurretIO { private final DCMotorSim turnSim; @@ -33,7 +33,7 @@ public class TurretIOSim implements TurretIO { private final SparkClosedLoopController controller; private final SparkMaxSim turnSparkSim; - public TurretIOSim() { + public TurretIOSimSpark() { turnSpark = new SparkMax(CAN2.turret, MotorType.kBrushless); controller = turnSpark.getClosedLoopController(); From 2708c7536065eb33e7873ddb6ea9a1c730664d42 Mon Sep 17 00:00:00 2001 From: DylanTaylor29 <132412179+DylanTaylor29@users.noreply.github.com> Date: Tue, 10 Feb 2026 19:18:12 -0500 Subject: [PATCH 2/9] added flywheelIOSimTalonFX --- .../subsystems/intake/IntakeConstants.java | 4 +- .../intake/IntakeRollerIOSimTalonFX.java | 4 +- .../launcher/FlywheelIOSimTalonFX.java | 131 ++++++++++++++++++ 3 files changed, 135 insertions(+), 4 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimTalonFX.java diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 6c88b94..cde3ec0 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -26,9 +26,9 @@ public class IntakeRoller { .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); // motor controller - public static final double motorReudction = 1.0; + public static final double motorReduction = 1.0; public static final AngularVelocity maxAngularVelocity = - KrakenX60Constants.kFreeSpeed.div(motorReudction); + KrakenX60Constants.kFreeSpeed.div(motorReduction); // simulation public static final DCMotor gearbox = DCMotor.getKrakenX60(1); diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOSimTalonFX.java b/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOSimTalonFX.java index 977abea..ff7e3be 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOSimTalonFX.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOSimTalonFX.java @@ -74,8 +74,8 @@ public void updateInputs(IntakeRollerIOInputs inputs) { intakeRollerSim.setInput(intakeMotorSim.getMotorVoltageMeasure().in(Volts)); intakeRollerSim.update(Robot.defaultPeriodSecs); intakeMotorSim.setRawRotorPosition( - intakeRollerSim.getAngularPositionRotations() * motorReudction); - intakeMotorSim.setRotorVelocity(intakeRollerSim.getAngularVelocityRPM() * motorReudction); + intakeRollerSim.getAngularPositionRotations() * motorReduction); + intakeMotorSim.setRotorVelocity(intakeRollerSim.getAngularVelocityRPM() * motorReduction); BaseStatusSignal.refreshAll(intakeVelocity, intakeAppliedVolts, intakeCurrent).isOK(); diff --git a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimTalonFX.java b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimTalonFX.java new file mode 100644 index 0000000..d1bbd70 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimTalonFX.java @@ -0,0 +1,131 @@ +package frc.robot.subsystems.launcher; + +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.Volts; +import static frc.robot.subsystems.launcher.LauncherConstants.FlywheelConstants.*; +import static frc.robot.util.PhoenixUtil.tryUntilOk; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.MotorAlignmentValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.sim.ChassisReference; +import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; + +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import frc.robot.Constants.CANBusPorts.CAN2; +import frc.robot.Robot; + +public class FlywheelIOSimTalonFX implements FlywheelIO { + private final DCMotorSim flywheelSim; + + private final TalonFX flywheelLeaderTalon; + private final TalonFX flywheelFollowerTalon; + private final TalonFXConfiguration config; + private final Debouncer flywheelConnectedDebounce = + new Debouncer(0.5, Debouncer.DebounceType.kFalling); + + // Voltage control requests + private final VoltageOut voltageRequest = new VoltageOut(0); + // private final VelocityVoltage velocityVoltageRequest = new VelocityVoltage(0.0); + + // Torque-current control requests + // private final TorqueCurrentFOC torqueCurrentRequest = new TorqueCurrentFOC(0); + private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest = + new VelocityTorqueCurrentFOC(0.0); + + // Inputs from flywheel motor + private final StatusSignal flywheelVelocity; + private final StatusSignal flywheelAppliedVolts; + private final StatusSignal flywheelCurrent; + + public FlywheelIOSimTalonFX() { + flywheelLeaderTalon = new TalonFX(CAN2.flywheelLeader, CAN2.bus); + flywheelFollowerTalon = new TalonFX(CAN2.flywheelFollower, CAN2.bus); + // Configuration + config = new TalonFXConfiguration(); + config.MotorOutput.withInverted(InvertedValue.CounterClockwise_Positive) + .withNeutralMode(NeutralModeValue.Brake); + config.Slot0 = flywheelGains; + tryUntilOk(5, () -> flywheelLeaderTalon.getConfigurator().apply(config, 0.25)); + tryUntilOk(5, () -> flywheelFollowerTalon.getConfigurator().apply(config, 0.25)); + + var flywheelMotorSim = flywheelLeaderTalon.getSimState(); + flywheelMotorSim.Orientation = ChassisReference.Clockwise_Positive; + flywheelMotorSim.setMotorType(MotorType.KrakenX60); + + flywheelSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(gearbox, 0.004, motorReduction), gearbox); + + flywheelVelocity = flywheelLeaderTalon.getVelocity(); + flywheelAppliedVolts = flywheelLeaderTalon.getMotorVoltage(); + flywheelCurrent = flywheelLeaderTalon.getStatorCurrent(); + + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, + flywheelVelocity, + flywheelAppliedVolts, + flywheelCurrent, + flywheelVelocity, + flywheelAppliedVolts, + flywheelCurrent); + + flywheelFollowerTalon.setControl( + new Follower(CAN2.flywheelLeader, MotorAlignmentValue.Opposed)); + } + + @Override + public void updateInputs(FlywheelIOInputs inputs) { + // Update simulation state + var flywheelMotorSim = flywheelLeaderTalon.getSimState(); + flywheelMotorSim.setSupplyVoltage(RobotController.getBatteryVoltage()); + flywheelSim.setInput(flywheelMotorSim.getMotorVoltageMeasure().in(Volts)); + flywheelSim.update(Robot.defaultPeriodSecs); + flywheelMotorSim.setRawRotorPosition( + flywheelSim.getAngularPositionRotations() * motorReduction); + flywheelMotorSim.setRotorVelocity(flywheelSim.getAngularVelocityRPM() * motorReduction); + + BaseStatusSignal.refreshAll( + flywheelVelocity, + flywheelAppliedVolts, + flywheelCurrent, + flywheelVelocity, + flywheelAppliedVolts, + flywheelCurrent) + .isOK(); + + inputs.appliedVolts = flywheelAppliedVolts.getValueAsDouble(); + inputs.connected = + flywheelConnectedDebounce.calculate( + flywheelAppliedVolts.getStatus().isOK() + && flywheelCurrent.getStatus().isOK() + && flywheelVelocity.getStatus().isOK()); + inputs.currentAmps = flywheelCurrent.getValueAsDouble(); + inputs.velocityRadPerSec = + Units.rotationsToRadians(flywheelVelocity.getValueAsDouble()) / gearboxRatio; + } + + @Override + public void setOpenLoop(double output) { + // flywheelLeaderTalon.setControl(voltageRequest.withOutput(output)); + flywheelSim.setInputVoltage(output); + } + + @Override + public void setVelocity(AngularVelocity angularVelocity) { + // flywheelLeaderTalon.setControl(velocityTorqueCurrentRequest.withVelocity(angularVelocity)); + flywheelSim.setState(0.0, angularVelocity.in(RadiansPerSecond)); + } +} From ec6509d516206d12f6b9d8610a3c961cb262e101 Mon Sep 17 00:00:00 2001 From: DylanTaylor29 <132412179+DylanTaylor29@users.noreply.github.com> Date: Tue, 10 Feb 2026 19:25:09 -0500 Subject: [PATCH 3/9] added flywheelSimIOTalonFX to robot.java --- src/main/java/frc/robot/Robot.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 2a4eee1..475716f 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -43,6 +43,7 @@ import frc.robot.subsystems.intake.IntakeRollerIO; import frc.robot.subsystems.intake.IntakeRollerIOSimTalonFX; import frc.robot.subsystems.launcher.FlywheelIO; +import frc.robot.subsystems.launcher.FlywheelIOSimTalonFX; import frc.robot.subsystems.launcher.FlywheelIOSimWPI; import frc.robot.subsystems.launcher.HoodIO; import frc.robot.subsystems.launcher.HoodIOSimSpark; @@ -164,7 +165,7 @@ public Robot() { drive::getPose, drive::getRobotRelativeChassisSpeeds, new TurretIOSimSpark(), - new FlywheelIOSimWPI(), + new FlywheelIOSimTalonFX(), new HoodIOSimSpark()); feeder = new Feeder(new SpindexerIOSimSpark(), new KickerIOSimSpark()); intake = new Intake(new IntakeRollerIOSimTalonFX()); From 7e63f3252bc673f1b5d1d7ffe2b842488eafd0da Mon Sep 17 00:00:00 2001 From: DylanTaylor29 <132412179+DylanTaylor29@users.noreply.github.com> Date: Tue, 10 Feb 2026 19:38:43 -0500 Subject: [PATCH 4/9] moved some logging commands --- src/main/java/frc/robot/Robot.java | 13 +++++++------ .../subsystems/launcher/FlywheelIOSimTalonFX.java | 4 ++-- 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 475716f..6d96ecf 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -227,6 +227,13 @@ public Robot() { feeder.setDefaultCommand(Commands.run(feeder::spinForward, feeder).withName("Spin forward")); intake.setDefaultCommand(Commands.run(intake::intake, intake).withName("Intake Command")); + + SmartDashboard.putData(CommandScheduler.getInstance()); + SmartDashboard.putData(drive); + SmartDashboard.putData(vision); + SmartDashboard.putData(launcher); + SmartDashboard.putData(feeder); + SmartDashboard.putData(intake); } /** This function is called periodically during all modes. */ @@ -242,12 +249,6 @@ public void robotPeriodic() { // This must be called from the robot's periodic block in order for anything in // the Command-based framework to work. CommandScheduler.getInstance().run(); - SmartDashboard.putData(CommandScheduler.getInstance()); - SmartDashboard.putData(drive); - SmartDashboard.putData(vision); - SmartDashboard.putData(launcher); - SmartDashboard.putData(feeder); - SmartDashboard.putData(intake); GameState.logValues(); diff --git a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimTalonFX.java b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimTalonFX.java index d1bbd70..0d04056 100644 --- a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimTalonFX.java +++ b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimTalonFX.java @@ -17,7 +17,6 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.sim.ChassisReference; import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; - import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.math.util.Units; @@ -67,7 +66,8 @@ public FlywheelIOSimTalonFX() { flywheelMotorSim.Orientation = ChassisReference.Clockwise_Positive; flywheelMotorSim.setMotorType(MotorType.KrakenX60); - flywheelSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(gearbox, 0.004, motorReduction), gearbox); + flywheelSim = + new DCMotorSim(LinearSystemId.createDCMotorSystem(gearbox, 0.004, motorReduction), gearbox); flywheelVelocity = flywheelLeaderTalon.getVelocity(); flywheelAppliedVolts = flywheelLeaderTalon.getMotorVoltage(); From 77b9fe9d4a69b4f4f7451d41bc0c07b950b2ab5a Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Tue, 10 Feb 2026 20:22:33 -0500 Subject: [PATCH 5/9] fix unit mismatch --- .../launcher/FlywheelIOSimTalonFX.java | 43 ++++++++----------- .../launcher/FlywheelIOTalonFX.java | 33 +++++++------- .../launcher/LauncherConstants.java | 1 - 3 files changed, 33 insertions(+), 44 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimTalonFX.java b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimTalonFX.java index 0d04056..15eb37a 100644 --- a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimTalonFX.java +++ b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimTalonFX.java @@ -1,7 +1,6 @@ package frc.robot.subsystems.launcher; import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.Volts; import static frc.robot.subsystems.launcher.LauncherConstants.FlywheelConstants.*; import static frc.robot.util.PhoenixUtil.tryUntilOk; @@ -10,6 +9,7 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; +import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; @@ -19,7 +19,6 @@ import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.system.plant.LinearSystemId; -import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; @@ -39,7 +38,7 @@ public class FlywheelIOSimTalonFX implements FlywheelIO { // Voltage control requests private final VoltageOut voltageRequest = new VoltageOut(0); - // private final VelocityVoltage velocityVoltageRequest = new VelocityVoltage(0.0); + private final VelocityVoltage velocityVoltageRequest = new VelocityVoltage(0.0); // Torque-current control requests // private final TorqueCurrentFOC torqueCurrentRequest = new TorqueCurrentFOC(0); @@ -88,44 +87,38 @@ public FlywheelIOSimTalonFX() { @Override public void updateInputs(FlywheelIOInputs inputs) { + inputs.connected = + flywheelConnectedDebounce.calculate( + BaseStatusSignal.refreshAll( + flywheelVelocity, + flywheelAppliedVolts, + flywheelCurrent, + flywheelVelocity, + flywheelAppliedVolts, + flywheelCurrent) + .isOK()); + // Update simulation state var flywheelMotorSim = flywheelLeaderTalon.getSimState(); flywheelMotorSim.setSupplyVoltage(RobotController.getBatteryVoltage()); - flywheelSim.setInput(flywheelMotorSim.getMotorVoltageMeasure().in(Volts)); + flywheelSim.setInput(flywheelMotorSim.getMotorVoltage()); flywheelSim.update(Robot.defaultPeriodSecs); flywheelMotorSim.setRawRotorPosition( flywheelSim.getAngularPositionRotations() * motorReduction); - flywheelMotorSim.setRotorVelocity(flywheelSim.getAngularVelocityRPM() * motorReduction); - - BaseStatusSignal.refreshAll( - flywheelVelocity, - flywheelAppliedVolts, - flywheelCurrent, - flywheelVelocity, - flywheelAppliedVolts, - flywheelCurrent) - .isOK(); + flywheelMotorSim.setRotorVelocity(flywheelSim.getAngularVelocity().times(motorReduction)); inputs.appliedVolts = flywheelAppliedVolts.getValueAsDouble(); - inputs.connected = - flywheelConnectedDebounce.calculate( - flywheelAppliedVolts.getStatus().isOK() - && flywheelCurrent.getStatus().isOK() - && flywheelVelocity.getStatus().isOK()); inputs.currentAmps = flywheelCurrent.getValueAsDouble(); - inputs.velocityRadPerSec = - Units.rotationsToRadians(flywheelVelocity.getValueAsDouble()) / gearboxRatio; + inputs.velocityRadPerSec = flywheelVelocity.getValue().in(RadiansPerSecond) / motorReduction; } @Override public void setOpenLoop(double output) { - // flywheelLeaderTalon.setControl(voltageRequest.withOutput(output)); - flywheelSim.setInputVoltage(output); + flywheelLeaderTalon.setControl(voltageRequest.withOutput(output)); } @Override public void setVelocity(AngularVelocity angularVelocity) { - // flywheelLeaderTalon.setControl(velocityTorqueCurrentRequest.withVelocity(angularVelocity)); - flywheelSim.setState(0.0, angularVelocity.in(RadiansPerSecond)); + flywheelLeaderTalon.setControl(velocityVoltageRequest.withVelocity(angularVelocity)); } } diff --git a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java index 28617d4..95359fd 100644 --- a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.launcher; +import static edu.wpi.first.units.Units.RadiansPerSecond; import static frc.robot.subsystems.launcher.LauncherConstants.FlywheelConstants.*; import static frc.robot.util.PhoenixUtil.tryUntilOk; @@ -8,13 +9,13 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; +import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.MotorAlignmentValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; @@ -29,7 +30,7 @@ public class FlywheelIOTalonFX implements FlywheelIO { // Voltage control requests private final VoltageOut voltageRequest = new VoltageOut(0); - // private final VelocityVoltage velocityVoltageRequest = new VelocityVoltage(0.0); + private final VelocityVoltage velocityVoltageRequest = new VelocityVoltage(0.0); // Torque-current control requests // private final TorqueCurrentFOC torqueCurrentRequest = new TorqueCurrentFOC(0); @@ -71,24 +72,20 @@ public FlywheelIOTalonFX() { @Override public void updateInputs(FlywheelIOInputs inputs) { - BaseStatusSignal.refreshAll( - flywheelVelocity, - flywheelAppliedVolts, - flywheelCurrent, - flywheelVelocity, - flywheelAppliedVolts, - flywheelCurrent) - .isOK(); - - inputs.appliedVolts = flywheelAppliedVolts.getValueAsDouble(); inputs.connected = flywheelConnectedDebounce.calculate( - flywheelAppliedVolts.getStatus().isOK() - && flywheelCurrent.getStatus().isOK() - && flywheelVelocity.getStatus().isOK()); + BaseStatusSignal.refreshAll( + flywheelVelocity, + flywheelAppliedVolts, + flywheelCurrent, + flywheelVelocity, + flywheelAppliedVolts, + flywheelCurrent) + .isOK()); + + inputs.appliedVolts = flywheelAppliedVolts.getValueAsDouble(); inputs.currentAmps = flywheelCurrent.getValueAsDouble(); - inputs.velocityRadPerSec = - Units.rotationsToRadians(flywheelVelocity.getValueAsDouble()) / gearboxRatio; + inputs.velocityRadPerSec = flywheelVelocity.getValue().in(RadiansPerSecond) / motorReduction; } @Override @@ -98,6 +95,6 @@ public void setOpenLoop(double output) { @Override public void setVelocity(AngularVelocity angularVelocity) { - flywheelLeaderTalon.setControl(velocityTorqueCurrentRequest.withVelocity(angularVelocity)); + flywheelLeaderTalon.setControl(velocityVoltageRequest.withVelocity(angularVelocity)); } } diff --git a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java index 8385cbc..98c1561 100644 --- a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java +++ b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java @@ -93,7 +93,6 @@ public static final class TurretConstants { public static final class FlywheelConstants { public static final Distance wheelRadius = Inches.of(1.5); - public static final double gearboxRatio = 1.0; // Velocity Controller public static final Slot0Configs flywheelGains = From f2ce39c4b3250b2a4fd9b356943c2ded625aefe2 Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Thu, 12 Feb 2026 18:10:09 -0500 Subject: [PATCH 6/9] bring intake roller sim up to par with flywheel --- src/main/java/frc/robot/Robot.java | 3 +- .../frc/robot/subsystems/intake/Intake.java | 2 +- .../subsystems/intake/IntakeConstants.java | 7 ++-- .../intake/IntakeRollerIOSimTalonFX.java | 41 +++++++++++-------- .../launcher/FlywheelIOSimTalonFX.java | 8 +--- .../launcher/FlywheelIOTalonFX.java | 8 +--- 6 files changed, 30 insertions(+), 39 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 6d96ecf..a5684c3 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -225,8 +225,7 @@ public Robot() { .withName("Aim at hub")); feeder.setDefaultCommand(Commands.run(feeder::spinForward, feeder).withName("Spin forward")); - - intake.setDefaultCommand(Commands.run(intake::intake, intake).withName("Intake Command")); + intake.setDefaultCommand(Commands.run(intake::intakeFuel, intake).withName("Intake fuel")); SmartDashboard.putData(CommandScheduler.getInstance()); SmartDashboard.putData(drive); diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index c06ab12..adfb677 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -33,7 +33,7 @@ public void stop() { io.setOpenLoop(0.0); } - public void intake() { + public void intakeFuel() { io.setVelocity(MetersPerSecond.of(1.0)); } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index cde3ec0..d632d81 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -11,15 +11,14 @@ public class IntakeConstants { public class IntakeRoller { - public static final double gearboxRatio = 1.0; - public static final Distance rollerRadius = Inches.of(3.0); + public static final Distance rollerRadius = Inches.of(0.75); // gains public static final Slot0Configs intakeGains = new Slot0Configs() - .withKP(1.0) + .withKP(0.1) .withKI(0.0) - .withKD(1.0) + .withKD(0.05) .withKS(0.0) .withKV(0.0) .withKA(0.0) diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOSimTalonFX.java b/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOSimTalonFX.java index ff7e3be..f02670d 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOSimTalonFX.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOSimTalonFX.java @@ -1,22 +1,20 @@ package frc.robot.subsystems.intake; -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Volts; +import static edu.wpi.first.units.Units.*; import static frc.robot.subsystems.intake.IntakeConstants.IntakeRoller.*; -import static frc.robot.subsystems.launcher.LauncherConstants.TurretConstants.motorReduction; import static frc.robot.util.PhoenixUtil.tryUntilOk; import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; +import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.sim.ChassisReference; import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; +import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; @@ -33,10 +31,12 @@ public class IntakeRollerIOSimTalonFX implements IntakeRollerIO { private final TalonFX intakeMotor; private final TalonFXConfiguration config; + private final Debouncer connectedDebounce = new Debouncer(0.5, Debouncer.DebounceType.kFalling); private final VoltageOut voltageRequest = new VoltageOut(0); - private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest = - new VelocityTorqueCurrentFOC(0.0); + private final VelocityVoltage velocityVoltageRequest = new VelocityVoltage(0.0); + // private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest = + // new VelocityTorqueCurrentFOC(0.0); // Inputs from intake motor private final StatusSignal intakeVelocity; @@ -56,7 +56,8 @@ public IntakeRollerIOSimTalonFX() { intakeMotorSim.setMotorType(MotorType.KrakenX60); intakeRollerSim = - new DCMotorSim(LinearSystemId.createDCMotorSystem(gearbox, 0.004, motorReduction), gearbox); + new DCMotorSim( + LinearSystemId.createDCMotorSystem(gearbox, 0.0005, motorReduction), gearbox); intakeVelocity = intakeMotor.getVelocity(); intakeAppliedVolts = intakeMotor.getMotorVoltage(); @@ -68,33 +69,37 @@ public IntakeRollerIOSimTalonFX() { @Override public void updateInputs(IntakeRollerIOInputs inputs) { + inputs.connected = + connectedDebounce.calculate( + BaseStatusSignal.refreshAll(intakeVelocity, intakeAppliedVolts, intakeCurrent).isOK()); + // Update simulation state var intakeMotorSim = intakeMotor.getSimState(); intakeMotorSim.setSupplyVoltage(RobotController.getBatteryVoltage()); - intakeRollerSim.setInput(intakeMotorSim.getMotorVoltageMeasure().in(Volts)); + intakeRollerSim.setInput(intakeMotorSim.getMotorVoltage()); intakeRollerSim.update(Robot.defaultPeriodSecs); intakeMotorSim.setRawRotorPosition( intakeRollerSim.getAngularPositionRotations() * motorReduction); - intakeMotorSim.setRotorVelocity(intakeRollerSim.getAngularVelocityRPM() * motorReduction); - - BaseStatusSignal.refreshAll(intakeVelocity, intakeAppliedVolts, intakeCurrent).isOK(); + intakeMotorSim.setRotorVelocity(intakeRollerSim.getAngularVelocity().times(motorReduction)); inputs.appliedVolts = intakeAppliedVolts.getValueAsDouble(); - inputs.connected = intakeMotor.isConnected(); inputs.currentAmps = intakeCurrent.getValueAsDouble(); inputs.velocityMetersPerSec = - intakeVelocity.getValueAsDouble() * rollerRadius.in(Meters) * (2 * Math.PI); + intakeVelocity.getValue().in(RadiansPerSecond) * rollerRadius.in(Meters) / motorReduction; } @Override public void setOpenLoop(double output) { - // intakeMotor.setControl(voltageRequest.withOutput(output)); - intakeRollerSim.setInputVoltage(output); + intakeMotor.setControl(voltageRequest.withOutput(output)); } @Override public void setVelocity(LinearVelocity tangentialVelocity) { - // intakeMotor.setControl(velocityTorqueCurrentRequest.withVelocity(angularVelocity)); - intakeRollerSim.setState(0.0, tangentialVelocity.in(MetersPerSecond)); + intakeMotor.setControl( + velocityVoltageRequest.withVelocity( + RadiansPerSecond.of( + tangentialVelocity.in(MetersPerSecond) + * motorReduction + / rollerRadius.in(Meters)))); } } diff --git a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimTalonFX.java b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimTalonFX.java index 15eb37a..faedeb3 100644 --- a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimTalonFX.java +++ b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimTalonFX.java @@ -89,13 +89,7 @@ public FlywheelIOSimTalonFX() { public void updateInputs(FlywheelIOInputs inputs) { inputs.connected = flywheelConnectedDebounce.calculate( - BaseStatusSignal.refreshAll( - flywheelVelocity, - flywheelAppliedVolts, - flywheelCurrent, - flywheelVelocity, - flywheelAppliedVolts, - flywheelCurrent) + BaseStatusSignal.refreshAll(flywheelVelocity, flywheelAppliedVolts, flywheelCurrent) .isOK()); // Update simulation state diff --git a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java index 95359fd..39d046d 100644 --- a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java @@ -74,13 +74,7 @@ public FlywheelIOTalonFX() { public void updateInputs(FlywheelIOInputs inputs) { inputs.connected = flywheelConnectedDebounce.calculate( - BaseStatusSignal.refreshAll( - flywheelVelocity, - flywheelAppliedVolts, - flywheelCurrent, - flywheelVelocity, - flywheelAppliedVolts, - flywheelCurrent) + BaseStatusSignal.refreshAll(flywheelVelocity, flywheelAppliedVolts, flywheelCurrent) .isOK()); inputs.appliedVolts = flywheelAppliedVolts.getValueAsDouble(); From 06ebf516cd39c2815dec0f1b2a4e825402ec189b Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Thu, 12 Feb 2026 18:33:38 -0500 Subject: [PATCH 7/9] use nominal robot voltage, add kV --- src/main/java/frc/robot/Robot.java | 3 +-- .../robot/subsystems/intake/IntakeConstants.java | 14 +++++++------- .../intake/IntakeRollerIOSimTalonFX.java | 4 ++-- .../subsystems/launcher/FlywheelIOSimTalonFX.java | 4 ++-- .../subsystems/launcher/LauncherConstants.java | 15 ++++++++------- 5 files changed, 20 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a5684c3..995641a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -43,7 +43,6 @@ import frc.robot.subsystems.intake.IntakeRollerIO; import frc.robot.subsystems.intake.IntakeRollerIOSimTalonFX; import frc.robot.subsystems.launcher.FlywheelIO; -import frc.robot.subsystems.launcher.FlywheelIOSimTalonFX; import frc.robot.subsystems.launcher.FlywheelIOSimWPI; import frc.robot.subsystems.launcher.HoodIO; import frc.robot.subsystems.launcher.HoodIOSimSpark; @@ -165,7 +164,7 @@ public Robot() { drive::getPose, drive::getRobotRelativeChassisSpeeds, new TurretIOSimSpark(), - new FlywheelIOSimTalonFX(), + new FlywheelIOSimWPI(), new HoodIOSimSpark()); feeder = new Feeder(new SpindexerIOSimSpark(), new KickerIOSimSpark()); intake = new Intake(new IntakeRollerIOSimTalonFX()); diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index d632d81..f8c79b9 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.intake; import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.RotationsPerSecond; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; @@ -8,27 +9,26 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; import frc.robot.Constants.MotorConstants.KrakenX60Constants; +import frc.robot.Constants.RobotConstants; public class IntakeConstants { public class IntakeRoller { public static final Distance rollerRadius = Inches.of(0.75); - // gains + // motor controller + public static final double motorReduction = 1.0; + public static final AngularVelocity maxAngularVelocity = + KrakenX60Constants.kFreeSpeed.div(motorReduction); public static final Slot0Configs intakeGains = new Slot0Configs() .withKP(0.1) .withKI(0.0) .withKD(0.05) .withKS(0.0) - .withKV(0.0) + .withKV(RobotConstants.kNominalVoltage / maxAngularVelocity.in(RotationsPerSecond)) .withKA(0.0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); - // motor controller - public static final double motorReduction = 1.0; - public static final AngularVelocity maxAngularVelocity = - KrakenX60Constants.kFreeSpeed.div(motorReduction); - // simulation public static final DCMotor gearbox = DCMotor.getKrakenX60(1); } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOSimTalonFX.java b/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOSimTalonFX.java index f02670d..9313bf6 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOSimTalonFX.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOSimTalonFX.java @@ -20,9 +20,9 @@ import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.simulation.DCMotorSim; import frc.robot.Constants.CANBusPorts.CAN2; +import frc.robot.Constants.RobotConstants; import frc.robot.Robot; public class IntakeRollerIOSimTalonFX implements IntakeRollerIO { @@ -75,7 +75,7 @@ public void updateInputs(IntakeRollerIOInputs inputs) { // Update simulation state var intakeMotorSim = intakeMotor.getSimState(); - intakeMotorSim.setSupplyVoltage(RobotController.getBatteryVoltage()); + intakeMotorSim.setSupplyVoltage(RobotConstants.kNominalVoltage); intakeRollerSim.setInput(intakeMotorSim.getMotorVoltage()); intakeRollerSim.update(Robot.defaultPeriodSecs); intakeMotorSim.setRawRotorPosition( diff --git a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimTalonFX.java b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimTalonFX.java index faedeb3..25d2f58 100644 --- a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimTalonFX.java +++ b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimTalonFX.java @@ -22,9 +22,9 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.simulation.DCMotorSim; import frc.robot.Constants.CANBusPorts.CAN2; +import frc.robot.Constants.RobotConstants; import frc.robot.Robot; public class FlywheelIOSimTalonFX implements FlywheelIO { @@ -94,7 +94,7 @@ public void updateInputs(FlywheelIOInputs inputs) { // Update simulation state var flywheelMotorSim = flywheelLeaderTalon.getSimState(); - flywheelMotorSim.setSupplyVoltage(RobotController.getBatteryVoltage()); + flywheelMotorSim.setSupplyVoltage(RobotConstants.kNominalVoltage); flywheelSim.setInput(flywheelMotorSim.getMotorVoltage()); flywheelSim.update(Robot.defaultPeriodSecs); flywheelMotorSim.setRawRotorPosition( diff --git a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java index 98c1561..3afc659 100644 --- a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java +++ b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java @@ -15,6 +15,7 @@ import frc.robot.Constants; import frc.robot.Constants.MotorConstants.KrakenX60Constants; import frc.robot.Constants.MotorConstants.NEO550Constants; +import frc.robot.Constants.RobotConstants; public final class LauncherConstants { @@ -95,21 +96,21 @@ public static final class FlywheelConstants { public static final Distance wheelRadius = Inches.of(1.5); // Velocity Controller + + // Motor controller + public static final double motorReduction = 1.0; + public static final AngularVelocity maxAngularVelocity = + KrakenX60Constants.kFreeSpeed.div(motorReduction); public static final Slot0Configs flywheelGains = new Slot0Configs() .withKP(1.0) .withKI(0.0) - .withKD(1.0) + .withKD(0.05) .withKS(0.0) - .withKV(0.0) + .withKV(RobotConstants.kNominalVoltage / maxAngularVelocity.in(RotationsPerSecond)) .withKA(0.0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); - // Motor controller - public static final double motorReduction = 1.0; - public static final AngularVelocity maxAngularVelocity = - KrakenX60Constants.kFreeSpeed.div(motorReduction); - // Simulation public static final double kPSim = 0.1; public static final DCMotor gearbox = DCMotor.getKrakenX60(2); From bc6c3590a496c9be8323a1936e93fdb0915a12af Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Thu, 12 Feb 2026 18:40:32 -0500 Subject: [PATCH 8/9] use talon sim --- src/main/java/frc/robot/Robot.java | 3 ++- .../subsystems/intake/IntakeRollerIOTalonFX.java | 16 ++++++++++------ .../launcher/FlywheelIOSimTalonFX.java | 8 ++------ .../subsystems/launcher/FlywheelIOTalonFX.java | 8 ++------ 4 files changed, 16 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 995641a..a5684c3 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -43,6 +43,7 @@ import frc.robot.subsystems.intake.IntakeRollerIO; import frc.robot.subsystems.intake.IntakeRollerIOSimTalonFX; import frc.robot.subsystems.launcher.FlywheelIO; +import frc.robot.subsystems.launcher.FlywheelIOSimTalonFX; import frc.robot.subsystems.launcher.FlywheelIOSimWPI; import frc.robot.subsystems.launcher.HoodIO; import frc.robot.subsystems.launcher.HoodIOSimSpark; @@ -164,7 +165,7 @@ public Robot() { drive::getPose, drive::getRobotRelativeChassisSpeeds, new TurretIOSimSpark(), - new FlywheelIOSimWPI(), + new FlywheelIOSimTalonFX(), new HoodIOSimSpark()); feeder = new Feeder(new SpindexerIOSimSpark(), new KickerIOSimSpark()); intake = new Intake(new IntakeRollerIOSimTalonFX()); diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOTalonFX.java index e0a5a4c..6f54dac 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOTalonFX.java @@ -9,7 +9,7 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; +import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; @@ -25,8 +25,9 @@ public class IntakeRollerIOTalonFX implements IntakeRollerIO { private final TalonFXConfiguration config; private final VoltageOut voltageRequest = new VoltageOut(0); - private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest = - new VelocityTorqueCurrentFOC(0.0); + private final VelocityVoltage velocityVoltageRequest = new VelocityVoltage(0.0); + // private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest = + // new VelocityTorqueCurrentFOC(0.0); // Inputs from intake motor private final StatusSignal intakeVelocity; @@ -69,8 +70,11 @@ public void setOpenLoop(double output) { @Override public void setVelocity(LinearVelocity tangentialVelocity) { - var angularVelocity = - RadiansPerSecond.of(tangentialVelocity.in(MetersPerSecond) / rollerRadius.in(Meters)); - intakeMotor.setControl(velocityTorqueCurrentRequest.withVelocity(angularVelocity)); + intakeMotor.setControl( + velocityVoltageRequest.withVelocity( + RadiansPerSecond.of( + tangentialVelocity.in(MetersPerSecond) + * motorReduction + / rollerRadius.in(Meters)))); } } diff --git a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimTalonFX.java b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimTalonFX.java index 25d2f58..fee0bef 100644 --- a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimTalonFX.java +++ b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimTalonFX.java @@ -8,7 +8,6 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; @@ -39,11 +38,8 @@ public class FlywheelIOSimTalonFX implements FlywheelIO { // Voltage control requests private final VoltageOut voltageRequest = new VoltageOut(0); private final VelocityVoltage velocityVoltageRequest = new VelocityVoltage(0.0); - - // Torque-current control requests - // private final TorqueCurrentFOC torqueCurrentRequest = new TorqueCurrentFOC(0); - private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest = - new VelocityTorqueCurrentFOC(0.0); + // private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest = + // new VelocityTorqueCurrentFOC(0.0); // Inputs from flywheel motor private final StatusSignal flywheelVelocity; diff --git a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java index 39d046d..1a7704b 100644 --- a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java @@ -8,7 +8,6 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; @@ -31,11 +30,8 @@ public class FlywheelIOTalonFX implements FlywheelIO { // Voltage control requests private final VoltageOut voltageRequest = new VoltageOut(0); private final VelocityVoltage velocityVoltageRequest = new VelocityVoltage(0.0); - - // Torque-current control requests - // private final TorqueCurrentFOC torqueCurrentRequest = new TorqueCurrentFOC(0); - private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest = - new VelocityTorqueCurrentFOC(0.0); + // private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest = + // new VelocityTorqueCurrentFOC(0.0); // Inputs from flywheel motor private final StatusSignal flywheelVelocity; From 6e2963bad639ad5b9813c8bd85ff2b951e8e75b6 Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Thu, 12 Feb 2026 18:44:53 -0500 Subject: [PATCH 9/9] update intakeRoller to match sim --- .../subsystems/intake/IntakeRollerIOTalonFX.java | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOTalonFX.java index 6f54dac..54a4ca6 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOTalonFX.java @@ -14,6 +14,7 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.LinearVelocity; @@ -23,6 +24,7 @@ public class IntakeRollerIOTalonFX implements IntakeRollerIO { private final TalonFX intakeMotor; private final TalonFXConfiguration config; + private final Debouncer connectedDebounce = new Debouncer(0.5, Debouncer.DebounceType.kFalling); private final VoltageOut voltageRequest = new VoltageOut(0); private final VelocityVoltage velocityVoltageRequest = new VelocityVoltage(0.0); @@ -52,15 +54,14 @@ public IntakeRollerIOTalonFX() { @Override public void updateInputs(IntakeRollerIOInputs inputs) { - BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, intakeVelocity, intakeAppliedVolts, intakeCurrent) - .isOK(); + inputs.connected = + connectedDebounce.calculate( + BaseStatusSignal.refreshAll(intakeVelocity, intakeAppliedVolts, intakeCurrent).isOK()); inputs.appliedVolts = intakeAppliedVolts.getValueAsDouble(); - inputs.connected = intakeMotor.isConnected(); inputs.currentAmps = intakeCurrent.getValueAsDouble(); inputs.velocityMetersPerSec = - intakeVelocity.getValueAsDouble() * rollerRadius.in(Meters) * (2 * Math.PI); + intakeVelocity.getValue().in(RadiansPerSecond) * rollerRadius.in(Meters) / motorReduction; } @Override