From d53a47bdc727d635d2c384531513aa11f0f0cd65 Mon Sep 17 00:00:00 2001 From: lQuasar9206 Date: Sat, 24 Jan 2026 22:24:03 -0800 Subject: [PATCH 1/5] not sure if correct but turretsubsystem auto stubs filled --- .../subsystems/shooter/TurretSubsystem.java | 90 ++++++++++++++----- 1 file changed, 70 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index 685b95a..6b03696 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -4,66 +4,116 @@ package frc.robot.subsystems.shooter; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.utils.LoggedTunableNumber; +import frc.robot.utils.autoaim.AutoAim; +import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; import java.util.function.Supplier; +import org.littletonrobotics.junction.AutoLogOutput; /** Pivoting hooded shooter (turret). !! COMP !! */ public class TurretSubsystem extends SubsystemBase implements Shooter { /** Creates a new TurretSubsystem. */ + public static double HOOD_GEAR_RATIO = 24.230769; + + public static Rotation2d HOOD_MAX_ROTATION = Rotation2d.fromDegrees(40); + public static Rotation2d HOOD_MIN_ROTATION = Rotation2d.fromDegrees(2); + + public static double FLYWHEEL_GEAR_RATIO = 28.0 / 24.0; + + public static double FLYWHEEL_VELOCITY_TOLERANCE_ROTATIONS_PER_SECOND = 5.0; + + HoodIO hoodIO; + HoodIOInputsAutoLogged hoodInputs = new HoodIOInputsAutoLogged(); + + FlywheelIO flywheelIO; + FlywheelIOInputsAutoLogged flywheelInputs = new FlywheelIOInputsAutoLogged(); + public TurretSubsystem() {} + private LoggedTunableNumber testDegrees = new LoggedTunableNumber("Shooter/Test Degrees", 10.0); + private LoggedTunableNumber testVelocity = new LoggedTunableNumber("Shooter/Test Velocity", 30.0); + @Override public void periodic() { // This method will be called once per scheduler run } - @Override - public Command shoot(Supplier robotPoseSupplier) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'shoot'"); - } - @Override public Command feed(Supplier robotPoseSupplier, Supplier feedTarget) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'feed'"); + return this.run( + () -> { + ShotData shotData = + AutoAim.FEED_SHOT_TREE.get( + robotPoseSupplier + .get() + .getTranslation() + .getDistance(feedTarget.get().getTranslation())); + hoodIO.setHoodPosition(shotData.hoodAngle()); + flywheelIO.setMotionProfiledFlywheelVelocity(shotData.flywheelVelocityRotPerSec()); + }); } @Override public Command rest() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'rest'"); + return this.run( + () -> { + hoodIO.setHoodPosition(HOOD_MIN_ROTATION); // TODO: TUNE TUCKED POSITION IF NEEDED + flywheelIO.setFlywheelVoltage(0.0); + }); } @Override public Command spit() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'spit'"); + return this.run( + () -> { + hoodIO.setHoodPosition(Rotation2d.kZero); + flywheelIO.setMotionProfiledFlywheelVelocity(20); + }); // TODO: TUNE HOOD POS AND FLYWHEEL VELOCITY } @Override + @AutoLogOutput(key = "Shooter/At Vel Setpoint") public boolean atFlywheelVelocitySetpoint() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'atFlywheelVelocitySetpoint'"); + return MathUtil.isNear( + flywheelInputs.flywheelLeaderVelocityRotationsPerSecond, + flywheelIO.getSetpointRotPerSec(), + FLYWHEEL_VELOCITY_TOLERANCE_ROTATIONS_PER_SECOND); } @Override + @AutoLogOutput(key = "Shooter/Hood/At Setpoint") public boolean atHoodSetpoint() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'atHoodSetpoint'"); + return MathUtil.isNear( + hoodInputs.hoodPositionRotations.getDegrees(), hoodIO.getHoodSetpoint().getDegrees(), 1); } @Override public Command zeroHood() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'zeroHood'"); + return this.runOnce(() -> hoodIO.resetEncoder(HOOD_MIN_ROTATION)); } @Override public Command testShoot() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'testShoot'"); + return this.run( + () -> { + hoodIO.setHoodPosition(Rotation2d.fromDegrees(testDegrees.get())); + flywheelIO.setMotionProfiledFlywheelVelocity(testVelocity.get()); + }); + } + + @Override + public Command shoot(Supplier robotPoseSupplier) { + return this.run( + () -> { + ShotData shotData = + AutoAim.HUB_SHOT_TREE.get(AutoAim.distanceToHub(robotPoseSupplier.get())); + hoodIO.setHoodPosition(shotData.hoodAngle()); + flywheelIO.setMotionProfiledFlywheelVelocity(shotData.flywheelVelocityRotPerSec()); + }); } } From 9c4531075f193b4367cdc09f53fafeff7d58d4c0 Mon Sep 17 00:00:00 2001 From: lQuasar9206 Date: Sun, 25 Jan 2026 13:37:31 -0800 Subject: [PATCH 2/5] more stuff fixed --- src/main/java/frc/robot/Robot.java | 2 +- .../frc/robot/subsystems/shooter/TurretSubsystem.java | 8 ++++++-- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d6d290e..03b0b27 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -259,7 +259,7 @@ public Robot() { case COMP: indexer = new SpindexerSubsystem(); intake = new LintakeSubsystem(); - shooter = new TurretSubsystem(); + shooter = new TurretSubsystem(new FlywheelIO(FlywheelIO.getFlywheelConfiguration(), canivore)); climber = new ClimberSubsystem(); // TODO climber break; } diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index 6b03696..a159762 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -14,6 +14,7 @@ import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; import java.util.function.Supplier; import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; /** Pivoting hooded shooter (turret). !! COMP !! */ public class TurretSubsystem extends SubsystemBase implements Shooter { @@ -33,14 +34,17 @@ public class TurretSubsystem extends SubsystemBase implements Shooter { FlywheelIO flywheelIO; FlywheelIOInputsAutoLogged flywheelInputs = new FlywheelIOInputsAutoLogged(); - public TurretSubsystem() {} + public TurretSubsystem(FlywheelIO flywheelIO) { + this.flywheelIO = flywheelIO; + } private LoggedTunableNumber testDegrees = new LoggedTunableNumber("Shooter/Test Degrees", 10.0); private LoggedTunableNumber testVelocity = new LoggedTunableNumber("Shooter/Test Velocity", 30.0); @Override public void periodic() { - // This method will be called once per scheduler run + flywheelIO.updateInputs(flywheelInputs); + Logger.processInputs("Shooter/Flywheel", flywheelInputs); } @Override From f9656e5dd58e0ecfc7feb0ec7d781dcbbec1d004 Mon Sep 17 00:00:00 2001 From: lQuasar9206 Date: Sun, 25 Jan 2026 13:58:42 -0800 Subject: [PATCH 3/5] comp bot case stuff --- src/main/java/frc/robot/Robot.java | 7 ++++++- .../java/frc/robot/subsystems/shooter/TurretSubsystem.java | 1 + 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 03b0b27..54cd82c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -259,7 +259,12 @@ public Robot() { case COMP: indexer = new SpindexerSubsystem(); intake = new LintakeSubsystem(); - shooter = new TurretSubsystem(new FlywheelIO(FlywheelIO.getFlywheelConfiguration(), canivore)); + shooter = + new TurretSubsystem( + ROBOT_MODE == RobotMode.REAL + ? new FlywheelIO(FlywheelIO.getFlywheelConfiguration(), canivore) + : new FlywheelIOSim(FlywheelIO.getFlywheelConfiguration(),canivore)); + climber = new ClimberSubsystem(); // TODO climber break; } diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index a159762..f0b3051 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -36,6 +36,7 @@ public class TurretSubsystem extends SubsystemBase implements Shooter { public TurretSubsystem(FlywheelIO flywheelIO) { this.flywheelIO = flywheelIO; + } private LoggedTunableNumber testDegrees = new LoggedTunableNumber("Shooter/Test Degrees", 10.0); From f0dc9b3cb7a0640bcfcd5f79fe0693de3c4201ec Mon Sep 17 00:00:00 2001 From: BlazingBora Date: Sun, 25 Jan 2026 14:19:19 -0800 Subject: [PATCH 4/5] added hood --- src/main/java/frc/robot/Robot.java | 7 ++++++- .../java/frc/robot/subsystems/shooter/TurretSubsystem.java | 3 ++- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 54cd82c..c0a1cd6 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -263,7 +263,12 @@ public Robot() { new TurretSubsystem( ROBOT_MODE == RobotMode.REAL ? new FlywheelIO(FlywheelIO.getFlywheelConfiguration(), canivore) - : new FlywheelIOSim(FlywheelIO.getFlywheelConfiguration(),canivore)); + : new FlywheelIOSim(FlywheelIO.getFlywheelConfiguration(),canivore), + + ROBOT_MODE == RobotMode.REAL + ? new HoodIO(HoodIO.getHoodConfiguration(), canivore) + : new HoodIOSim(canivore)); + climber = new ClimberSubsystem(); // TODO climber break; diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index f0b3051..30f288b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -34,8 +34,9 @@ public class TurretSubsystem extends SubsystemBase implements Shooter { FlywheelIO flywheelIO; FlywheelIOInputsAutoLogged flywheelInputs = new FlywheelIOInputsAutoLogged(); - public TurretSubsystem(FlywheelIO flywheelIO) { + public TurretSubsystem(FlywheelIO flywheelIO, HoodIO hoodIO) { this.flywheelIO = flywheelIO; + this.hoodIO = hoodIO; } From 52ef37deb6f5e3707fe847ad057f0eff2f07be75 Mon Sep 17 00:00:00 2001 From: BlazingBora Date: Sun, 25 Jan 2026 14:20:35 -0800 Subject: [PATCH 5/5] added hood --- src/main/java/frc/robot/Robot.java | 18 ++++++++---------- .../subsystems/shooter/TurretSubsystem.java | 1 - 2 files changed, 8 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index c0a1cd6..35b0438 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -259,16 +259,14 @@ public Robot() { case COMP: indexer = new SpindexerSubsystem(); intake = new LintakeSubsystem(); - shooter = - new TurretSubsystem( - ROBOT_MODE == RobotMode.REAL - ? new FlywheelIO(FlywheelIO.getFlywheelConfiguration(), canivore) - : new FlywheelIOSim(FlywheelIO.getFlywheelConfiguration(),canivore), - - ROBOT_MODE == RobotMode.REAL - ? new HoodIO(HoodIO.getHoodConfiguration(), canivore) - : new HoodIOSim(canivore)); - + shooter = + new TurretSubsystem( + ROBOT_MODE == RobotMode.REAL + ? new FlywheelIO(FlywheelIO.getFlywheelConfiguration(), canivore) + : new FlywheelIOSim(FlywheelIO.getFlywheelConfiguration(), canivore), + ROBOT_MODE == RobotMode.REAL + ? new HoodIO(HoodIO.getHoodConfiguration(), canivore) + : new HoodIOSim(canivore)); climber = new ClimberSubsystem(); // TODO climber break; diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index 30f288b..8a563b4 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -37,7 +37,6 @@ public class TurretSubsystem extends SubsystemBase implements Shooter { public TurretSubsystem(FlywheelIO flywheelIO, HoodIO hoodIO) { this.flywheelIO = flywheelIO; this.hoodIO = hoodIO; - } private LoggedTunableNumber testDegrees = new LoggedTunableNumber("Shooter/Test Degrees", 10.0);