From 83bcd35744db05d00a9d2cbd121096dbcad9ed45 Mon Sep 17 00:00:00 2001 From: Ghost <145223182+GhostR406@users.noreply.github.com> Date: Wed, 4 Mar 2026 20:40:00 -0500 Subject: [PATCH 1/2] adding 2nd flywheel --- src/main/java/frc/robot/Constants.java | 12 ++++-- src/main/java/frc/robot/RobotContainer.java | 43 +++++++++++++------ .../java/frc/robot/subsystems/Shooter.java | 19 +++++--- 3 files changed, 51 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d82af9b..1d5d8d1 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -294,9 +294,9 @@ public class ShooterFlywheelConstants { // Velocity PID private static Slot0Configs SLOT0CONFIG = - new Slot0Configs().withKP(1000.0).withKI(0.0).withKD(0.0); + new Slot0Configs().withKP(0.0).withKI(0.0).withKD(0.0).withKS(10.0); - public static TalonFXConfiguration getFXConfig() { + public static TalonFXConfiguration getFXConfig(boolean invert) { TalonFXConfiguration config = new TalonFXConfiguration(); config.CurrentLimits.SupplyCurrentLimitEnable = Robot.isReal(); @@ -311,7 +311,13 @@ public static TalonFXConfiguration getFXConfig() { config.Voltage.PeakReverseVoltage = -12.0; config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + //config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + if (invert == true){ + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + } + else { + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + } config.SoftwareLimitSwitch.ForwardSoftLimitEnable = false; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 330cbdd..d24bb4b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -104,12 +104,17 @@ public RobotContainer() { new FlywheelMechanismReal( new MotorIOTalonFX( ShooterFlywheelConstants.NAME, - ShooterFlywheelConstants.getFXConfig(), + ShooterFlywheelConstants.getFXConfig(false), Ports.ShooterRoller)), new FlywheelMechanismReal( new MotorIOTalonFX( ShooterFlywheelConstants.NAME, - ShooterFlywheelConstants.getFXConfig(), + ShooterFlywheelConstants.getFXConfig(true), + Ports.ShooterRoller)), + new FlywheelMechanismReal( + new MotorIOTalonFX( + ShooterFlywheelConstants.NAME, + ShooterFlywheelConstants.getFXConfig(false), Ports.ShooterRoller)), new RotaryMechanismReal( new MotorIOTalonFX( @@ -167,7 +172,7 @@ public RobotContainer() { new FlywheelMechanismSim( new MotorIOTalonFXSim( ShooterFlywheelConstants.NAME, - ShooterFlywheelConstants.getFXConfig(), + ShooterFlywheelConstants.getFXConfig(false), Ports.ShooterRoller), ShooterFlywheelConstants.DCMOTOR, ShooterFlywheelConstants.MOI, @@ -175,7 +180,15 @@ public RobotContainer() { new FlywheelMechanismSim( new MotorIOTalonFXSim( ShooterFlywheelConstants.NAME, - ShooterFlywheelConstants.getFXConfig(), + ShooterFlywheelConstants.getFXConfig(true), + Ports.ShooterRoller), + ShooterFlywheelConstants.DCMOTOR, + ShooterFlywheelConstants.MOI, + ShooterFlywheelConstants.TOLERANCE), + new FlywheelMechanismSim( + new MotorIOTalonFXSim( + ShooterFlywheelConstants.NAME, + ShooterFlywheelConstants.getFXConfig(false), Ports.ShooterRoller), ShooterFlywheelConstants.DCMOTOR, ShooterFlywheelConstants.MOI, @@ -236,6 +249,7 @@ public RobotContainer() { shooter = new Shooter( + new FlywheelMechanism() {}, new FlywheelMechanism() {}, new FlywheelMechanism() {}, new RotaryMechanism(null, null) {}); @@ -287,17 +301,17 @@ private void configureButtonBindings() { () -> -controller.getRightX())); // Lock to 0° when A button is held - controller - .a() - .whileTrue( - DriveCommands.joystickDriveAtAngle( - drive, - () -> -controller.getLeftY(), - () -> -controller.getLeftX(), - () -> new Rotation2d())); + // controller + // .a() + // .whileTrue( + // DriveCommands.joystickDriveAtAngle( + // drive, + // () -> -controller.getLeftY(), + // () -> -controller.getLeftX(), + // () -> new Rotation2d())); - // Switch to X pattern when X button is pressed - controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); + // // Switch to X pattern when X button is pressed + // controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); // Reset gyro to 0° when B button is pressed controller @@ -328,6 +342,7 @@ private void configureButtonBindings() { controller.y().onTrue(Commands.runOnce(() -> intake.setVelocity(1))); controller.leftBumper().onTrue(intake.intake()); controller.rightBumper().onTrue(intake.stowAndStopRollers()); + controller.a().onTrue(shooter.runFlywheel()); } /** diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 31ee0ca..e8d7263 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -28,7 +28,8 @@ public class Shooter extends SubsystemBase { - private final FlywheelMechanism _flywheel; + private final FlywheelMechanism _rflywheel; + private final FlywheelMechanism _lflywheel; private final FlywheelMechanism _feeder; private final RotaryMechanism _hood; @@ -36,8 +37,9 @@ public class Shooter extends SubsystemBase { private double desiredVelo; private double hoodAngle; - public Shooter(FlywheelMechanism flywheel, FlywheelMechanism feeder, RotaryMechanism hood) { - _flywheel = flywheel; + public Shooter(FlywheelMechanism lflywheel, FlywheelMechanism rflywheel, FlywheelMechanism feeder, RotaryMechanism hood) { + _lflywheel = lflywheel; + _rflywheel = rflywheel; _feeder = feeder; _hood = hood; } @@ -53,8 +55,9 @@ public void setFlywheelVelocity(double velocity) { // store the desired velocity then send converted velocity to the mechanism this.desiredVelo = velocity; AngularVelocity angVelo = RotationsPerSecond.of(velocity); - - _flywheel.runVelocity(angVelo, ShooterConstants.ACCELERATION, PIDSlot.SLOT_0); + AngularVelocity negangVelo = RotationsPerSecond.of(velocity); + _lflywheel.runVelocity(angVelo, ShooterConstants.ACCELERATION, PIDSlot.SLOT_0); + _rflywheel.runVelocity(negangVelo, ShooterConstants.ACCELERATION, PIDSlot.SLOT_0); } public enum State { @@ -74,7 +77,7 @@ public enum State { // Checks if the flywheel is at speed and returns a boolean public boolean flyAtVelocity() { - return Math.abs(desiredVelo - _flywheel.getVelocity().in(RotationsPerSecond)) + return ((Math.abs(desiredVelo - _lflywheel.getVelocity().in(RotationsPerSecond)) + Math.abs(desiredVelo - _rflywheel.getVelocity().in(RotationsPerSecond))) / 2) <= ShooterConstants.FLYWHEEL_VELOCITY_TOLERANCE; } @@ -129,6 +132,10 @@ public Command shoot(double velocity) { Commands.runOnce(() -> setFlywheelVelocity(0))); } + public Command runFlywheel(){ + return Commands.runOnce(() -> setFlywheelVelocity(1)); + } + public void simShoot() { if (Robot.robotContainer.intake.simBalls <= 0) return; From c22a0dd5c5d01e0cbe3d9ad855549bcfc8d61bf7 Mon Sep 17 00:00:00 2001 From: rhit-collinkn Date: Wed, 4 Mar 2026 22:23:48 -0500 Subject: [PATCH 2/2] formatting --- src/main/java/frc/robot/Constants.java | 7 +++---- src/main/java/frc/robot/subsystems/Shooter.java | 14 ++++++++++---- 2 files changed, 13 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d184772..c23c8bb 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -319,11 +319,10 @@ public static TalonFXConfiguration getFXConfig(boolean invert) { config.Voltage.PeakReverseVoltage = -12.0; config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - //config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - if (invert == true){ + // config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + if (invert == true) { config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - } - else { + } else { config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index e8d7263..2cc0c7a 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -37,7 +37,11 @@ public class Shooter extends SubsystemBase { private double desiredVelo; private double hoodAngle; - public Shooter(FlywheelMechanism lflywheel, FlywheelMechanism rflywheel, FlywheelMechanism feeder, RotaryMechanism hood) { + public Shooter( + FlywheelMechanism lflywheel, + FlywheelMechanism rflywheel, + FlywheelMechanism feeder, + RotaryMechanism hood) { _lflywheel = lflywheel; _rflywheel = rflywheel; _feeder = feeder; @@ -56,7 +60,7 @@ public void setFlywheelVelocity(double velocity) { this.desiredVelo = velocity; AngularVelocity angVelo = RotationsPerSecond.of(velocity); AngularVelocity negangVelo = RotationsPerSecond.of(velocity); - _lflywheel.runVelocity(angVelo, ShooterConstants.ACCELERATION, PIDSlot.SLOT_0); + _lflywheel.runVelocity(angVelo, ShooterConstants.ACCELERATION, PIDSlot.SLOT_0); _rflywheel.runVelocity(negangVelo, ShooterConstants.ACCELERATION, PIDSlot.SLOT_0); } @@ -77,7 +81,9 @@ public enum State { // Checks if the flywheel is at speed and returns a boolean public boolean flyAtVelocity() { - return ((Math.abs(desiredVelo - _lflywheel.getVelocity().in(RotationsPerSecond)) + Math.abs(desiredVelo - _rflywheel.getVelocity().in(RotationsPerSecond))) / 2) + return ((Math.abs(desiredVelo - _lflywheel.getVelocity().in(RotationsPerSecond)) + + Math.abs(desiredVelo - _rflywheel.getVelocity().in(RotationsPerSecond))) + / 2) <= ShooterConstants.FLYWHEEL_VELOCITY_TOLERANCE; } @@ -132,7 +138,7 @@ public Command shoot(double velocity) { Commands.runOnce(() -> setFlywheelVelocity(0))); } - public Command runFlywheel(){ + public Command runFlywheel() { return Commands.runOnce(() -> setFlywheelVelocity(1)); }