From d53a47bdc727d635d2c384531513aa11f0f0cd65 Mon Sep 17 00:00:00 2001 From: lQuasar9206 Date: Sat, 24 Jan 2026 22:24:03 -0800 Subject: [PATCH 01/13] 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 02/13] 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 03/13] 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 04/13] 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 05/13] 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); From c7dd328bb0386d8435198698dd4cc6165c6775ca Mon Sep 17 00:00:00 2001 From: lQuasar9206 Date: Fri, 30 Jan 2026 22:21:49 -0800 Subject: [PATCH 06/13] discard this push --- src/main/java/frc/robot/Robot.java | 19 +++++++++++---- .../robot/subsystems/climber/ClimberSubsystem | 24 +++++++++++++++++++ .../subsystems/shooter/TurretSubsystem.java | 2 +- 3 files changed, 39 insertions(+), 6 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/climber/ClimberSubsystem diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 54cd82c..0c55a26 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -183,6 +183,10 @@ public void placeGamePiecesOnField() {} SimulatedArena.overrideInstance(new EvergreenArena()); } + Indexer indexer = null; + Intake intake = null; + Shooter shooter = null; + // this is here because it doesn't like that the power distribution logger is never closed @SuppressWarnings("resource") public Robot() { @@ -259,11 +263,11 @@ 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)); + shooter = + new TurretSubsystem( + ROBOT_MODE == RobotMode.REAL + ? new FlywheelIO(FlywheelIO.getFlywheelConfiguration(), canivore) + : new FlywheelIOSim(FlywheelIO.getFlywheelConfiguration(), canivore)); climber = new ClimberSubsystem(); // TODO climber break; @@ -466,6 +470,11 @@ private void addAutos() { // autoChooser.addOption("Index Roller Sysid", indexer.runRollerSysId()); // autoChooser.addOption("Intake Roller Sysid", intake.runRollerSysid()); // autoChooser.addOption("Flywheel Sysid", shooter.runFlywheelSysid()); + autoChooser.addOption("Pitcheck/Intake ", Commands.sequence( + intake.intake().withTimeout(1), + intake.rest().withTimeout(1), + intake.outtake().withTimeout(1)) + ); haveAutosGenerated = true; System.out.println("Done generating autos"); } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem new file mode 100644 index 0000000..be9467d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem @@ -0,0 +1,24 @@ +package frc.robot.subsystems.climber; + +import static edu.wpi.first.units.Units.Volts; + +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.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; +import frc.robot.utils.autoaim.AutoAim; +import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; +import java.util.function.Supplier; +import org.littletonrobotics.junction.Logger; + + + +public class ClimberSubsystem extends SubsystemBase{ + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index f0b3051..1b53b79 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -36,7 +36,6 @@ public class TurretSubsystem extends SubsystemBase implements Shooter { public TurretSubsystem(FlywheelIO flywheelIO) { this.flywheelIO = flywheelIO; - } private LoggedTunableNumber testDegrees = new LoggedTunableNumber("Shooter/Test Degrees", 10.0); @@ -46,6 +45,7 @@ public TurretSubsystem(FlywheelIO flywheelIO) { public void periodic() { flywheelIO.updateInputs(flywheelInputs); Logger.processInputs("Shooter/Flywheel", flywheelInputs); + } @Override From 65e282b7abdbabe1c13791c3d1b38c420c2f3aed Mon Sep 17 00:00:00 2001 From: lQuasar9206 Date: Fri, 30 Jan 2026 22:25:06 -0800 Subject: [PATCH 07/13] testing --- src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index a0d7ced..c4b3d94 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -46,6 +46,7 @@ public TurretSubsystem(FlywheelIO flywheelIO, HoodIO hoodIO) { public void periodic() { flywheelIO.updateInputs(flywheelInputs); Logger.processInputs("Shooter/Flywheel", flywheelInputs); + } From 147c49036a576d03b74bc4be0d9473cfdb56e6ee Mon Sep 17 00:00:00 2001 From: lQuasar9206 Date: Fri, 30 Jan 2026 22:46:32 -0800 Subject: [PATCH 08/13] still need to work out sim stuff --- src/main/java/frc/robot/Robot.java | 4 +-- .../frc/robot/subsystems/shooter/HoodIO.java | 35 +++++++++++++++++-- .../robot/subsystems/shooter/HoodIOSim.java | 8 ++--- .../subsystems/shooter/ShooterSubsystem.java | 3 +- .../subsystems/shooter/TurretSubsystem.java | 3 +- 5 files changed, 42 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 945fe9c..e34e584 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -239,7 +239,7 @@ public Robot() { shooter = new ShooterSubsystem( ROBOT_MODE == RobotMode.REAL - ? new HoodIO(HoodIO.getHoodConfiguration(), canivore) + ? new HoodIO(HoodIO.getHoodAlphaConfiguration(), canivore) : new HoodIOSim(canivore), ROBOT_MODE == RobotMode.REAL ? new FlywheelIO(FlywheelIO.getFlywheelConfiguration(), canivore) @@ -269,7 +269,7 @@ public Robot() { ? new FlywheelIO(FlywheelIO.getFlywheelConfiguration(), canivore) : new FlywheelIOSim(FlywheelIO.getFlywheelConfiguration(), canivore), ROBOT_MODE == RobotMode.REAL - ? new HoodIO(HoodIO.getHoodConfiguration(), canivore) + ? new HoodIO(HoodIO.getHoodCompConfiguration(), canivore) : new HoodIOSim(canivore)); climber = new ClimberSubsystem(); // TODO climber diff --git a/src/main/java/frc/robot/subsystems/shooter/HoodIO.java b/src/main/java/frc/robot/subsystems/shooter/HoodIO.java index 3c38920..56a7f07 100644 --- a/src/main/java/frc/robot/subsystems/shooter/HoodIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/HoodIO.java @@ -55,7 +55,7 @@ public static class HoodIOInputs { public HoodIO(TalonFXConfiguration talonFXConfiguration, CANBus canbus) { hoodMotor = new TalonFX(11, canbus); - hoodMotor.getConfigurator().apply(HoodIO.getHoodConfiguration()); + hoodMotor.getConfigurator().apply(HoodIO.getHoodAlphaConfiguration()); hoodPositionRotations = hoodMotor.getPosition(); hoodAngularVelocity = hoodMotor.getVelocity(); @@ -75,7 +75,7 @@ public HoodIO(TalonFXConfiguration talonFXConfiguration, CANBus canbus) { hoodMotor.optimizeBusUtilization(); } - public static TalonFXConfiguration getHoodConfiguration() { + public static TalonFXConfiguration getHoodAlphaConfiguration() { TalonFXConfiguration config = new TalonFXConfiguration(); config.MotorOutput.NeutralMode = NeutralModeValue.Brake; @@ -86,7 +86,7 @@ public static TalonFXConfiguration getHoodConfiguration() { config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - config.Feedback.SensorToMechanismRatio = ShooterSubsystem.HOOD_GEAR_RATIO; + config.Feedback.SensorToMechanismRatio = ShooterSubsystem.HOOD_GEAR_RATIO_A; config.Slot0.GravityType = GravityTypeValue.Arm_Cosine; @@ -103,6 +103,35 @@ public static TalonFXConfiguration getHoodConfiguration() { return config; } + public static TalonFXConfiguration getHoodCompConfiguration(){ + TalonFXConfiguration config = new TalonFXConfiguration(); + + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + + config.Feedback.SensorToMechanismRatio = ShooterSubsystem.HOOD_GEAR_RATIO_C; + + config.Slot0.GravityType = GravityTypeValue.Arm_Cosine; + + config.Slot0.kS = 0; + config.Slot0.kG = 0; + config.Slot0.kV = 0; + config.Slot0.kP = 0; + config.Slot0.kD = 0; + + config.CurrentLimits.StatorCurrentLimit = 80.0; + config.CurrentLimits.StatorCurrentLimitEnable = true; + config.CurrentLimits.SupplyCurrentLimit = 60.0; + + return config; + + } + public void setHoodVoltage(double hoodVoltage) { hoodMotor.setControl(voltageOut.withOutput(hoodVoltage)); } diff --git a/src/main/java/frc/robot/subsystems/shooter/HoodIOSim.java b/src/main/java/frc/robot/subsystems/shooter/HoodIOSim.java index 7aafe9e..2e306af 100644 --- a/src/main/java/frc/robot/subsystems/shooter/HoodIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/HoodIOSim.java @@ -17,7 +17,7 @@ public class HoodIOSim extends HoodIO { private final DCMotorSim hoodPhysicsSim = new DCMotorSim( LinearSystemId.createDCMotorSystem( - DCMotor.getKrakenX44Foc(1), 0.01, ShooterSubsystem.HOOD_GEAR_RATIO), + DCMotor.getKrakenX44Foc(1), 0.01, ShooterSubsystem.HOOD_GEAR_RATIO_A), DCMotor.getKrakenX44Foc(1)); // will get updated when i get specs @@ -27,7 +27,7 @@ public class HoodIOSim extends HoodIO { private double lastSimTime = 0.0; public HoodIOSim(CANBus canbus) { - super(HoodIO.getHoodConfiguration(), canbus); + super(HoodIO.getHoodAlphaConfiguration(), canbus); hoodMotorSim = hoodMotor.getSimState(); hoodMotorSim.setMotorType(MotorType.KrakenX44); hoodMotorSim.Orientation = ChassisReference.Clockwise_Positive; @@ -47,9 +47,9 @@ public HoodIOSim(CANBus canbus) { // rotor position stuff added later when i have access to onshape hoodMotorSim.setRawRotorPosition( - hoodPhysicsSim.getAngularPositionRad() * (ShooterSubsystem.HOOD_GEAR_RATIO)); + hoodPhysicsSim.getAngularPositionRad() * (ShooterSubsystem.HOOD_GEAR_RATIO_A)); hoodMotorSim.setRotorVelocity( - hoodPhysicsSim.getAngularVelocityRPM() / 60.0 * ShooterSubsystem.HOOD_GEAR_RATIO); + hoodPhysicsSim.getAngularVelocityRPM() / 60.0 * ShooterSubsystem.HOOD_GEAR_RATIO_A); }); simNotifier.startPeriodic(simLoopPeriod); } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index cd4ab04..3e24b9c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -25,7 +25,8 @@ /** Fixed shooter. !! ALPHA !! */ public class ShooterSubsystem extends SubsystemBase implements Shooter { - public static double HOOD_GEAR_RATIO = 24.230769; + public static double HOOD_GEAR_RATIO_A = 24.230769; + public static double HOOD_GEAR_RATIO_C = 1; public static Rotation2d HOOD_MAX_ROTATION = Rotation2d.fromDegrees(40); public static Rotation2d HOOD_MIN_ROTATION = Rotation2d.fromDegrees(2); diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index c4b3d94..141878b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -46,8 +46,9 @@ public TurretSubsystem(FlywheelIO flywheelIO, HoodIO hoodIO) { public void periodic() { flywheelIO.updateInputs(flywheelInputs); Logger.processInputs("Shooter/Flywheel", flywheelInputs); + hoodIO.updateInputs(hoodInputs); + Logger.processInputs("Shooter/Hood", hoodInputs); - } @Override From d90f86afb766fd9d209f6bdd1e6cd05cb874c48d Mon Sep 17 00:00:00 2001 From: lQuasar9206 Date: Sat, 31 Jan 2026 11:57:55 -0800 Subject: [PATCH 09/13] hood comp/alpha integration done --- src/main/java/frc/robot/Robot.java | 8 +++- .../frc/robot/subsystems/shooter/HoodIO.java | 4 +- .../robot/subsystems/shooter/HoodIOSim.java | 47 ++++++++++++++----- .../subsystems/shooter/ShooterSubsystem.java | 2 +- .../subsystems/shooter/TurretSubsystem.java | 3 +- 5 files changed, 45 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e34e584..57382ca 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -240,7 +240,7 @@ public Robot() { new ShooterSubsystem( ROBOT_MODE == RobotMode.REAL ? new HoodIO(HoodIO.getHoodAlphaConfiguration(), canivore) - : new HoodIOSim(canivore), + : new HoodIOSim(canivore,HoodIO.getHoodAlphaConfiguration(),ShooterSubsystem.HOOD_GEAR_RATIO_A), ROBOT_MODE == RobotMode.REAL ? new FlywheelIO(FlywheelIO.getFlywheelConfiguration(), canivore) : new FlywheelIOSim(FlywheelIO.getFlywheelConfiguration(), canivore)); @@ -270,7 +270,7 @@ public Robot() { : new FlywheelIOSim(FlywheelIO.getFlywheelConfiguration(), canivore), ROBOT_MODE == RobotMode.REAL ? new HoodIO(HoodIO.getHoodCompConfiguration(), canivore) - : new HoodIOSim(canivore)); + : new HoodIOSim(canivore, HoodIO.getHoodCompConfiguration(),TurretSubsystem.HOOD_GEAR_RATIO_C)); climber = new ClimberSubsystem(); // TODO climber break; @@ -300,6 +300,10 @@ public Robot() { // log if we have uncommitted changes switch (BuildConstants.DIRTY) { case 0: + + + + Logger.recordMetadata("GitDirty", "All changes committed"); break; case 1: diff --git a/src/main/java/frc/robot/subsystems/shooter/HoodIO.java b/src/main/java/frc/robot/subsystems/shooter/HoodIO.java index 56a7f07..5383d91 100644 --- a/src/main/java/frc/robot/subsystems/shooter/HoodIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/HoodIO.java @@ -55,7 +55,7 @@ public static class HoodIOInputs { public HoodIO(TalonFXConfiguration talonFXConfiguration, CANBus canbus) { hoodMotor = new TalonFX(11, canbus); - hoodMotor.getConfigurator().apply(HoodIO.getHoodAlphaConfiguration()); + hoodMotor.getConfigurator().apply(talonFXConfiguration); hoodPositionRotations = hoodMotor.getPosition(); hoodAngularVelocity = hoodMotor.getVelocity(); @@ -114,7 +114,7 @@ public static TalonFXConfiguration getHoodCompConfiguration(){ config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - config.Feedback.SensorToMechanismRatio = ShooterSubsystem.HOOD_GEAR_RATIO_C; + config.Feedback.SensorToMechanismRatio = TurretSubsystem.HOOD_GEAR_RATIO_C; config.Slot0.GravityType = GravityTypeValue.Arm_Cosine; diff --git a/src/main/java/frc/robot/subsystems/shooter/HoodIOSim.java b/src/main/java/frc/robot/subsystems/shooter/HoodIOSim.java index 2e306af..4f814e2 100644 --- a/src/main/java/frc/robot/subsystems/shooter/HoodIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/HoodIOSim.java @@ -2,6 +2,7 @@ import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.sim.ChassisReference; import com.ctre.phoenix6.sim.TalonFXSimState; import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; @@ -14,20 +15,34 @@ public class HoodIOSim extends HoodIO { TalonFXSimState hoodMotorSim; - private final DCMotorSim hoodPhysicsSim = - new DCMotorSim( - LinearSystemId.createDCMotorSystem( - DCMotor.getKrakenX44Foc(1), 0.01, ShooterSubsystem.HOOD_GEAR_RATIO_A), - DCMotor.getKrakenX44Foc(1)); - + private final DCMotorSim hoodPhysicsSim; + // will get updated when i get specs private final double simLoopPeriod = 0.002; // 2 ms private Notifier simNotifier = null; private double lastSimTime = 0.0; - public HoodIOSim(CANBus canbus) { - super(HoodIO.getHoodAlphaConfiguration(), canbus); + + + + + + + + + + + + + public HoodIOSim(CANBus canbus, TalonFXConfiguration config, double gearRatio ) { + super(config, canbus); + hoodPhysicsSim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem( + DCMotor.getKrakenX44Foc(1), 0.01,gearRatio), + DCMotor.getKrakenX44Foc(1)); + hoodMotorSim = hoodMotor.getSimState(); hoodMotorSim.setMotorType(MotorType.KrakenX44); hoodMotorSim.Orientation = ChassisReference.Clockwise_Positive; @@ -45,12 +60,18 @@ public HoodIOSim(CANBus canbus) { hoodPhysicsSim.update(deltaTime); // rotor position stuff added later when i have access to onshape - - hoodMotorSim.setRawRotorPosition( - hoodPhysicsSim.getAngularPositionRad() * (ShooterSubsystem.HOOD_GEAR_RATIO_A)); - hoodMotorSim.setRotorVelocity( - hoodPhysicsSim.getAngularVelocityRPM() / 60.0 * ShooterSubsystem.HOOD_GEAR_RATIO_A); + + hoodMotorSim.setRawRotorPosition( + hoodPhysicsSim.getAngularPositionRad() * (gearRatio)); + hoodMotorSim.setRotorVelocity( + hoodPhysicsSim.getAngularVelocityRPM() / 60.0 * gearRatio); + + + + }); + simNotifier.startPeriodic(simLoopPeriod); } } + diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 3e24b9c..b7feb93 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -26,7 +26,7 @@ /** Fixed shooter. !! ALPHA !! */ public class ShooterSubsystem extends SubsystemBase implements Shooter { public static double HOOD_GEAR_RATIO_A = 24.230769; - public static double HOOD_GEAR_RATIO_C = 1; + public static Rotation2d HOOD_MAX_ROTATION = Rotation2d.fromDegrees(40); public static Rotation2d HOOD_MIN_ROTATION = Rotation2d.fromDegrees(2); diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index 141878b..e12a6e5 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -19,7 +19,8 @@ /** 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 double HOOD_GEAR_RATIO = 1; + public static double HOOD_GEAR_RATIO_C = 1; public static Rotation2d HOOD_MAX_ROTATION = Rotation2d.fromDegrees(40); public static Rotation2d HOOD_MIN_ROTATION = Rotation2d.fromDegrees(2); From 85804a461c8d166806205a66d20fbdae3e2b565d Mon Sep 17 00:00:00 2001 From: lQuasar9206 Date: Sat, 31 Jan 2026 12:08:04 -0800 Subject: [PATCH 10/13] hopefully shooter is done --- src/main/java/frc/robot/Robot.java | 8 +++--- .../robot/subsystems/shooter/FlywheelIO.java | 25 ++++++++++++++++++- .../subsystems/shooter/FlywheelIOSim.java | 19 ++++++++------ .../subsystems/shooter/TurretSubsystem.java | 2 +- 4 files changed, 40 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 57382ca..4a20ca8 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -242,8 +242,8 @@ public Robot() { ? new HoodIO(HoodIO.getHoodAlphaConfiguration(), canivore) : new HoodIOSim(canivore,HoodIO.getHoodAlphaConfiguration(),ShooterSubsystem.HOOD_GEAR_RATIO_A), ROBOT_MODE == RobotMode.REAL - ? new FlywheelIO(FlywheelIO.getFlywheelConfiguration(), canivore) - : new FlywheelIOSim(FlywheelIO.getFlywheelConfiguration(), canivore)); + ? new FlywheelIO(FlywheelIO.getFlywheelAlphaConfiguration(), canivore) + : new FlywheelIOSim(FlywheelIO.getFlywheelAlphaConfiguration(), canivore, ShooterSubsystem.FLYWHEEL_GEAR_RATIO)); intake = new FintakeSubsystem( @@ -266,8 +266,8 @@ public Robot() { shooter = new TurretSubsystem( ROBOT_MODE == RobotMode.REAL - ? new FlywheelIO(FlywheelIO.getFlywheelConfiguration(), canivore) - : new FlywheelIOSim(FlywheelIO.getFlywheelConfiguration(), canivore), + ? new FlywheelIO(FlywheelIO.getFlywheelCompConfiguration(), canivore) + : new FlywheelIOSim(FlywheelIO.getFlywheelCompConfiguration(), canivore, TurretSubsystem.FLYWHEEL_GEAR_RATIO_C), ROBOT_MODE == RobotMode.REAL ? new HoodIO(HoodIO.getHoodCompConfiguration(), canivore) : new HoodIOSim(canivore, HoodIO.getHoodCompConfiguration(),TurretSubsystem.HOOD_GEAR_RATIO_C)); diff --git a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java b/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java index 304e65b..7cdabaa 100644 --- a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java @@ -113,7 +113,7 @@ public FlywheelIO(TalonFXConfiguration config, CANBus canbus) { flywheelFollower.optimizeBusUtilization(); } - public static TalonFXConfiguration getFlywheelConfiguration() { + public static TalonFXConfiguration getFlywheelAlphaConfiguration() { TalonFXConfiguration config = new TalonFXConfiguration(); config.MotorOutput.NeutralMode = NeutralModeValue.Coast; @@ -136,6 +136,29 @@ public static TalonFXConfiguration getFlywheelConfiguration() { return config; } + public static TalonFXConfiguration getFlywheelCompConfiguration() { + TalonFXConfiguration config = new TalonFXConfiguration(); + + config.MotorOutput.NeutralMode = NeutralModeValue.Coast; + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + + config.Feedback.SensorToMechanismRatio = TurretSubsystem.FLYWHEEL_GEAR_RATIO_C; + + config.Slot0.kS = 0; + config.Slot0.kV = 0; + config.Slot0.kA = 0; + config.Slot0.kP = 0; + config.Slot0.kD = 0; + + config.CurrentLimits.StatorCurrentLimit = 120.0; + config.CurrentLimits.StatorCurrentLimitEnable = true; + config.CurrentLimits.SupplyCurrentLimit = 80.0; + + config.MotionMagic.MotionMagicAcceleration = 100.0; + + return config; + } + public void setFlywheelVoltage(double volts) { flywheelLeader.setControl(voltageOut.withOutput(volts)); } diff --git a/src/main/java/frc/robot/subsystems/shooter/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/shooter/FlywheelIOSim.java index 82910fb..828b57c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/FlywheelIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/FlywheelIOSim.java @@ -22,18 +22,21 @@ /** Add your docs here. */ public class FlywheelIOSim extends FlywheelIO { TalonFXSimState leaderFxSimState; - DCMotorSim physicsSim = - new DCMotorSim( - LinearSystemId.createDCMotorSystem( - DCMotor.getKrakenX60Foc(2), 0.0136, ShooterSubsystem.FLYWHEEL_GEAR_RATIO), - DCMotor.getKrakenX60Foc(2)); + DCMotorSim physicsSim; + private final double simLoopPeriod = 0.002; private Notifier simNotifier; private double lastSimTime = 0.0; - public FlywheelIOSim(TalonFXConfiguration config, CANBus canbus) { + public FlywheelIOSim(TalonFXConfiguration config, CANBus canbus, double gearRatio) { + super(config, canbus); + physicsSim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem( + DCMotor.getKrakenX60Foc(2), 0.0136, gearRatio), + DCMotor.getKrakenX60Foc(2)); leaderFxSimState = flywheelLeader.getSimState(); leaderFxSimState.setMotorType(MotorType.KrakenX60); leaderFxSimState.Orientation = ChassisReference.CounterClockwise_Positive; @@ -51,9 +54,9 @@ public FlywheelIOSim(TalonFXConfiguration config, CANBus canbus) { physicsSim.update(deltaTime); leaderFxSimState.setRawRotorPosition( - physicsSim.getAngularPosition().in(Rotations) * physicsSim.getGearing()); + physicsSim.getAngularPosition().in(Rotations) * gearRatio); leaderFxSimState.setRotorVelocity( - physicsSim.getAngularVelocity().in(RotationsPerSecond) * physicsSim.getGearing()); + physicsSim.getAngularVelocity().in(RotationsPerSecond) * gearRatio); }); simNotifier.startPeriodic(simLoopPeriod); diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index e12a6e5..af4bde0 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -19,8 +19,8 @@ /** Pivoting hooded shooter (turret). !! COMP !! */ public class TurretSubsystem extends SubsystemBase implements Shooter { /** Creates a new TurretSubsystem. */ - public static double HOOD_GEAR_RATIO = 1; public static double HOOD_GEAR_RATIO_C = 1; + public static double FLYWHEEL_GEAR_RATIO_C = 1; public static Rotation2d HOOD_MAX_ROTATION = Rotation2d.fromDegrees(40); public static Rotation2d HOOD_MIN_ROTATION = Rotation2d.fromDegrees(2); From 7b43f6c918d615f3be8e8e8582a66cd241cb1a8a Mon Sep 17 00:00:00 2001 From: lQuasar9206 Date: Sun, 1 Feb 2026 20:11:06 -0800 Subject: [PATCH 11/13] real gear ratios --- .../java/frc/robot/subsystems/shooter/TurretSubsystem.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index af4bde0..edb4d1b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -19,13 +19,13 @@ /** Pivoting hooded shooter (turret). !! COMP !! */ public class TurretSubsystem extends SubsystemBase implements Shooter { /** Creates a new TurretSubsystem. */ - public static double HOOD_GEAR_RATIO_C = 1; - public static double FLYWHEEL_GEAR_RATIO_C = 1; + public static double HOOD_GEAR_RATIO_C = 58.96875; + public static double FLYWHEEL_GEAR_RATIO_C = 0.84615384615; 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; From 09d761ec46901260cfc03b18f91c8cb036f18307 Mon Sep 17 00:00:00 2001 From: lQuasar9206 Date: Sun, 1 Feb 2026 20:34:51 -0800 Subject: [PATCH 12/13] renamed gearratios to prevent confusion --- src/main/java/frc/robot/Robot.java | 41 +++++++++++-------- .../robot/subsystems/shooter/FlywheelIO.java | 2 +- .../subsystems/shooter/FlywheelIOSim.java | 12 +++--- .../frc/robot/subsystems/shooter/HoodIO.java | 3 +- .../robot/subsystems/shooter/HoodIOSim.java | 38 +++++------------ .../subsystems/shooter/ShooterSubsystem.java | 4 +- .../subsystems/shooter/TurretSubsystem.java | 4 +- 7 files changed, 45 insertions(+), 59 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4a20ca8..ce9b804 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -183,9 +183,9 @@ public void placeGamePiecesOnField() {} SimulatedArena.overrideInstance(new EvergreenArena()); } - Indexer indexer = null; - Intake intake = null; - Shooter shooter = null; + Indexer indexer = null; + Intake intake = null; + Shooter shooter = null; // this is here because it doesn't like that the power distribution logger is never closed @SuppressWarnings("resource") @@ -240,10 +240,16 @@ public Robot() { new ShooterSubsystem( ROBOT_MODE == RobotMode.REAL ? new HoodIO(HoodIO.getHoodAlphaConfiguration(), canivore) - : new HoodIOSim(canivore,HoodIO.getHoodAlphaConfiguration(),ShooterSubsystem.HOOD_GEAR_RATIO_A), + : new HoodIOSim( + canivore, + HoodIO.getHoodAlphaConfiguration(), + ShooterSubsystem.HOOD_GEAR_RATIO_A), ROBOT_MODE == RobotMode.REAL ? new FlywheelIO(FlywheelIO.getFlywheelAlphaConfiguration(), canivore) - : new FlywheelIOSim(FlywheelIO.getFlywheelAlphaConfiguration(), canivore, ShooterSubsystem.FLYWHEEL_GEAR_RATIO)); + : new FlywheelIOSim( + FlywheelIO.getFlywheelAlphaConfiguration(), + canivore, + ShooterSubsystem.FLYWHEEL_GEAR_RATIO_A)); intake = new FintakeSubsystem( @@ -267,10 +273,16 @@ public Robot() { new TurretSubsystem( ROBOT_MODE == RobotMode.REAL ? new FlywheelIO(FlywheelIO.getFlywheelCompConfiguration(), canivore) - : new FlywheelIOSim(FlywheelIO.getFlywheelCompConfiguration(), canivore, TurretSubsystem.FLYWHEEL_GEAR_RATIO_C), + : new FlywheelIOSim( + FlywheelIO.getFlywheelCompConfiguration(), + canivore, + TurretSubsystem.FLYWHEEL_GEAR_RATIO_C), ROBOT_MODE == RobotMode.REAL ? new HoodIO(HoodIO.getHoodCompConfiguration(), canivore) - : new HoodIOSim(canivore, HoodIO.getHoodCompConfiguration(),TurretSubsystem.HOOD_GEAR_RATIO_C)); + : new HoodIOSim( + canivore, + HoodIO.getHoodCompConfiguration(), + TurretSubsystem.HOOD_GEAR_RATIO_C)); climber = new ClimberSubsystem(); // TODO climber break; @@ -300,10 +312,6 @@ public Robot() { // log if we have uncommitted changes switch (BuildConstants.DIRTY) { case 0: - - - - Logger.recordMetadata("GitDirty", "All changes committed"); break; case 1: @@ -477,11 +485,12 @@ private void addAutos() { // autoChooser.addOption("Index Roller Sysid", indexer.runRollerSysId()); // autoChooser.addOption("Intake Roller Sysid", intake.runRollerSysid()); // autoChooser.addOption("Flywheel Sysid", shooter.runFlywheelSysid()); - autoChooser.addOption("Pitcheck/Intake ", Commands.sequence( - intake.intake().withTimeout(1), - intake.rest().withTimeout(1), - intake.outtake().withTimeout(1)) - ); + autoChooser.addOption( + "Pitcheck/Intake ", + Commands.sequence( + intake.intake().withTimeout(1), + intake.rest().withTimeout(1), + intake.outtake().withTimeout(1))); haveAutosGenerated = true; System.out.println("Done generating autos"); } diff --git a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java b/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java index 7cdabaa..43f6e66 100644 --- a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java @@ -119,7 +119,7 @@ public static TalonFXConfiguration getFlywheelAlphaConfiguration() { config.MotorOutput.NeutralMode = NeutralModeValue.Coast; config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - config.Feedback.SensorToMechanismRatio = ShooterSubsystem.FLYWHEEL_GEAR_RATIO; + config.Feedback.SensorToMechanismRatio = ShooterSubsystem.FLYWHEEL_GEAR_RATIO_A; config.Slot0.kS = 0.43477; config.Slot0.kV = 0.144; diff --git a/src/main/java/frc/robot/subsystems/shooter/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/shooter/FlywheelIOSim.java index 828b57c..81455b3 100644 --- a/src/main/java/frc/robot/subsystems/shooter/FlywheelIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/FlywheelIOSim.java @@ -22,8 +22,7 @@ /** Add your docs here. */ public class FlywheelIOSim extends FlywheelIO { TalonFXSimState leaderFxSimState; - DCMotorSim physicsSim; - + DCMotorSim physicsSim; private final double simLoopPeriod = 0.002; private Notifier simNotifier; @@ -32,11 +31,10 @@ public class FlywheelIOSim extends FlywheelIO { public FlywheelIOSim(TalonFXConfiguration config, CANBus canbus, double gearRatio) { super(config, canbus); - physicsSim = - new DCMotorSim( - LinearSystemId.createDCMotorSystem( - DCMotor.getKrakenX60Foc(2), 0.0136, gearRatio), - DCMotor.getKrakenX60Foc(2)); + physicsSim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60Foc(2), 0.0136, gearRatio), + DCMotor.getKrakenX60Foc(2)); leaderFxSimState = flywheelLeader.getSimState(); leaderFxSimState.setMotorType(MotorType.KrakenX60); leaderFxSimState.Orientation = ChassisReference.CounterClockwise_Positive; diff --git a/src/main/java/frc/robot/subsystems/shooter/HoodIO.java b/src/main/java/frc/robot/subsystems/shooter/HoodIO.java index 5383d91..7a26750 100644 --- a/src/main/java/frc/robot/subsystems/shooter/HoodIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/HoodIO.java @@ -103,7 +103,7 @@ public static TalonFXConfiguration getHoodAlphaConfiguration() { return config; } - public static TalonFXConfiguration getHoodCompConfiguration(){ + public static TalonFXConfiguration getHoodCompConfiguration() { TalonFXConfiguration config = new TalonFXConfiguration(); config.MotorOutput.NeutralMode = NeutralModeValue.Brake; @@ -129,7 +129,6 @@ public static TalonFXConfiguration getHoodCompConfiguration(){ config.CurrentLimits.SupplyCurrentLimit = 60.0; return config; - } public void setHoodVoltage(double hoodVoltage) { diff --git a/src/main/java/frc/robot/subsystems/shooter/HoodIOSim.java b/src/main/java/frc/robot/subsystems/shooter/HoodIOSim.java index 4f814e2..41858f1 100644 --- a/src/main/java/frc/robot/subsystems/shooter/HoodIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/HoodIOSim.java @@ -16,33 +16,20 @@ public class HoodIOSim extends HoodIO { TalonFXSimState hoodMotorSim; private final DCMotorSim hoodPhysicsSim; - + // will get updated when i get specs private final double simLoopPeriod = 0.002; // 2 ms private Notifier simNotifier = null; private double lastSimTime = 0.0; - - - - - - - - - - - - - public HoodIOSim(CANBus canbus, TalonFXConfiguration config, double gearRatio ) { + public HoodIOSim(CANBus canbus, TalonFXConfiguration config, double gearRatio) { super(config, canbus); hoodPhysicsSim = - new DCMotorSim( - LinearSystemId.createDCMotorSystem( - DCMotor.getKrakenX44Foc(1), 0.01,gearRatio), - DCMotor.getKrakenX44Foc(1)); - + new DCMotorSim( + LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX44Foc(1), 0.01, gearRatio), + DCMotor.getKrakenX44Foc(1)); + hoodMotorSim = hoodMotor.getSimState(); hoodMotorSim.setMotorType(MotorType.KrakenX44); hoodMotorSim.Orientation = ChassisReference.Clockwise_Positive; @@ -60,18 +47,13 @@ public HoodIOSim(CANBus canbus, TalonFXConfiguration config, double gearRatio ) hoodPhysicsSim.update(deltaTime); // rotor position stuff added later when i have access to onshape - - hoodMotorSim.setRawRotorPosition( + + hoodMotorSim.setRawRotorPosition( hoodPhysicsSim.getAngularPositionRad() * (gearRatio)); - hoodMotorSim.setRotorVelocity( + hoodMotorSim.setRotorVelocity( hoodPhysicsSim.getAngularVelocityRPM() / 60.0 * gearRatio); - - - - }); - + simNotifier.startPeriodic(simLoopPeriod); } } - diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index b7feb93..6431d57 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -26,11 +26,11 @@ /** Fixed shooter. !! ALPHA !! */ public class ShooterSubsystem extends SubsystemBase implements Shooter { public static double HOOD_GEAR_RATIO_A = 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_GEAR_RATIO_A = 28.0 / 24.0; public static double FLYWHEEL_VELOCITY_TOLERANCE_ROTATIONS_PER_SECOND = 5.0; diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index edb4d1b..807dbef 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -20,13 +20,12 @@ public class TurretSubsystem extends SubsystemBase implements Shooter { /** Creates a new TurretSubsystem. */ public static double HOOD_GEAR_RATIO_C = 58.96875; + public static double FLYWHEEL_GEAR_RATIO_C = 0.84615384615; public static Rotation2d HOOD_MAX_ROTATION = Rotation2d.fromDegrees(40); public static Rotation2d HOOD_MIN_ROTATION = Rotation2d.fromDegrees(2); - - public static double FLYWHEEL_VELOCITY_TOLERANCE_ROTATIONS_PER_SECOND = 5.0; HoodIO hoodIO; @@ -49,7 +48,6 @@ public void periodic() { Logger.processInputs("Shooter/Flywheel", flywheelInputs); hoodIO.updateInputs(hoodInputs); Logger.processInputs("Shooter/Hood", hoodInputs); - } @Override From 5c47735138f2bffbdc897f77afcf8629921475a3 Mon Sep 17 00:00:00 2001 From: lQuasar9206 Date: Mon, 2 Feb 2026 21:06:16 -0800 Subject: [PATCH 13/13] fixed some stuff --- src/main/java/frc/robot/Robot.java | 58 +++++++------------ .../robot/subsystems/climber/ClimberSubsystem | 24 -------- .../subsystems/climber/ClimberSubsystem.java | 17 ------ .../robot/subsystems/shooter/FlywheelIO.java | 14 ++--- .../subsystems/shooter/FlywheelIOSim.java | 5 +- .../frc/robot/subsystems/shooter/HoodIO.java | 12 ++-- .../robot/subsystems/shooter/HoodIOSim.java | 4 +- .../subsystems/shooter/ShooterSubsystem.java | 4 +- .../subsystems/shooter/TurretSubsystem.java | 22 ++++++- 9 files changed, 60 insertions(+), 100 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/climber/ClimberSubsystem delete mode 100644 src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ce9b804..87d1bee 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -30,7 +30,6 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.components.rollers.RollerIO; import frc.robot.components.rollers.RollerIOSim; -import frc.robot.subsystems.climber.ClimberSubsystem; import frc.robot.subsystems.indexer.Indexer; import frc.robot.subsystems.indexer.LindexerSubsystem; import frc.robot.subsystems.indexer.SpindexerSubsystem; @@ -150,7 +149,6 @@ public enum RobotEdition { private final LEDSubsystem leds; // climber only exists for the comp bot - this is accounted for later - private ClimberSubsystem climber; private final Superstructure superstructure; @@ -183,10 +181,6 @@ public void placeGamePiecesOnField() {} SimulatedArena.overrideInstance(new EvergreenArena()); } - Indexer indexer = null; - Intake intake = null; - Shooter shooter = null; - // this is here because it doesn't like that the power distribution logger is never closed @SuppressWarnings("resource") public Robot() { @@ -239,17 +233,17 @@ public Robot() { shooter = new ShooterSubsystem( ROBOT_MODE == RobotMode.REAL - ? new HoodIO(HoodIO.getHoodAlphaConfiguration(), canivore) + ? new HoodIO(HoodIO.getAlphaHood(), canivore, 11) : new HoodIOSim( - canivore, - HoodIO.getHoodAlphaConfiguration(), - ShooterSubsystem.HOOD_GEAR_RATIO_A), + canivore, HoodIO.getAlphaHood(), ShooterSubsystem.HOOD_GEAR_RATIO, 11), ROBOT_MODE == RobotMode.REAL - ? new FlywheelIO(FlywheelIO.getFlywheelAlphaConfiguration(), canivore) + ? new FlywheelIO(FlywheelIO.getAlphaFlywheel(), canivore, 12, 13) : new FlywheelIOSim( - FlywheelIO.getFlywheelAlphaConfiguration(), + FlywheelIO.getAlphaFlywheel(), canivore, - ShooterSubsystem.FLYWHEEL_GEAR_RATIO_A)); + ShooterSubsystem.FLYWHEEL_GEAR_RATIO, + 11, + 12)); intake = new FintakeSubsystem( @@ -272,19 +266,19 @@ public Robot() { shooter = new TurretSubsystem( ROBOT_MODE == RobotMode.REAL - ? new FlywheelIO(FlywheelIO.getFlywheelCompConfiguration(), canivore) + ? new FlywheelIO(FlywheelIO.getCompFlywheel(), canivore, 12, 13) : new FlywheelIOSim( - FlywheelIO.getFlywheelCompConfiguration(), + FlywheelIO.getCompFlywheel(), canivore, - TurretSubsystem.FLYWHEEL_GEAR_RATIO_C), + TurretSubsystem.FLYWHEEL_GEAR_RATIO, + 11, + 12), ROBOT_MODE == RobotMode.REAL - ? new HoodIO(HoodIO.getHoodCompConfiguration(), canivore) + ? new HoodIO(HoodIO.getCompHood(), canivore, 11) : new HoodIOSim( - canivore, - HoodIO.getHoodCompConfiguration(), - TurretSubsystem.HOOD_GEAR_RATIO_C)); + canivore, HoodIO.getCompHood(), TurretSubsystem.HOOD_GEAR_RATIO, 11)); - climber = new ClimberSubsystem(); // TODO climber + // TODO climber break; } // now that we've assigned the correct subsystems based on robot edition, we can pass them into @@ -293,8 +287,6 @@ public Robot() { // if this is alpha, we won't have assigned a climber yet // this creates a placeholder "no-operation" climber that will just not do anything, but is not // null (and we need it to be not null) - if (climber == null) - climber = new ClimberSubsystem(); // TODO new ClimberSubsystem(new ClimberIO() {}) and such DriverStation.silenceJoystickConnectionWarning(true); SignalLogger.enableAutoLogging(false); @@ -479,22 +471,14 @@ private void addAutos() { System.out.println("------- Regenerating Autos"); System.out.println( "Regenerating Autos on " + DriverStation.getAlliance().map((a) -> a.toString())); - - // Sysid Autos - // autoChooser.addOption("Hood Sysid", shooter.runHoodSysid()); - // autoChooser.addOption("Index Roller Sysid", indexer.runRollerSysId()); - // autoChooser.addOption("Intake Roller Sysid", intake.runRollerSysid()); - // autoChooser.addOption("Flywheel Sysid", shooter.runFlywheelSysid()); - autoChooser.addOption( - "Pitcheck/Intake ", - Commands.sequence( - intake.intake().withTimeout(1), - intake.rest().withTimeout(1), - intake.outtake().withTimeout(1))); - haveAutosGenerated = true; - System.out.println("Done generating autos"); } + // Sysid Autos + // autoChooser.addOption("Hood Sysid", shooter.runHoodSysid()); + // autoChooser.addOption("Index Roller Sysid", indexer.runRollerSysId()); + // autoChooser.addOption("Intake Roller Sysid", intake.runRollerSysid()); + // autoChooser.addOption("Flywheel Sysid", shooter.runFlywheelSysid()); + @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem deleted file mode 100644 index be9467d..0000000 --- a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem +++ /dev/null @@ -1,24 +0,0 @@ -package frc.robot.subsystems.climber; - -import static edu.wpi.first.units.Units.Volts; - -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.Commands; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; -import frc.robot.utils.autoaim.AutoAim; -import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; -import java.util.function.Supplier; -import org.littletonrobotics.junction.Logger; - - - -public class ClimberSubsystem extends SubsystemBase{ - -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java deleted file mode 100644 index 2b5d698..0000000 --- a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java +++ /dev/null @@ -1,17 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems.climber; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class ClimberSubsystem extends SubsystemBase { - /** Creates a new ClimberSubsystem. */ - public ClimberSubsystem() {} - - @Override - public void periodic() { - // This method will be called once per scheduler run - } -} diff --git a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java b/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java index 43f6e66..c8471d3 100644 --- a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java @@ -66,9 +66,9 @@ public static class FlywheelIOInputs { // todo: tune acceleration - public FlywheelIO(TalonFXConfiguration config, CANBus canbus) { - flywheelLeader = new TalonFX(12, canbus); - flywheelFollower = new TalonFX(13, canbus); + public FlywheelIO(TalonFXConfiguration config, CANBus canbus, int leaderID, int followerID) { + flywheelLeader = new TalonFX(leaderID, canbus); + flywheelFollower = new TalonFX(followerID, canbus); flywheelLeader.getConfigurator().apply(config); flywheelFollower.getConfigurator().apply(config); @@ -113,13 +113,13 @@ public FlywheelIO(TalonFXConfiguration config, CANBus canbus) { flywheelFollower.optimizeBusUtilization(); } - public static TalonFXConfiguration getFlywheelAlphaConfiguration() { + public static TalonFXConfiguration getAlphaFlywheel() { TalonFXConfiguration config = new TalonFXConfiguration(); config.MotorOutput.NeutralMode = NeutralModeValue.Coast; config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - config.Feedback.SensorToMechanismRatio = ShooterSubsystem.FLYWHEEL_GEAR_RATIO_A; + config.Feedback.SensorToMechanismRatio = ShooterSubsystem.FLYWHEEL_GEAR_RATIO; config.Slot0.kS = 0.43477; config.Slot0.kV = 0.144; @@ -136,13 +136,13 @@ public static TalonFXConfiguration getFlywheelAlphaConfiguration() { return config; } - public static TalonFXConfiguration getFlywheelCompConfiguration() { + public static TalonFXConfiguration getCompFlywheel() { TalonFXConfiguration config = new TalonFXConfiguration(); config.MotorOutput.NeutralMode = NeutralModeValue.Coast; config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - config.Feedback.SensorToMechanismRatio = TurretSubsystem.FLYWHEEL_GEAR_RATIO_C; + config.Feedback.SensorToMechanismRatio = TurretSubsystem.FLYWHEEL_GEAR_RATIO; config.Slot0.kS = 0; config.Slot0.kV = 0; diff --git a/src/main/java/frc/robot/subsystems/shooter/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/shooter/FlywheelIOSim.java index 81455b3..8e1ca9b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/FlywheelIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/FlywheelIOSim.java @@ -28,9 +28,10 @@ public class FlywheelIOSim extends FlywheelIO { private Notifier simNotifier; private double lastSimTime = 0.0; - public FlywheelIOSim(TalonFXConfiguration config, CANBus canbus, double gearRatio) { + public FlywheelIOSim( + TalonFXConfiguration config, CANBus canbus, double gearRatio, int leaderID, int followerID) { - super(config, canbus); + super(config, canbus, leaderID, followerID); physicsSim = new DCMotorSim( LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60Foc(2), 0.0136, gearRatio), diff --git a/src/main/java/frc/robot/subsystems/shooter/HoodIO.java b/src/main/java/frc/robot/subsystems/shooter/HoodIO.java index 7a26750..91860be 100644 --- a/src/main/java/frc/robot/subsystems/shooter/HoodIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/HoodIO.java @@ -53,8 +53,8 @@ public static class HoodIOInputs { private Rotation2d hoodSetpoint = Rotation2d.kZero; - public HoodIO(TalonFXConfiguration talonFXConfiguration, CANBus canbus) { - hoodMotor = new TalonFX(11, canbus); + public HoodIO(TalonFXConfiguration talonFXConfiguration, CANBus canbus, int deviceID) { + hoodMotor = new TalonFX(deviceID, canbus); hoodMotor.getConfigurator().apply(talonFXConfiguration); hoodPositionRotations = hoodMotor.getPosition(); @@ -75,7 +75,7 @@ public HoodIO(TalonFXConfiguration talonFXConfiguration, CANBus canbus) { hoodMotor.optimizeBusUtilization(); } - public static TalonFXConfiguration getHoodAlphaConfiguration() { + public static TalonFXConfiguration getAlphaHood() { TalonFXConfiguration config = new TalonFXConfiguration(); config.MotorOutput.NeutralMode = NeutralModeValue.Brake; @@ -86,7 +86,7 @@ public static TalonFXConfiguration getHoodAlphaConfiguration() { config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - config.Feedback.SensorToMechanismRatio = ShooterSubsystem.HOOD_GEAR_RATIO_A; + config.Feedback.SensorToMechanismRatio = ShooterSubsystem.HOOD_GEAR_RATIO; config.Slot0.GravityType = GravityTypeValue.Arm_Cosine; @@ -103,7 +103,7 @@ public static TalonFXConfiguration getHoodAlphaConfiguration() { return config; } - public static TalonFXConfiguration getHoodCompConfiguration() { + public static TalonFXConfiguration getCompHood() { TalonFXConfiguration config = new TalonFXConfiguration(); config.MotorOutput.NeutralMode = NeutralModeValue.Brake; @@ -114,7 +114,7 @@ public static TalonFXConfiguration getHoodCompConfiguration() { config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - config.Feedback.SensorToMechanismRatio = TurretSubsystem.HOOD_GEAR_RATIO_C; + config.Feedback.SensorToMechanismRatio = TurretSubsystem.HOOD_GEAR_RATIO; config.Slot0.GravityType = GravityTypeValue.Arm_Cosine; diff --git a/src/main/java/frc/robot/subsystems/shooter/HoodIOSim.java b/src/main/java/frc/robot/subsystems/shooter/HoodIOSim.java index 41858f1..a8e57b2 100644 --- a/src/main/java/frc/robot/subsystems/shooter/HoodIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/HoodIOSim.java @@ -23,8 +23,8 @@ public class HoodIOSim extends HoodIO { private Notifier simNotifier = null; private double lastSimTime = 0.0; - public HoodIOSim(CANBus canbus, TalonFXConfiguration config, double gearRatio) { - super(config, canbus); + public HoodIOSim(CANBus canbus, TalonFXConfiguration config, double gearRatio, int deviceID) { + super(config, canbus, deviceID); hoodPhysicsSim = new DCMotorSim( LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX44Foc(1), 0.01, gearRatio), diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 6431d57..20b80af 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -25,12 +25,12 @@ /** Fixed shooter. !! ALPHA !! */ public class ShooterSubsystem extends SubsystemBase implements Shooter { - public static double HOOD_GEAR_RATIO_A = 24.230769; + 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_A = 28.0 / 24.0; + public static double FLYWHEEL_GEAR_RATIO = 28.0 / 24.0; public static double FLYWHEEL_VELOCITY_TOLERANCE_ROTATIONS_PER_SECOND = 5.0; diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index 807dbef..9a2fb8f 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -5,10 +5,13 @@ package frc.robot.subsystems.shooter; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.LinearFilter; 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.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.utils.LoggedTunableNumber; import frc.robot.utils.autoaim.AutoAim; import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; @@ -19,14 +22,16 @@ /** Pivoting hooded shooter (turret). !! COMP !! */ public class TurretSubsystem extends SubsystemBase implements Shooter { /** Creates a new TurretSubsystem. */ - public static double HOOD_GEAR_RATIO_C = 58.96875; + public static double HOOD_GEAR_RATIO = 58.96875; - public static double FLYWHEEL_GEAR_RATIO_C = 0.84615384615; + public static double FLYWHEEL_GEAR_RATIO = 0.84615384615; public static Rotation2d HOOD_MAX_ROTATION = Rotation2d.fromDegrees(40); public static Rotation2d HOOD_MIN_ROTATION = Rotation2d.fromDegrees(2); + public static double CURRENT_ZERO_THRESHOLD = 30.0; public static double FLYWHEEL_VELOCITY_TOLERANCE_ROTATIONS_PER_SECOND = 5.0; + double currentFilterValue = 0.0; HoodIO hoodIO; HoodIOInputsAutoLogged hoodInputs = new HoodIOInputsAutoLogged(); @@ -34,6 +39,8 @@ public class TurretSubsystem extends SubsystemBase implements Shooter { FlywheelIO flywheelIO; FlywheelIOInputsAutoLogged flywheelInputs = new FlywheelIOInputsAutoLogged(); + private LinearFilter currentFilter = LinearFilter.movingAverage(10); + public TurretSubsystem(FlywheelIO flywheelIO, HoodIO hoodIO) { this.flywheelIO = flywheelIO; this.hoodIO = hoodIO; @@ -48,6 +55,8 @@ public void periodic() { Logger.processInputs("Shooter/Flywheel", flywheelInputs); hoodIO.updateInputs(hoodInputs); Logger.processInputs("Shooter/Hood", hoodInputs); + + currentFilterValue = currentFilter.calculate(hoodInputs.hoodStatorCurrentAmps); } @Override @@ -78,7 +87,7 @@ public Command rest() { public Command spit() { return this.run( () -> { - hoodIO.setHoodPosition(Rotation2d.kZero); + hoodIO.setHoodPosition(HOOD_MIN_ROTATION); flywheelIO.setMotionProfiledFlywheelVelocity(20); }); // TODO: TUNE HOOD POS AND FLYWHEEL VELOCITY } @@ -104,6 +113,13 @@ public Command zeroHood() { return this.runOnce(() -> hoodIO.resetEncoder(HOOD_MIN_ROTATION)); } + public Command runCurrentZeroing() { + return this.run(() -> hoodIO.setHoodVoltage(-3.0)) + .until( + new Trigger(() -> Math.abs(currentFilterValue) > CURRENT_ZERO_THRESHOLD).debounce(0.25)) + .andThen(Commands.parallel(Commands.print("Hood Zeroed"), zeroHood())); + } + @Override public Command testShoot() { return this.run(