From 968fde0d7ca2a80349252da2c11ea7ce0d3ddaf6 Mon Sep 17 00:00:00 2001 From: Srijan-Murai Date: Tue, 13 Jan 2026 19:36:28 -0500 Subject: [PATCH 1/7] made flywheel(shooter) subsystem --- src/main/java/frc/robot/Robot.java | 1 - src/main/java/frc/robot/RobotContainer.java | 10 +++++ .../robot/subsystems/flywheel/Flywheel.java | 34 +++++++++++++++++ .../subsystems/flywheel/FlywheelConfigs.java | 16 ++++++++ .../robot/subsystems/flywheel/FlywheelIO.java | 23 ++++++++++++ .../subsystems/flywheel/FlywheelIOSim.java | 37 +++++++++++++++++++ .../flywheel/FlywheelIOTalonFX.java | 34 +++++++++++++++++ 7 files changed, 154 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/subsystems/flywheel/Flywheel.java create mode 100644 src/main/java/frc/robot/subsystems/flywheel/FlywheelConfigs.java create mode 100644 src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java create mode 100644 src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSim.java create mode 100644 src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 68c00ed..a670d71 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -137,4 +137,3 @@ public void simulationInit() {} @Override public void simulationPeriodic() {} } - diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6b2016e..837e3c0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -24,6 +24,9 @@ import frc.robot.subsystems.drive.ModuleIO; import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOTalonFX; +import frc.robot.subsystems.flywheel.FlywheelIO; +import frc.robot.subsystems.flywheel.FlywheelIOSim; +import frc.robot.subsystems.flywheel.FlywheelIOTalonFX; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; /** @@ -35,6 +38,7 @@ public class RobotContainer { // Subsystems private final Drive drive; + private final FlywheelIO flywheel; // Controller private final CommandXboxController controller = new CommandXboxController(0); @@ -71,6 +75,8 @@ public RobotContainer() { // new ModuleIOTalonFXS(TunerConstants.FrontRight), // new ModuleIOTalonFXS(TunerConstants.BackLeft), // new ModuleIOTalonFXS(TunerConstants.BackRight)); + + flywheel = new FlywheelIOTalonFX(); break; case SIM: @@ -82,6 +88,8 @@ public RobotContainer() { new ModuleIOSim(TunerConstants.FrontRight), new ModuleIOSim(TunerConstants.BackLeft), new ModuleIOSim(TunerConstants.BackRight)); + + flywheel = new FlywheelIOSim(); break; default: @@ -93,6 +101,8 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}); + + flywheel = new FlywheelIO() {}; break; } diff --git a/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java b/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java new file mode 100644 index 0000000..c2d2686 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java @@ -0,0 +1,34 @@ +package frc.robot.subsystems.flywheel; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Flywheel extends SubsystemBase { + // Auto logged classes for AKit + private final FlywheelIO io; + private final FlywheelIOInputsAutoLogged inputs = new FlywheelIOInputsAutoLogged(); + + @Override + public void periodic() { + io.updateInputs(inputs); + } + + public Flywheel(FlywheelIO io) { + // Recording inputs from roller + this.io = io; + // Logger.processInputs("Flywheel", inputs); error + } + + public Command setRoller(double power) { + return runOnce(() -> io.setRunning(true)) + .andThen( + runEnd( + () -> { + io.setRollerDuty(power); + }, + () -> { + io.setRollerDuty(0); + })) + .andThen(() -> io.setRunning(false)); + } +} diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelConfigs.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelConfigs.java new file mode 100644 index 0000000..f29a658 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/flywheel/FlywheelConfigs.java @@ -0,0 +1,16 @@ +package frc.robot.subsystems.flywheel; + +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.InvertedValue; + +public class FlywheelConfigs { + static final int leftFXID = 41; + static final int rightFXID = 67; + + static TalonFXConfiguration rollerConfig = + new TalonFXConfiguration() + .withMotorOutput( + new MotorOutputConfigs() + .withInverted(InvertedValue.Clockwise_Positive)); +} diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java new file mode 100644 index 0000000..b1f14ab --- /dev/null +++ b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java @@ -0,0 +1,23 @@ +package frc.robot.subsystems.flywheel; + +import org.littletonrobotics.junction.AutoLog; + +public interface FlywheelIO { + + @AutoLog + public static class FlywheelIOInputs { + public double leftFXVelocityRPM = 0; + public double leftFXSpeed = 0; + + public double rightFXVelocityRPM = 0; + public double rightFXSpeed = 0; + } + + public default void updateInputs(FlywheelIOInputs inputs) {} + + public default void setRollerDuty(double power) {} + + public default void launchRing() {} + + public default void setRunning(boolean runIntake) {} +} diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSim.java new file mode 100644 index 0000000..c524ac8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSim.java @@ -0,0 +1,37 @@ +package frc.robot.subsystems.flywheel; + +import com.ctre.phoenix6.controls.DutyCycleOut; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.hardware.TalonFX; + +public class FlywheelIOSim implements FlywheelIO { + private final TalonFX leftFX; + private final TalonFX rightFX; + + public FlywheelIOSim() { + leftFX = new TalonFX(FlywheelConfigs.leftFXID); + rightFX = new TalonFX(FlywheelConfigs.rightFXID); + // make lower a follower of upper + leftFX.setControl(new Follower(FlywheelConfigs.rightFXID, false)); + // add motors to sim + // PhysicsSim.getInstance().addTalonFX(leftFX); + // PhysicsSim.getInstance().addTalonFX(rightFX); + leftFX.getConfigurator().apply(FlywheelConfigs.rollerConfig); + rightFX.getConfigurator().apply(FlywheelConfigs.rollerConfig); + } + + @Override + public void updateInputs(FlywheelIO.FlywheelIOInputs inputs) { + inputs.leftFXVelocityRPM = leftFX.getVelocity().getValueAsDouble(); + inputs.leftFXSpeed = leftFX.getDutyCycle().getValueAsDouble(); + + inputs.rightFXVelocityRPM = rightFX.getVelocity().getValueAsDouble(); + inputs.rightFXSpeed = rightFX.getDutyCycle().getValueAsDouble(); + } + + @Override + public void setRollerDuty(double power) { + leftFX.setControl(new DutyCycleOut(power)); + // right is follower so left only + } +} diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java new file mode 100644 index 0000000..51bb5dc --- /dev/null +++ b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java @@ -0,0 +1,34 @@ +package frc.robot.subsystems.flywheel; + +import com.ctre.phoenix6.controls.DutyCycleOut; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.hardware.TalonFX; + +public class FlywheelIOTalonFX implements FlywheelIO { + private final TalonFX leftFX; + private final TalonFX rightFX; + + public FlywheelIOTalonFX() { + leftFX = new TalonFX(FlywheelConfigs.leftFXID); + rightFX = new TalonFX(FlywheelConfigs.rightFXID); + // make left a follower of right + leftFX.setControl(new Follower(rightFX.getDeviceID(), false)); + leftFX.getConfigurator().apply(FlywheelConfigs.rollerConfig); + rightFX.getConfigurator().apply(FlywheelConfigs.rollerConfig); + } + + @Override + public void updateInputs(FlywheelIO.FlywheelIOInputs inputs) { + inputs.leftFXVelocityRPM = leftFX.getVelocity().getValueAsDouble(); + inputs.leftFXSpeed = leftFX.getDutyCycle().getValueAsDouble(); + + inputs.rightFXVelocityRPM = rightFX.getVelocity().getValueAsDouble(); + inputs.rightFXSpeed = rightFX.getDutyCycle().getValueAsDouble(); + } + + @Override + public void setRollerDuty(double power) { + leftFX.setControl(new DutyCycleOut(power)); + // right is follower so left only + } +} From 5e9c37959ebf3d2260e7add54b48ad3c80029b5b Mon Sep 17 00:00:00 2001 From: Srijan-Murai Date: Tue, 13 Jan 2026 20:22:02 -0500 Subject: [PATCH 2/7] removed sim file, renamed motors and used MotionMagicVelocityTorqueCurrentFOC --- src/main/java/frc/robot/RobotContainer.java | 3 +- .../robot/subsystems/flywheel/Flywheel.java | 17 ++++----- .../subsystems/flywheel/FlywheelConfigs.java | 4 +- .../robot/subsystems/flywheel/FlywheelIO.java | 12 +++--- .../subsystems/flywheel/FlywheelIOSim.java | 37 ------------------- .../flywheel/FlywheelIOTalonFX.java | 28 +++++++------- 6 files changed, 29 insertions(+), 72 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSim.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 837e3c0..964305a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -25,7 +25,6 @@ import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOTalonFX; import frc.robot.subsystems.flywheel.FlywheelIO; -import frc.robot.subsystems.flywheel.FlywheelIOSim; import frc.robot.subsystems.flywheel.FlywheelIOTalonFX; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; @@ -89,7 +88,7 @@ public RobotContainer() { new ModuleIOSim(TunerConstants.BackLeft), new ModuleIOSim(TunerConstants.BackRight)); - flywheel = new FlywheelIOSim(); + flywheel = new FlywheelIOTalonFX(); break; default: diff --git a/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java b/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java index c2d2686..037f72e 100644 --- a/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java +++ b/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java @@ -20,15 +20,12 @@ public Flywheel(FlywheelIO io) { } public Command setRoller(double power) { - return runOnce(() -> io.setRunning(true)) - .andThen( - runEnd( - () -> { - io.setRollerDuty(power); - }, - () -> { - io.setRollerDuty(0); - })) - .andThen(() -> io.setRunning(false)); + return runEnd( + () -> { + io.setRoller(power); + }, + () -> { + io.setRoller(0); + }); } } diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelConfigs.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelConfigs.java index f29a658..1870998 100644 --- a/src/main/java/frc/robot/subsystems/flywheel/FlywheelConfigs.java +++ b/src/main/java/frc/robot/subsystems/flywheel/FlywheelConfigs.java @@ -5,8 +5,8 @@ import com.ctre.phoenix6.signals.InvertedValue; public class FlywheelConfigs { - static final int leftFXID = 41; - static final int rightFXID = 67; + static final int leftMotorID = 41; + static final int rightMotorID = 67; static TalonFXConfiguration rollerConfig = new TalonFXConfiguration() diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java index b1f14ab..fa40f77 100644 --- a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java +++ b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java @@ -6,18 +6,16 @@ public interface FlywheelIO { @AutoLog public static class FlywheelIOInputs { - public double leftFXVelocityRPM = 0; - public double leftFXSpeed = 0; + public double leftMotorVelocityRPM = 0; + public double leftMotorSpeed = 0; - public double rightFXVelocityRPM = 0; - public double rightFXSpeed = 0; + public double rightMotorVelocityRPM = 0; + public double rightMotorSpeed = 0; } public default void updateInputs(FlywheelIOInputs inputs) {} - public default void setRollerDuty(double power) {} + public default void setRoller(double power) {} public default void launchRing() {} - - public default void setRunning(boolean runIntake) {} } diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSim.java deleted file mode 100644 index c524ac8..0000000 --- a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSim.java +++ /dev/null @@ -1,37 +0,0 @@ -package frc.robot.subsystems.flywheel; - -import com.ctre.phoenix6.controls.DutyCycleOut; -import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.hardware.TalonFX; - -public class FlywheelIOSim implements FlywheelIO { - private final TalonFX leftFX; - private final TalonFX rightFX; - - public FlywheelIOSim() { - leftFX = new TalonFX(FlywheelConfigs.leftFXID); - rightFX = new TalonFX(FlywheelConfigs.rightFXID); - // make lower a follower of upper - leftFX.setControl(new Follower(FlywheelConfigs.rightFXID, false)); - // add motors to sim - // PhysicsSim.getInstance().addTalonFX(leftFX); - // PhysicsSim.getInstance().addTalonFX(rightFX); - leftFX.getConfigurator().apply(FlywheelConfigs.rollerConfig); - rightFX.getConfigurator().apply(FlywheelConfigs.rollerConfig); - } - - @Override - public void updateInputs(FlywheelIO.FlywheelIOInputs inputs) { - inputs.leftFXVelocityRPM = leftFX.getVelocity().getValueAsDouble(); - inputs.leftFXSpeed = leftFX.getDutyCycle().getValueAsDouble(); - - inputs.rightFXVelocityRPM = rightFX.getVelocity().getValueAsDouble(); - inputs.rightFXSpeed = rightFX.getDutyCycle().getValueAsDouble(); - } - - @Override - public void setRollerDuty(double power) { - leftFX.setControl(new DutyCycleOut(power)); - // right is follower so left only - } -} diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java index 51bb5dc..2b40861 100644 --- a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java @@ -1,34 +1,34 @@ package frc.robot.subsystems.flywheel; -import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.MotionMagicVelocityTorqueCurrentFOC; import com.ctre.phoenix6.hardware.TalonFX; public class FlywheelIOTalonFX implements FlywheelIO { - private final TalonFX leftFX; - private final TalonFX rightFX; + private final TalonFX leftMotor; + private final TalonFX rightMotor; public FlywheelIOTalonFX() { - leftFX = new TalonFX(FlywheelConfigs.leftFXID); - rightFX = new TalonFX(FlywheelConfigs.rightFXID); + leftMotor = new TalonFX(FlywheelConfigs.leftMotorID); + rightMotor = new TalonFX(FlywheelConfigs.rightMotorID); // make left a follower of right - leftFX.setControl(new Follower(rightFX.getDeviceID(), false)); - leftFX.getConfigurator().apply(FlywheelConfigs.rollerConfig); - rightFX.getConfigurator().apply(FlywheelConfigs.rollerConfig); + leftMotor.setControl(new Follower(rightMotor.getDeviceID(), false)); + leftMotor.getConfigurator().apply(FlywheelConfigs.rollerConfig); + rightMotor.getConfigurator().apply(FlywheelConfigs.rollerConfig); } @Override public void updateInputs(FlywheelIO.FlywheelIOInputs inputs) { - inputs.leftFXVelocityRPM = leftFX.getVelocity().getValueAsDouble(); - inputs.leftFXSpeed = leftFX.getDutyCycle().getValueAsDouble(); + inputs.leftMotorVelocityRPM = leftMotor.getVelocity().getValueAsDouble(); + inputs.leftMotorSpeed = leftMotor.getDutyCycle().getValueAsDouble(); - inputs.rightFXVelocityRPM = rightFX.getVelocity().getValueAsDouble(); - inputs.rightFXSpeed = rightFX.getDutyCycle().getValueAsDouble(); + inputs.rightMotorVelocityRPM = rightMotor.getVelocity().getValueAsDouble(); + inputs.rightMotorSpeed = rightMotor.getDutyCycle().getValueAsDouble(); } @Override - public void setRollerDuty(double power) { - leftFX.setControl(new DutyCycleOut(power)); + public void setRoller(double power) { + leftMotor.setControl(new MotionMagicVelocityTorqueCurrentFOC(power)); // right is follower so left only } } From 5bbcac7fbb41a23144faca702df401317dee6987 Mon Sep 17 00:00:00 2001 From: Srijan-Murai Date: Tue, 20 Jan 2026 18:59:18 -0500 Subject: [PATCH 3/7] added physicssim and withOutput --- .../robot/subsystems/flywheel/Flywheel.java | 3 +- .../robot/subsystems/flywheel/FlywheelIO.java | 2 - .../flywheel/FlywheelIOTalonFX.java | 12 +++- .../java/frc/robot/util/sim/PhysicsSim.java | 68 +++++++++++++++++++ .../frc/robot/util/sim/TalonFXSimProfile.java | 67 ++++++++++++++++++ 5 files changed, 147 insertions(+), 5 deletions(-) create mode 100644 src/main/java/frc/robot/util/sim/PhysicsSim.java create mode 100644 src/main/java/frc/robot/util/sim/TalonFXSimProfile.java diff --git a/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java b/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java index 037f72e..b21d6b6 100644 --- a/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java +++ b/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java @@ -2,6 +2,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; public class Flywheel extends SubsystemBase { // Auto logged classes for AKit @@ -16,7 +17,7 @@ public void periodic() { public Flywheel(FlywheelIO io) { // Recording inputs from roller this.io = io; - // Logger.processInputs("Flywheel", inputs); error + Logger.processInputs("Flywheel", inputs); } public Command setRoller(double power) { diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java index fa40f77..215ec9d 100644 --- a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java +++ b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java @@ -16,6 +16,4 @@ public static class FlywheelIOInputs { public default void updateInputs(FlywheelIOInputs inputs) {} public default void setRoller(double power) {} - - public default void launchRing() {} } diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java index 2b40861..7dea89b 100644 --- a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java @@ -1,13 +1,16 @@ package frc.robot.subsystems.flywheel; import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.MotionMagicVelocityTorqueCurrentFOC; +import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; +import frc.robot.Robot; public class FlywheelIOTalonFX implements FlywheelIO { private final TalonFX leftMotor; private final TalonFX rightMotor; + private final VoltageOut voltageRequest = new VoltageOut(0); + public FlywheelIOTalonFX() { leftMotor = new TalonFX(FlywheelConfigs.leftMotorID); rightMotor = new TalonFX(FlywheelConfigs.rightMotorID); @@ -15,6 +18,11 @@ public FlywheelIOTalonFX() { leftMotor.setControl(new Follower(rightMotor.getDeviceID(), false)); leftMotor.getConfigurator().apply(FlywheelConfigs.rollerConfig); rightMotor.getConfigurator().apply(FlywheelConfigs.rollerConfig); + // add to sim + if (Robot.isSimulation()) { + // PhysicsSim.getInstance.addTalonFX(leftMotor); + // PhysicsSim.getInstance.addTalonFX(rightMotor); + } } @Override @@ -28,7 +36,7 @@ public void updateInputs(FlywheelIO.FlywheelIOInputs inputs) { @Override public void setRoller(double power) { - leftMotor.setControl(new MotionMagicVelocityTorqueCurrentFOC(power)); + leftMotor.setControl(voltageRequest.withOutput(power)); // right is follower so left only } } diff --git a/src/main/java/frc/robot/util/sim/PhysicsSim.java b/src/main/java/frc/robot/util/sim/PhysicsSim.java new file mode 100644 index 0000000..8316ddd --- /dev/null +++ b/src/main/java/frc/robot/util/sim/PhysicsSim.java @@ -0,0 +1,68 @@ +package frc.robot.util.sim; + +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import java.util.ArrayList; + +/** Manages physics simulation for CTRE products. */ +public class PhysicsSim { + private static final PhysicsSim sim = new PhysicsSim(); + private final ArrayList simProfiles = new ArrayList<>(); + + /** Gets the robot simulator instance. */ + public static PhysicsSim getInstance() { + return sim; + } + + /** + * Adds a TalonFX controller to the simulator. + * + * @param talonFX The TalonFX device + */ + public void addTalonFX(TalonFX talonFX) { + if (talonFX != null) { + TalonFXSimProfile simTalonFX = new TalonFXSimProfile(talonFX, 0.001); + simProfiles.add(simTalonFX); + } + } + + public void addTalonFX(TalonFX talonFX, CANcoder cancoder) { + if (talonFX != null && cancoder != null) { + TalonFXSimProfile simTalonFX = new TalonFXSimProfile(talonFX, 0.001, cancoder); + simProfiles.add(simTalonFX); + } + } + + /** Runs the simulator: - enable the robot - simulate sensors */ + public void run() { + // Simulate devices + for (SimProfile simProfile : simProfiles) { + simProfile.run(); + } + } + + /** Holds information about a simulated device. */ + static class SimProfile { + private double _lastTime; + private boolean _running = false; + + /** Runs the simulation profile. Implemented by device-specific profiles. */ + public void run() {} + + /** Returns the time since last call, in seconds. */ + protected double getPeriod() { + // set the start time if not yet running + if (!_running) { + _lastTime = Utils.getCurrentTimeSeconds(); + _running = true; + } + + double now = Utils.getCurrentTimeSeconds(); + final double period = now - _lastTime; + _lastTime = now; + + return period; + } + } +} diff --git a/src/main/java/frc/robot/util/sim/TalonFXSimProfile.java b/src/main/java/frc/robot/util/sim/TalonFXSimProfile.java new file mode 100644 index 0000000..67a98f2 --- /dev/null +++ b/src/main/java/frc/robot/util/sim/TalonFXSimProfile.java @@ -0,0 +1,67 @@ +package frc.robot.util.sim; + +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.sim.CANcoderSimState; +import com.ctre.phoenix6.sim.TalonFXSimState; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; + +/** Holds information about a simulated TalonFX. */ +class TalonFXSimProfile extends PhysicsSim.SimProfile { + private static final double MOTOR_RESISTANCE = + 0.002; // Assume 2mOhm resistance for voltage drop calculation + + private final DCMotorSim motorSim; + private final TalonFXSimState talonFXSim; + private CANcoderSimState cancoderSimState; + + /** + * Creates a new simulation profile for a TalonFX device. + * + * @param talonFX The TalonFX device + * @param rotorInertia Rotational Inertia of the mechanism at the rotor + */ + public TalonFXSimProfile(final TalonFX talonFX, final double rotorInertia) { + var gearbox = DCMotor.getKrakenX60Foc(1); + this.motorSim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem(gearbox, rotorInertia, 1.0), gearbox); + this.talonFXSim = talonFX.getSimState(); + } + + public TalonFXSimProfile(final TalonFX talonFX, final double rotorInertia, CANcoder cancoder) { + this(talonFX, rotorInertia); + cancoderSimState = cancoder.getSimState(); + } + + /** + * Runs the simulation profile. + * + *

This uses very rudimentary physics simulation and exists to allow users to test features + * of our products in simulation using our examples out of the box. Users may modify this to + * utilize more accurate physics simulation. + */ + public void run() { + /// DEVICE SPEED SIMULATION + + motorSim.setInputVoltage(talonFXSim.getMotorVoltage()); + + motorSim.update(getPeriod()); + + /// SET SIM PHYSICS INPUTS + final double position_rot = motorSim.getAngularPositionRotations(); + final double velocity_rps = Units.radiansToRotations(motorSim.getAngularVelocityRPM()); + + if (cancoderSimState != null) { + cancoderSimState.setRawPosition(position_rot); + cancoderSimState.setVelocity(velocity_rps); + } + talonFXSim.setRawRotorPosition(position_rot); + talonFXSim.setRotorVelocity(velocity_rps); + + talonFXSim.setSupplyVoltage(12 - talonFXSim.getSupplyCurrent() * MOTOR_RESISTANCE); + } +} From 6e30178cf9b4599cdd1e8cb930dd3a485729db68 Mon Sep 17 00:00:00 2001 From: Srijan-Murai Date: Tue, 20 Jan 2026 19:11:22 -0500 Subject: [PATCH 4/7] controller binding for flywheel --- src/main/java/frc/robot/RobotContainer.java | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 964305a..946239e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -172,6 +172,13 @@ private void configureButtonBindings() { Rotation2d.kZero)), drive) .ignoringDisable(true)); + controller + .leftTrigger() + .whileTrue( + Commands.run( + () -> flywheel.setRoller(1) + ) + ); } /** From 2847609aead58e73914ee351fb3247bb68dceb84 Mon Sep 17 00:00:00 2001 From: Srijan-Murai Date: Tue, 20 Jan 2026 19:22:47 -0500 Subject: [PATCH 5/7] added physicssim, used voltagerequest, added bindings --- .../frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java index 7dea89b..bdc7fdb 100644 --- a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java @@ -4,6 +4,7 @@ import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import frc.robot.Robot; +import frc.robot.util.sim.PhysicsSim; public class FlywheelIOTalonFX implements FlywheelIO { private final TalonFX leftMotor; @@ -20,8 +21,8 @@ public FlywheelIOTalonFX() { rightMotor.getConfigurator().apply(FlywheelConfigs.rollerConfig); // add to sim if (Robot.isSimulation()) { - // PhysicsSim.getInstance.addTalonFX(leftMotor); - // PhysicsSim.getInstance.addTalonFX(rightMotor); + PhysicsSim.getInstance().addTalonFX(leftMotor); + PhysicsSim.getInstance().addTalonFX(rightMotor); } } From 612be044b879162bb2ca9184d9ab4d2061a780ab Mon Sep 17 00:00:00 2001 From: Srijan-Murai Date: Tue, 20 Jan 2026 19:51:39 -0500 Subject: [PATCH 6/7] switched from VoltageOut to MotionMagicVelocityVoltage --- src/main/java/frc/robot/RobotContainer.java | 8 +------- .../frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java | 5 +++-- 2 files changed, 4 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 946239e..1ac7323 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -172,13 +172,7 @@ private void configureButtonBindings() { Rotation2d.kZero)), drive) .ignoringDisable(true)); - controller - .leftTrigger() - .whileTrue( - Commands.run( - () -> flywheel.setRoller(1) - ) - ); + controller.leftTrigger().whileTrue(Commands.run(() -> flywheel.setRoller(1))); } /** diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java index bdc7fdb..ea447c0 100644 --- a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.flywheel; import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import frc.robot.Robot; @@ -10,7 +11,7 @@ public class FlywheelIOTalonFX implements FlywheelIO { private final TalonFX leftMotor; private final TalonFX rightMotor; - private final VoltageOut voltageRequest = new VoltageOut(0); + private final MotionMagicVelocityVoltage motionRequest = new MotionMagicVelocityVoltage(0); public FlywheelIOTalonFX() { leftMotor = new TalonFX(FlywheelConfigs.leftMotorID); @@ -37,7 +38,7 @@ public void updateInputs(FlywheelIO.FlywheelIOInputs inputs) { @Override public void setRoller(double power) { - leftMotor.setControl(voltageRequest.withOutput(power)); + leftMotor.setControl(motionRequest.withVelocity(power)); // right is follower so left only } } From f5d87a92f6fdf242ab4513498dc4f39fe8048742 Mon Sep 17 00:00:00 2001 From: Srijan-Murai Date: Tue, 20 Jan 2026 20:06:56 -0500 Subject: [PATCH 7/7] removed unnecessary comments and renamed power to velocity --- src/main/java/frc/robot/subsystems/flywheel/Flywheel.java | 7 +++---- .../java/frc/robot/subsystems/flywheel/FlywheelIO.java | 2 +- .../frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java | 6 ++---- 3 files changed, 6 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java b/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java index b21d6b6..4bb6523 100644 --- a/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java +++ b/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java @@ -12,18 +12,17 @@ public class Flywheel extends SubsystemBase { @Override public void periodic() { io.updateInputs(inputs); + Logger.processInputs("Flywheel", inputs); } public Flywheel(FlywheelIO io) { - // Recording inputs from roller this.io = io; - Logger.processInputs("Flywheel", inputs); } - public Command setRoller(double power) { + public Command setRoller(double velocity) { return runEnd( () -> { - io.setRoller(power); + io.setRoller(velocity); }, () -> { io.setRoller(0); diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java index 215ec9d..83ea527 100644 --- a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java +++ b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java @@ -15,5 +15,5 @@ public static class FlywheelIOInputs { public default void updateInputs(FlywheelIOInputs inputs) {} - public default void setRoller(double power) {} + public default void setRoller(double velocity) {} } diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java index ea447c0..b989c01 100644 --- a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java @@ -20,7 +20,6 @@ public FlywheelIOTalonFX() { leftMotor.setControl(new Follower(rightMotor.getDeviceID(), false)); leftMotor.getConfigurator().apply(FlywheelConfigs.rollerConfig); rightMotor.getConfigurator().apply(FlywheelConfigs.rollerConfig); - // add to sim if (Robot.isSimulation()) { PhysicsSim.getInstance().addTalonFX(leftMotor); PhysicsSim.getInstance().addTalonFX(rightMotor); @@ -37,8 +36,7 @@ public void updateInputs(FlywheelIO.FlywheelIOInputs inputs) { } @Override - public void setRoller(double power) { - leftMotor.setControl(motionRequest.withVelocity(power)); - // right is follower so left only + public void setRoller(double velocity) { + leftMotor.setControl(motionRequest.withVelocity(velocity)); } }