From df2b3850194411626856854a74288cec7818e222 Mon Sep 17 00:00:00 2001 From: dolev Date: Wed, 1 Jul 2026 17:14:31 +0300 Subject: [PATCH 1/7] 67 --- .../robot/statemachine/RobotCommander.java | 49 ++++++ .../robot/statemachine/ShootingChecks.java | 146 ++++++++++++++++++ .../constants/flywheel/FlywheelConstants.java | 3 + .../constants/hood/HoodConstants.java | 3 + .../constants/turret/TurretConstants.java | 4 + 5 files changed, 205 insertions(+) diff --git a/src/main/java/frc/robot/statemachine/RobotCommander.java b/src/main/java/frc/robot/statemachine/RobotCommander.java index bc6418c300..43afcafe7e 100644 --- a/src/main/java/frc/robot/statemachine/RobotCommander.java +++ b/src/main/java/frc/robot/statemachine/RobotCommander.java @@ -198,6 +198,15 @@ public boolean isReadyToScore() { ); } + public boolean isReadyToDumbScore() { + return ShootingChecks.isReadyToDumbScore( + robot, + StateMachineConstants.FLYWHEEL_VELOCITY_TOLERANCE_RPS_TO_START_SCORING, + StateMachineConstants.HOOD_POSITION_TOLERANCE_TO_START_SCORING, + StateMachineConstants.TURRET_TOLERANCE_TO_START_SCORING + ); + } + public boolean isReadyToPass() { return ShootingChecks.isReadyToPass( robot, @@ -218,6 +227,15 @@ public boolean canContinueScoring() { ); } + public boolean canContinueDumbScoring() { + return ShootingChecks.canContinueDumbScoring( + robot, + StateMachineConstants.FLYWHEEL_VELOCITY_TOLERANCE_RPS_TO_CONTINUE_SCORING, + StateMachineConstants.HOOD_POSITION_TOLERANCE_TO_CONTINUE_SCORING, + StateMachineConstants.TURRET_TOLERANCE_TO_CONTINUE_SCORING + ); + } + public boolean canContinuePassing() { return ShootingChecks.canContinuePassing( robot, @@ -271,6 +289,21 @@ public Command scoreSequence() { ); } + public Command dumbScoreSequence() { + return new ParallelCommandGroup( + shooterStateHandler.setState(ShooterState.SHOOT), + new SequentialCommandGroup( + asSubsystemCommand(funnelStateHandler.setState(FunnelState.STOP), RobotState.PRE_SCORE).until(this::isReadyToDumbScore), + new RepeatCommand( + new SequentialCommandGroup( + asSubsystemCommand(funnelStateHandler.setState(FunnelState.PRE_SHOOT).until(this::isReadyToDumbScore), RobotState.PRE_SCORE), + asSubsystemCommand(funnelStateHandler.setState(FunnelState.SHOOT).until(() -> !canContinueDumbScoring()), RobotState.SCORE) + ) + ) + ) + ); + } + public Command passSequence() { return new ParallelCommandGroup( shooterStateHandler.setState(ShooterState.SHOOT), @@ -287,6 +320,22 @@ public Command passSequence() { ); } + public Command dumbPassSequence() { + return new ParallelCommandGroup( + shooterStateHandler.setState(ShooterState.SHOOT), + new RepeatCommand( + new SequentialCommandGroup( + new ParallelCommandGroup( + asSubsystemCommand(funnelStateHandler.setState(FunnelState.PRE_SHOOT).until(this::isReadyToPass), RobotState.PRE_PASS) + ), + new ParallelCommandGroup( + asSubsystemCommand(funnelStateHandler.setState(FunnelState.SHOOT).until(() -> !canContinuePassing()), RobotState.PASS) + ) + ) + ) + ); + } + public Command calibrationScoreSequence() { return new ParallelCommandGroup( shooterStateHandler.setState(ShooterState.CALIBRATION), diff --git a/src/main/java/frc/robot/statemachine/ShootingChecks.java b/src/main/java/frc/robot/statemachine/ShootingChecks.java index f9cfc45720..ae2b38cc4e 100644 --- a/src/main/java/frc/robot/statemachine/ShootingChecks.java +++ b/src/main/java/frc/robot/statemachine/ShootingChecks.java @@ -6,6 +6,9 @@ import frc.constants.field.Field; import frc.robot.Robot; import frc.robot.statemachine.shooterstatehandler.ShooterConstants; +import frc.robot.subsystems.constants.flywheel.FlywheelConstants; +import frc.robot.subsystems.constants.hood.HoodConstants; +import frc.robot.subsystems.constants.turret.TurretConstants; import frc.utils.HubUtil; import frc.utils.time.TimeUtil; import org.littletonrobotics.junction.Logger; @@ -125,6 +128,45 @@ private static boolean isReadyToShoot( return isReadyToShoot; } + private static boolean isReadyToDumbShoot( + Robot robot, + Rotation2d flywheelVelocityToleranceRPS, + Rotation2d flywheelTargetVelocityRPS, + Rotation2d hoodPositionTolerance, + Rotation2d hoodTargetPosition, + Rotation2d turretTolerance, + Rotation2d targetTurretPosition, + String actionLogPath + ) { + String logPath = shootingChecksLogPath + "/IsReadyTo" + actionLogPath; + + boolean isAtTurretAtTarget = isTurretAtTargetPosition( + robot.getTurret().getPosition(), + targetTurretPosition, + turretTolerance, + logPath + ); + + boolean isFlywheelReadyToShoot = isFlywheelAtVelocity( + robot.getFlyWheel().getVelocity(), + flywheelTargetVelocityRPS, + flywheelVelocityToleranceRPS, + logPath + ); + + boolean isHoodAtPosition = isHoodAtPosition( + robot.getHood().getPosition(), + hoodTargetPosition, + hoodPositionTolerance, + logPath + ); + + boolean isReadyToDumbShoot = isAtTurretAtTarget && isFlywheelReadyToShoot && isHoodAtPosition; + Logger.recordOutput(shootingChecksLogPath + "/IsReadyToDumbShoot", isReadyToDumbShoot); + + return isReadyToDumbShoot; + } + private static boolean canContinueShooting( Robot robot, Rotation2d flywheelVelocityToleranceRPS, @@ -168,6 +210,48 @@ private static boolean canContinueShooting( return canContinueShooting; } + private static boolean canContinueDumbShooting( + Robot robot, + Rotation2d flywheelVelocityToleranceRPS, + Rotation2d flywheelTargetVelocityRPS, + Rotation2d hoodPositionTolerance, + Rotation2d hoodTargetPosition, + Rotation2d turretTolerance, + Rotation2d turretTarget, + String actionLogPath + ) { + String logPath = shootingChecksLogPath + "/CanContinue" + actionLogPath; + + boolean isAtTurretAtTarget = isTurretAtTargetPosition( + robot.getTurret().getPosition(), + turretTarget, + turretTolerance, + logPath + ); + + boolean isFlywheelReadyToShoot = isFlywheelAtVelocity( + robot.getFlyWheel().getVelocity(), + flywheelTargetVelocityRPS, + flywheelVelocityToleranceRPS, + logPath + ); + + boolean isHoodAtPosition = isHoodAtPosition( + robot.getHood().getPosition(), + hoodTargetPosition, + hoodPositionTolerance, + logPath + ); + + boolean canContinueDumbShooting = isAtTurretAtTarget + && isFlywheelReadyToShoot + && isHoodAtPosition; + + Logger.recordOutput(shootingChecksLogPath + "/CanContinueDumbShooting", canContinueDumbShooting); + + return canContinueDumbShooting; + } + public static boolean calibrationIsReadyToShoot( Robot robot, Rotation2d flywheelVelocityToleranceRPS, @@ -245,6 +329,27 @@ public static boolean isReadyToScore( return isReadyToScore; } + public static boolean isReadyToDumbScore( + Robot robot, + Rotation2d flywheelVelocityToleranceRPS, + Rotation2d hoodPositionTolerance, + Rotation2d turretTolerance + ) { + boolean isReadyToDumbShoot = isReadyToDumbShoot( + robot, + flywheelVelocityToleranceRPS, + FlywheelConstants.DUMB_MODE_FLYWHEEL_SCORE_VELOCITY, + hoodPositionTolerance, + HoodConstants.DUMB_MODE_HOOD_SCORE_POSITION, + turretTolerance, + TurretConstants.DUMB_MODE_TURRET_SCORE_POSITION, + "DumbScore" + ); + + Logger.recordOutput(shootingChecksLogPath + "isReadyToDumbScore", isReadyToDumbShoot); + + return isReadyToDumbShoot; + } public static boolean isReadyToPass( Robot robot, Rotation2d flywheelVelocityToleranceRPS, @@ -271,6 +376,28 @@ public static boolean isReadyToPass( return isReadyToPass; } + public static boolean isReadyToDumbPass( + Robot robot, + Rotation2d flywheelVelocityToleranceRPS, + Rotation2d hoodPositionTolerance, + Rotation2d turretTolerance + ) { + boolean isReadyToDumbShoot = isReadyToDumbShoot( + robot, + flywheelVelocityToleranceRPS, + FlywheelConstants.DUMB_MODE_FLYWHEEL_PASS_VELOCITY, + hoodPositionTolerance, + HoodConstants.DUMB_MODE_HOOD_PASS_POSITION, + turretTolerance, + TurretConstants.DUMB_MODE_TURRET_PASS_POSITION, + "DumbPass" + ); + + Logger.recordOutput(shootingChecksLogPath + "/IsReadyToDumbPass", isReadyToDumbShoot); + + return isReadyToDumbShoot; + } + public static boolean canContinueScoring( Robot robot, Rotation2d flywheelVelocityToleranceRPS, @@ -300,6 +427,25 @@ public static boolean canContinueScoring( return canContinueScoring; } + public static boolean canContinueDumbScoring( + Robot robot, + Rotation2d flywheelVelocityToleranceRPS, + Rotation2d hoodPositionTolerance, + Rotation2d turretTolerance + ) { + boolean canContinueDumbShooting = canContinueDumbShooting( + robot, + flywheelVelocityToleranceRPS, + hoodPositionTolerance, + turretTolerance, + "Scoring" + ); + + Logger.recordOutput(shootingChecksLogPath + "/CanContinueDumbScoring", canContinueDumbShooting); + + return canContinueDumbShooting; + } + public static boolean canContinuePassing( Robot robot, Rotation2d flywheelVelocityToleranceRPS, diff --git a/src/main/java/frc/robot/subsystems/constants/flywheel/FlywheelConstants.java b/src/main/java/frc/robot/subsystems/constants/flywheel/FlywheelConstants.java index ea28ac5be1..4f2c6f4805 100644 --- a/src/main/java/frc/robot/subsystems/constants/flywheel/FlywheelConstants.java +++ b/src/main/java/frc/robot/subsystems/constants/flywheel/FlywheelConstants.java @@ -15,6 +15,9 @@ public class FlywheelConstants { public static final InvertedValue IS_MASTER_INVERTED = InvertedValue.CounterClockwise_Positive; public static final InvertedValue IS_FOLLOWER_INVERTED = InvertedValue.Clockwise_Positive; + public static Rotation2d DUMB_MODE_FLYWHEEL_SCORE_VELOCITY = Rotation2d.fromRotations(60); + public static Rotation2d DUMB_MODE_FLYWHEEL_PASS_VELOCITY = Rotation2d.fromRotations(60); + public static final double kP = 0.3; public static final double kI = 0; public static final double kD = 0; diff --git a/src/main/java/frc/robot/subsystems/constants/hood/HoodConstants.java b/src/main/java/frc/robot/subsystems/constants/hood/HoodConstants.java index 1ff7cb16b6..6dac61725a 100644 --- a/src/main/java/frc/robot/subsystems/constants/hood/HoodConstants.java +++ b/src/main/java/frc/robot/subsystems/constants/hood/HoodConstants.java @@ -22,6 +22,9 @@ public class HoodConstants { public static final Slot0Configs REAL_SLOT = new Slot0Configs(); public static final Slot0Configs SIMULATION_SLOT = new Slot0Configs(); + public static final Rotation2d DUMB_MODE_HOOD_SCORE_POSITION = Rotation2d.fromDegrees(0); + public static final Rotation2d DUMB_MODE_HOOD_PASS_POSITION = Rotation2d.fromDegrees(0); + static { FEEDBACK_CONFIGS.RotorToSensorRatio = 1; FEEDBACK_CONFIGS.SensorToMechanismRatio = 30.0; diff --git a/src/main/java/frc/robot/subsystems/constants/turret/TurretConstants.java b/src/main/java/frc/robot/subsystems/constants/turret/TurretConstants.java index 3bf7892a40..16bc9fd1ae 100644 --- a/src/main/java/frc/robot/subsystems/constants/turret/TurretConstants.java +++ b/src/main/java/frc/robot/subsystems/constants/turret/TurretConstants.java @@ -29,6 +29,10 @@ public class TurretConstants { public static final Slot0Configs REAL_SLOTS_CONFIG = new Slot0Configs(); public static final Slot0Configs SIMULATION_SLOTS_CONFIG = new Slot0Configs(); + public static final Rotation2d DUMB_MODE_TURRET_SCORE_POSITION = Rotation2d.fromDegrees(0); + public static final Rotation2d DUMB_MODE_TURRET_PASS_POSITION = Rotation2d.fromDegrees(0); + + static { FEEDBACK_CONFIGS.SensorToMechanismRatio = 70; From f68e121d50f3d5946e91a670bb77afe743d3c4bc Mon Sep 17 00:00:00 2001 From: dolev Date: Wed, 1 Jul 2026 17:22:15 +0300 Subject: [PATCH 2/7] 67 --- .../java/frc/robot/statemachine/ShootingChecks.java | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/statemachine/ShootingChecks.java b/src/main/java/frc/robot/statemachine/ShootingChecks.java index ae2b38cc4e..441f2d2981 100644 --- a/src/main/java/frc/robot/statemachine/ShootingChecks.java +++ b/src/main/java/frc/robot/statemachine/ShootingChecks.java @@ -138,7 +138,7 @@ private static boolean isReadyToDumbShoot( Rotation2d targetTurretPosition, String actionLogPath ) { - String logPath = shootingChecksLogPath + "/IsReadyTo" + actionLogPath; + String logPath = shootingChecksLogPath + "/IsReadyToDumb" + actionLogPath; boolean isAtTurretAtTarget = isTurretAtTargetPosition( robot.getTurret().getPosition(), @@ -217,14 +217,14 @@ private static boolean canContinueDumbShooting( Rotation2d hoodPositionTolerance, Rotation2d hoodTargetPosition, Rotation2d turretTolerance, - Rotation2d turretTarget, + Rotation2d turretTargetPosition, String actionLogPath ) { String logPath = shootingChecksLogPath + "/CanContinue" + actionLogPath; boolean isAtTurretAtTarget = isTurretAtTargetPosition( robot.getTurret().getPosition(), - turretTarget, + turretTargetPosition, turretTolerance, logPath ); @@ -343,7 +343,7 @@ public static boolean isReadyToDumbScore( HoodConstants.DUMB_MODE_HOOD_SCORE_POSITION, turretTolerance, TurretConstants.DUMB_MODE_TURRET_SCORE_POSITION, - "DumbScore" + "Score" ); Logger.recordOutput(shootingChecksLogPath + "isReadyToDumbScore", isReadyToDumbShoot); @@ -390,7 +390,7 @@ public static boolean isReadyToDumbPass( HoodConstants.DUMB_MODE_HOOD_PASS_POSITION, turretTolerance, TurretConstants.DUMB_MODE_TURRET_PASS_POSITION, - "DumbPass" + "Pass" ); Logger.recordOutput(shootingChecksLogPath + "/IsReadyToDumbPass", isReadyToDumbShoot); @@ -436,8 +436,11 @@ public static boolean canContinueDumbScoring( boolean canContinueDumbShooting = canContinueDumbShooting( robot, flywheelVelocityToleranceRPS, + FlywheelConstants.DUMB_MODE_FLYWHEEL_SCORE_VELOCITY, hoodPositionTolerance, + HoodConstants.DUMB_MODE_HOOD_SCORE_POSITION, turretTolerance, + TurretConstants.DUMB_MODE_TURRET_SCORE_POSITION, "Scoring" ); From 3874193cd1eebb3e8801123818c926467f572786 Mon Sep 17 00:00:00 2001 From: dolev Date: Wed, 1 Jul 2026 17:34:34 +0300 Subject: [PATCH 3/7] 67 --- .../robot/statemachine/RobotCommander.java | 20 ++++++++++++++-- .../robot/statemachine/ShootingChecks.java | 24 ++++++++++++++++++- 2 files changed, 41 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/statemachine/RobotCommander.java b/src/main/java/frc/robot/statemachine/RobotCommander.java index 43afcafe7e..54790d75e6 100644 --- a/src/main/java/frc/robot/statemachine/RobotCommander.java +++ b/src/main/java/frc/robot/statemachine/RobotCommander.java @@ -217,6 +217,14 @@ public boolean isReadyToPass() { ); } + public boolean isReadyToDumbPass() { + return ShootingChecks.isReadyToDumbPass( + robot, + StateMachineConstants.FLYWHEEL_VELOCITY_TOLERANCE_RPS_TO_START_PASSING, + StateMachineConstants.HOOD_POSITION_TOLERANCE_TO_START_PASSING, + StateMachineConstants.TURRET_TOLERANCE_TO_START_PASSING + ); + } public boolean canContinueScoring() { return ShootingChecks.canContinueScoring( robot, @@ -245,6 +253,14 @@ public boolean canContinuePassing() { StateMachineConstants.MAX_DISTANCE_TO_PASS_METERS ); } + public boolean canContinueDumbPassing() { + return ShootingChecks.canContinueDumbPassing( + robot, + StateMachineConstants.FLYWHEEL_VELOCITY_TOLERANCE_RPS_TO_CONTINUE_PASSING, + StateMachineConstants.HOOD_POSITION_TOLERANCE_TO_CONTINUE_PASSING, + StateMachineConstants.TURRET_TOLERANCE_TO_CONTINUE_PASSING + ); + } private boolean calibrationIsReadyToScore() { return ShootingChecks.calibrationIsReadyToShoot( @@ -326,10 +342,10 @@ public Command dumbPassSequence() { new RepeatCommand( new SequentialCommandGroup( new ParallelCommandGroup( - asSubsystemCommand(funnelStateHandler.setState(FunnelState.PRE_SHOOT).until(this::isReadyToPass), RobotState.PRE_PASS) + asSubsystemCommand(funnelStateHandler.setState(FunnelState.PRE_SHOOT).until(this::isReadyToDumbPass), RobotState.PRE_PASS) ), new ParallelCommandGroup( - asSubsystemCommand(funnelStateHandler.setState(FunnelState.SHOOT).until(() -> !canContinuePassing()), RobotState.PASS) + asSubsystemCommand(funnelStateHandler.setState(FunnelState.SHOOT).until(() -> !canContinueDumbPassing()), RobotState.PASS) ) ) ) diff --git a/src/main/java/frc/robot/statemachine/ShootingChecks.java b/src/main/java/frc/robot/statemachine/ShootingChecks.java index 441f2d2981..0d26ac1acf 100644 --- a/src/main/java/frc/robot/statemachine/ShootingChecks.java +++ b/src/main/java/frc/robot/statemachine/ShootingChecks.java @@ -220,7 +220,7 @@ private static boolean canContinueDumbShooting( Rotation2d turretTargetPosition, String actionLogPath ) { - String logPath = shootingChecksLogPath + "/CanContinue" + actionLogPath; + String logPath = shootingChecksLogPath + "/CanContinueDumb" + actionLogPath; boolean isAtTurretAtTarget = isTurretAtTargetPosition( robot.getTurret().getPosition(), @@ -473,6 +473,28 @@ public static boolean canContinuePassing( return canContinuePassing; } + public static boolean canContinueDumbPassing( + Robot robot, + Rotation2d flywheelVelocityToleranceRPS, + Rotation2d hoodPositionTolerance, + Rotation2d headingTolerance + ) { + boolean canContinueDumbShooting = canContinueDumbShooting( + robot, + flywheelVelocityToleranceRPS, + FlywheelConstants.DUMB_MODE_FLYWHEEL_PASS_VELOCITY, + hoodPositionTolerance, + HoodConstants.DUMB_MODE_HOOD_PASS_POSITION, + headingTolerance, + TurretConstants.DUMB_MODE_TURRET_PASS_POSITION, + "Passing" + ); + + Logger.recordOutput(shootingChecksLogPath + "/CanContinueDumbPassing", canContinueDumbShooting); + + return canContinueDumbShooting; + } + public static boolean isOurHubReadyToStartShooting(double distanceFromHubMeters) { boolean isOurHubReadyToStartShooting = HubUtil .isOurHubActive(TimeUtil.getTimeSinceTeleopInitSeconds() + ShootingCalculations.getDistanceToBallFlightTime(distanceFromHubMeters)); From 9c1324f735d4081671d452151f5c4d2f7ef1089d Mon Sep 17 00:00:00 2001 From: dolev Date: Wed, 1 Jul 2026 17:43:29 +0300 Subject: [PATCH 4/7] code is complete --- src/main/java/frc/JoysticksBindings.java | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/main/java/frc/JoysticksBindings.java b/src/main/java/frc/JoysticksBindings.java index e06eedd909..4ab789f074 100644 --- a/src/main/java/frc/JoysticksBindings.java +++ b/src/main/java/frc/JoysticksBindings.java @@ -12,6 +12,7 @@ import frc.joysticks.SmartJoystick; import frc.robot.Robot; import frc.robot.autonomous.PathFollowingCommandsBuilder; +import frc.robot.statemachine.RobotCommander; import frc.robot.statemachine.RobotState; import frc.robot.statemachine.funnelstatehandler.FunnelState; import frc.robot.statemachine.intakestatehandler.IntakeState; @@ -137,6 +138,17 @@ private static void mainJoystickButtons(Robot robot) { private static void secondJoystickButtons(Robot robot) { SmartJoystick usedJoystick = SECOND_JOYSTICK; // bindings... + // Shoot & Pass... + usedJoystick.R1.onTrue( + robot.getRobotCommander() + .driveWithChangingState(RobotState.PRE_SCORE, robot.getRobotCommander().dumbScoreSequence(), () -> getScoringState(usedJoystick)) + ); + usedJoystick.getAxisAsButton(Axis.RIGHT_TRIGGER) + .onTrue( + robot.getRobotCommander() + .driveWithChangingState(RobotState.PRE_PASS, robot.getRobotCommander().dumbPassSequence(), () -> getPassState(usedJoystick)) + ); + usedJoystick.X.onTrue(robot.getRobotCommander().setState(RobotState.RESET_SUBSYSTEMS)); } private static void thirdJoystickButtons(Robot robot) { From 3a5831d1214e0c69a62b9d76471a0ce9e34ccaef Mon Sep 17 00:00:00 2001 From: dolev Date: Wed, 1 Jul 2026 17:46:27 +0300 Subject: [PATCH 5/7] code is complete --- src/main/java/frc/JoysticksBindings.java | 13 +- .../robot/statemachine/RobotCommander.java | 80 ++++---- .../robot/statemachine/ShootingChecks.java | 177 ++++++++---------- .../constants/turret/TurretConstants.java | 4 +- 4 files changed, 133 insertions(+), 141 deletions(-) diff --git a/src/main/java/frc/JoysticksBindings.java b/src/main/java/frc/JoysticksBindings.java index 4ab789f074..7cc99db261 100644 --- a/src/main/java/frc/JoysticksBindings.java +++ b/src/main/java/frc/JoysticksBindings.java @@ -12,7 +12,6 @@ import frc.joysticks.SmartJoystick; import frc.robot.Robot; import frc.robot.autonomous.PathFollowingCommandsBuilder; -import frc.robot.statemachine.RobotCommander; import frc.robot.statemachine.RobotState; import frc.robot.statemachine.funnelstatehandler.FunnelState; import frc.robot.statemachine.intakestatehandler.IntakeState; @@ -140,14 +139,14 @@ private static void secondJoystickButtons(Robot robot) { // bindings... // Shoot & Pass... usedJoystick.R1.onTrue( - robot.getRobotCommander() - .driveWithChangingState(RobotState.PRE_SCORE, robot.getRobotCommander().dumbScoreSequence(), () -> getScoringState(usedJoystick)) + robot.getRobotCommander() + .driveWithChangingState(RobotState.PRE_SCORE, robot.getRobotCommander().dumbScoreSequence(), () -> getScoringState(usedJoystick)) ); usedJoystick.getAxisAsButton(Axis.RIGHT_TRIGGER) - .onTrue( - robot.getRobotCommander() - .driveWithChangingState(RobotState.PRE_PASS, robot.getRobotCommander().dumbPassSequence(), () -> getPassState(usedJoystick)) - ); + .onTrue( + robot.getRobotCommander() + .driveWithChangingState(RobotState.PRE_PASS, robot.getRobotCommander().dumbPassSequence(), () -> getPassState(usedJoystick)) + ); usedJoystick.X.onTrue(robot.getRobotCommander().setState(RobotState.RESET_SUBSYSTEMS)); } diff --git a/src/main/java/frc/robot/statemachine/RobotCommander.java b/src/main/java/frc/robot/statemachine/RobotCommander.java index 54790d75e6..6428730e73 100644 --- a/src/main/java/frc/robot/statemachine/RobotCommander.java +++ b/src/main/java/frc/robot/statemachine/RobotCommander.java @@ -200,10 +200,10 @@ public boolean isReadyToScore() { public boolean isReadyToDumbScore() { return ShootingChecks.isReadyToDumbScore( - robot, - StateMachineConstants.FLYWHEEL_VELOCITY_TOLERANCE_RPS_TO_START_SCORING, - StateMachineConstants.HOOD_POSITION_TOLERANCE_TO_START_SCORING, - StateMachineConstants.TURRET_TOLERANCE_TO_START_SCORING + robot, + StateMachineConstants.FLYWHEEL_VELOCITY_TOLERANCE_RPS_TO_START_SCORING, + StateMachineConstants.HOOD_POSITION_TOLERANCE_TO_START_SCORING, + StateMachineConstants.TURRET_TOLERANCE_TO_START_SCORING ); } @@ -219,12 +219,13 @@ public boolean isReadyToPass() { public boolean isReadyToDumbPass() { return ShootingChecks.isReadyToDumbPass( - robot, - StateMachineConstants.FLYWHEEL_VELOCITY_TOLERANCE_RPS_TO_START_PASSING, - StateMachineConstants.HOOD_POSITION_TOLERANCE_TO_START_PASSING, - StateMachineConstants.TURRET_TOLERANCE_TO_START_PASSING + robot, + StateMachineConstants.FLYWHEEL_VELOCITY_TOLERANCE_RPS_TO_START_PASSING, + StateMachineConstants.HOOD_POSITION_TOLERANCE_TO_START_PASSING, + StateMachineConstants.TURRET_TOLERANCE_TO_START_PASSING ); } + public boolean canContinueScoring() { return ShootingChecks.canContinueScoring( robot, @@ -237,10 +238,10 @@ public boolean canContinueScoring() { public boolean canContinueDumbScoring() { return ShootingChecks.canContinueDumbScoring( - robot, - StateMachineConstants.FLYWHEEL_VELOCITY_TOLERANCE_RPS_TO_CONTINUE_SCORING, - StateMachineConstants.HOOD_POSITION_TOLERANCE_TO_CONTINUE_SCORING, - StateMachineConstants.TURRET_TOLERANCE_TO_CONTINUE_SCORING + robot, + StateMachineConstants.FLYWHEEL_VELOCITY_TOLERANCE_RPS_TO_CONTINUE_SCORING, + StateMachineConstants.HOOD_POSITION_TOLERANCE_TO_CONTINUE_SCORING, + StateMachineConstants.TURRET_TOLERANCE_TO_CONTINUE_SCORING ); } @@ -253,12 +254,13 @@ public boolean canContinuePassing() { StateMachineConstants.MAX_DISTANCE_TO_PASS_METERS ); } + public boolean canContinueDumbPassing() { return ShootingChecks.canContinueDumbPassing( - robot, - StateMachineConstants.FLYWHEEL_VELOCITY_TOLERANCE_RPS_TO_CONTINUE_PASSING, - StateMachineConstants.HOOD_POSITION_TOLERANCE_TO_CONTINUE_PASSING, - StateMachineConstants.TURRET_TOLERANCE_TO_CONTINUE_PASSING + robot, + StateMachineConstants.FLYWHEEL_VELOCITY_TOLERANCE_RPS_TO_CONTINUE_PASSING, + StateMachineConstants.HOOD_POSITION_TOLERANCE_TO_CONTINUE_PASSING, + StateMachineConstants.TURRET_TOLERANCE_TO_CONTINUE_PASSING ); } @@ -307,16 +309,22 @@ public Command scoreSequence() { public Command dumbScoreSequence() { return new ParallelCommandGroup( - shooterStateHandler.setState(ShooterState.SHOOT), - new SequentialCommandGroup( - asSubsystemCommand(funnelStateHandler.setState(FunnelState.STOP), RobotState.PRE_SCORE).until(this::isReadyToDumbScore), - new RepeatCommand( - new SequentialCommandGroup( - asSubsystemCommand(funnelStateHandler.setState(FunnelState.PRE_SHOOT).until(this::isReadyToDumbScore), RobotState.PRE_SCORE), - asSubsystemCommand(funnelStateHandler.setState(FunnelState.SHOOT).until(() -> !canContinueDumbScoring()), RobotState.SCORE) - ) + shooterStateHandler.setState(ShooterState.SHOOT), + new SequentialCommandGroup( + asSubsystemCommand(funnelStateHandler.setState(FunnelState.STOP), RobotState.PRE_SCORE).until(this::isReadyToDumbScore), + new RepeatCommand( + new SequentialCommandGroup( + asSubsystemCommand( + funnelStateHandler.setState(FunnelState.PRE_SHOOT).until(this::isReadyToDumbScore), + RobotState.PRE_SCORE + ), + asSubsystemCommand( + funnelStateHandler.setState(FunnelState.SHOOT).until(() -> !canContinueDumbScoring()), + RobotState.SCORE ) + ) ) + ) ); } @@ -338,17 +346,23 @@ public Command passSequence() { public Command dumbPassSequence() { return new ParallelCommandGroup( - shooterStateHandler.setState(ShooterState.SHOOT), - new RepeatCommand( - new SequentialCommandGroup( - new ParallelCommandGroup( - asSubsystemCommand(funnelStateHandler.setState(FunnelState.PRE_SHOOT).until(this::isReadyToDumbPass), RobotState.PRE_PASS) - ), - new ParallelCommandGroup( - asSubsystemCommand(funnelStateHandler.setState(FunnelState.SHOOT).until(() -> !canContinueDumbPassing()), RobotState.PASS) - ) + shooterStateHandler.setState(ShooterState.SHOOT), + new RepeatCommand( + new SequentialCommandGroup( + new ParallelCommandGroup( + asSubsystemCommand( + funnelStateHandler.setState(FunnelState.PRE_SHOOT).until(this::isReadyToDumbPass), + RobotState.PRE_PASS ) + ), + new ParallelCommandGroup( + asSubsystemCommand( + funnelStateHandler.setState(FunnelState.SHOOT).until(() -> !canContinueDumbPassing()), + RobotState.PASS + ) + ) ) + ) ); } diff --git a/src/main/java/frc/robot/statemachine/ShootingChecks.java b/src/main/java/frc/robot/statemachine/ShootingChecks.java index 0d26ac1acf..97113494b5 100644 --- a/src/main/java/frc/robot/statemachine/ShootingChecks.java +++ b/src/main/java/frc/robot/statemachine/ShootingChecks.java @@ -129,37 +129,27 @@ private static boolean isReadyToShoot( } private static boolean isReadyToDumbShoot( - Robot robot, - Rotation2d flywheelVelocityToleranceRPS, - Rotation2d flywheelTargetVelocityRPS, - Rotation2d hoodPositionTolerance, - Rotation2d hoodTargetPosition, - Rotation2d turretTolerance, - Rotation2d targetTurretPosition, - String actionLogPath + Robot robot, + Rotation2d flywheelVelocityToleranceRPS, + Rotation2d flywheelTargetVelocityRPS, + Rotation2d hoodPositionTolerance, + Rotation2d hoodTargetPosition, + Rotation2d turretTolerance, + Rotation2d targetTurretPosition, + String actionLogPath ) { String logPath = shootingChecksLogPath + "/IsReadyToDumb" + actionLogPath; - boolean isAtTurretAtTarget = isTurretAtTargetPosition( - robot.getTurret().getPosition(), - targetTurretPosition, - turretTolerance, - logPath - ); + boolean isAtTurretAtTarget = isTurretAtTargetPosition(robot.getTurret().getPosition(), targetTurretPosition, turretTolerance, logPath); boolean isFlywheelReadyToShoot = isFlywheelAtVelocity( - robot.getFlyWheel().getVelocity(), - flywheelTargetVelocityRPS, - flywheelVelocityToleranceRPS, - logPath + robot.getFlyWheel().getVelocity(), + flywheelTargetVelocityRPS, + flywheelVelocityToleranceRPS, + logPath ); - boolean isHoodAtPosition = isHoodAtPosition( - robot.getHood().getPosition(), - hoodTargetPosition, - hoodPositionTolerance, - logPath - ); + boolean isHoodAtPosition = isHoodAtPosition(robot.getHood().getPosition(), hoodTargetPosition, hoodPositionTolerance, logPath); boolean isReadyToDumbShoot = isAtTurretAtTarget && isFlywheelReadyToShoot && isHoodAtPosition; Logger.recordOutput(shootingChecksLogPath + "/IsReadyToDumbShoot", isReadyToDumbShoot); @@ -211,41 +201,29 @@ private static boolean canContinueShooting( } private static boolean canContinueDumbShooting( - Robot robot, - Rotation2d flywheelVelocityToleranceRPS, - Rotation2d flywheelTargetVelocityRPS, - Rotation2d hoodPositionTolerance, - Rotation2d hoodTargetPosition, - Rotation2d turretTolerance, - Rotation2d turretTargetPosition, - String actionLogPath + Robot robot, + Rotation2d flywheelVelocityToleranceRPS, + Rotation2d flywheelTargetVelocityRPS, + Rotation2d hoodPositionTolerance, + Rotation2d hoodTargetPosition, + Rotation2d turretTolerance, + Rotation2d turretTargetPosition, + String actionLogPath ) { String logPath = shootingChecksLogPath + "/CanContinueDumb" + actionLogPath; - boolean isAtTurretAtTarget = isTurretAtTargetPosition( - robot.getTurret().getPosition(), - turretTargetPosition, - turretTolerance, - logPath - ); + boolean isAtTurretAtTarget = isTurretAtTargetPosition(robot.getTurret().getPosition(), turretTargetPosition, turretTolerance, logPath); boolean isFlywheelReadyToShoot = isFlywheelAtVelocity( - robot.getFlyWheel().getVelocity(), - flywheelTargetVelocityRPS, - flywheelVelocityToleranceRPS, - logPath + robot.getFlyWheel().getVelocity(), + flywheelTargetVelocityRPS, + flywheelVelocityToleranceRPS, + logPath ); - boolean isHoodAtPosition = isHoodAtPosition( - robot.getHood().getPosition(), - hoodTargetPosition, - hoodPositionTolerance, - logPath - ); + boolean isHoodAtPosition = isHoodAtPosition(robot.getHood().getPosition(), hoodTargetPosition, hoodPositionTolerance, logPath); - boolean canContinueDumbShooting = isAtTurretAtTarget - && isFlywheelReadyToShoot - && isHoodAtPosition; + boolean canContinueDumbShooting = isAtTurretAtTarget && isFlywheelReadyToShoot && isHoodAtPosition; Logger.recordOutput(shootingChecksLogPath + "/CanContinueDumbShooting", canContinueDumbShooting); @@ -330,26 +308,27 @@ public static boolean isReadyToScore( } public static boolean isReadyToDumbScore( - Robot robot, - Rotation2d flywheelVelocityToleranceRPS, - Rotation2d hoodPositionTolerance, - Rotation2d turretTolerance + Robot robot, + Rotation2d flywheelVelocityToleranceRPS, + Rotation2d hoodPositionTolerance, + Rotation2d turretTolerance ) { boolean isReadyToDumbShoot = isReadyToDumbShoot( - robot, - flywheelVelocityToleranceRPS, - FlywheelConstants.DUMB_MODE_FLYWHEEL_SCORE_VELOCITY, - hoodPositionTolerance, - HoodConstants.DUMB_MODE_HOOD_SCORE_POSITION, - turretTolerance, - TurretConstants.DUMB_MODE_TURRET_SCORE_POSITION, - "Score" + robot, + flywheelVelocityToleranceRPS, + FlywheelConstants.DUMB_MODE_FLYWHEEL_SCORE_VELOCITY, + hoodPositionTolerance, + HoodConstants.DUMB_MODE_HOOD_SCORE_POSITION, + turretTolerance, + TurretConstants.DUMB_MODE_TURRET_SCORE_POSITION, + "Score" ); Logger.recordOutput(shootingChecksLogPath + "isReadyToDumbScore", isReadyToDumbShoot); return isReadyToDumbShoot; } + public static boolean isReadyToPass( Robot robot, Rotation2d flywheelVelocityToleranceRPS, @@ -377,20 +356,20 @@ public static boolean isReadyToPass( } public static boolean isReadyToDumbPass( - Robot robot, - Rotation2d flywheelVelocityToleranceRPS, - Rotation2d hoodPositionTolerance, - Rotation2d turretTolerance + Robot robot, + Rotation2d flywheelVelocityToleranceRPS, + Rotation2d hoodPositionTolerance, + Rotation2d turretTolerance ) { boolean isReadyToDumbShoot = isReadyToDumbShoot( - robot, - flywheelVelocityToleranceRPS, - FlywheelConstants.DUMB_MODE_FLYWHEEL_PASS_VELOCITY, - hoodPositionTolerance, - HoodConstants.DUMB_MODE_HOOD_PASS_POSITION, - turretTolerance, - TurretConstants.DUMB_MODE_TURRET_PASS_POSITION, - "Pass" + robot, + flywheelVelocityToleranceRPS, + FlywheelConstants.DUMB_MODE_FLYWHEEL_PASS_VELOCITY, + hoodPositionTolerance, + HoodConstants.DUMB_MODE_HOOD_PASS_POSITION, + turretTolerance, + TurretConstants.DUMB_MODE_TURRET_PASS_POSITION, + "Pass" ); Logger.recordOutput(shootingChecksLogPath + "/IsReadyToDumbPass", isReadyToDumbShoot); @@ -428,20 +407,20 @@ public static boolean canContinueScoring( } public static boolean canContinueDumbScoring( - Robot robot, - Rotation2d flywheelVelocityToleranceRPS, - Rotation2d hoodPositionTolerance, - Rotation2d turretTolerance + Robot robot, + Rotation2d flywheelVelocityToleranceRPS, + Rotation2d hoodPositionTolerance, + Rotation2d turretTolerance ) { boolean canContinueDumbShooting = canContinueDumbShooting( - robot, - flywheelVelocityToleranceRPS, - FlywheelConstants.DUMB_MODE_FLYWHEEL_SCORE_VELOCITY, - hoodPositionTolerance, - HoodConstants.DUMB_MODE_HOOD_SCORE_POSITION, - turretTolerance, - TurretConstants.DUMB_MODE_TURRET_SCORE_POSITION, - "Scoring" + robot, + flywheelVelocityToleranceRPS, + FlywheelConstants.DUMB_MODE_FLYWHEEL_SCORE_VELOCITY, + hoodPositionTolerance, + HoodConstants.DUMB_MODE_HOOD_SCORE_POSITION, + turretTolerance, + TurretConstants.DUMB_MODE_TURRET_SCORE_POSITION, + "Scoring" ); Logger.recordOutput(shootingChecksLogPath + "/CanContinueDumbScoring", canContinueDumbShooting); @@ -474,20 +453,20 @@ public static boolean canContinuePassing( } public static boolean canContinueDumbPassing( - Robot robot, - Rotation2d flywheelVelocityToleranceRPS, - Rotation2d hoodPositionTolerance, - Rotation2d headingTolerance + Robot robot, + Rotation2d flywheelVelocityToleranceRPS, + Rotation2d hoodPositionTolerance, + Rotation2d headingTolerance ) { boolean canContinueDumbShooting = canContinueDumbShooting( - robot, - flywheelVelocityToleranceRPS, - FlywheelConstants.DUMB_MODE_FLYWHEEL_PASS_VELOCITY, - hoodPositionTolerance, - HoodConstants.DUMB_MODE_HOOD_PASS_POSITION, - headingTolerance, - TurretConstants.DUMB_MODE_TURRET_PASS_POSITION, - "Passing" + robot, + flywheelVelocityToleranceRPS, + FlywheelConstants.DUMB_MODE_FLYWHEEL_PASS_VELOCITY, + hoodPositionTolerance, + HoodConstants.DUMB_MODE_HOOD_PASS_POSITION, + headingTolerance, + TurretConstants.DUMB_MODE_TURRET_PASS_POSITION, + "Passing" ); Logger.recordOutput(shootingChecksLogPath + "/CanContinueDumbPassing", canContinueDumbShooting); diff --git a/src/main/java/frc/robot/subsystems/constants/turret/TurretConstants.java b/src/main/java/frc/robot/subsystems/constants/turret/TurretConstants.java index 16bc9fd1ae..64cdf2379f 100644 --- a/src/main/java/frc/robot/subsystems/constants/turret/TurretConstants.java +++ b/src/main/java/frc/robot/subsystems/constants/turret/TurretConstants.java @@ -29,8 +29,8 @@ public class TurretConstants { public static final Slot0Configs REAL_SLOTS_CONFIG = new Slot0Configs(); public static final Slot0Configs SIMULATION_SLOTS_CONFIG = new Slot0Configs(); - public static final Rotation2d DUMB_MODE_TURRET_SCORE_POSITION = Rotation2d.fromDegrees(0); - public static final Rotation2d DUMB_MODE_TURRET_PASS_POSITION = Rotation2d.fromDegrees(0); + public static final Rotation2d DUMB_MODE_TURRET_SCORE_POSITION = Rotation2d.fromDegrees(0); + public static final Rotation2d DUMB_MODE_TURRET_PASS_POSITION = Rotation2d.fromDegrees(0); static { From 8b0de30a69268352ed870ab51adc610ee4659c88 Mon Sep 17 00:00:00 2001 From: dolev Date: Wed, 1 Jul 2026 19:10:15 +0300 Subject: [PATCH 6/7] antigravity fix --- .../robot/statemachine/RobotCommander.java | 17 ++++++++++++-- .../ShooterStateHandler.java | 22 +++++++++++++++++++ 2 files changed, 37 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/statemachine/RobotCommander.java b/src/main/java/frc/robot/statemachine/RobotCommander.java index 6428730e73..58eec021e4 100644 --- a/src/main/java/frc/robot/statemachine/RobotCommander.java +++ b/src/main/java/frc/robot/statemachine/RobotCommander.java @@ -12,6 +12,11 @@ import frc.robot.subsystems.swerve.Swerve; import frc.robot.subsystems.swerve.states.SwerveState; import frc.robot.subsystems.swerve.states.aimassist.AimAssist; + + +import frc.robot.subsystems.constants.flywheel.FlywheelConstants; +import frc.robot.subsystems.constants.hood.HoodConstants; +import frc.robot.subsystems.constants.turret.TurretConstants; import org.littletonrobotics.junction.Logger; import java.util.Set; @@ -309,7 +314,11 @@ public Command scoreSequence() { public Command dumbScoreSequence() { return new ParallelCommandGroup( - shooterStateHandler.setState(ShooterState.SHOOT), + shooterStateHandler.setDumbState( + TurretConstants.DUMB_MODE_TURRET_SCORE_POSITION, + HoodConstants.DUMB_MODE_HOOD_SCORE_POSITION, + FlywheelConstants.DUMB_MODE_FLYWHEEL_SCORE_VELOCITY + ), new SequentialCommandGroup( asSubsystemCommand(funnelStateHandler.setState(FunnelState.STOP), RobotState.PRE_SCORE).until(this::isReadyToDumbScore), new RepeatCommand( @@ -346,7 +355,11 @@ public Command passSequence() { public Command dumbPassSequence() { return new ParallelCommandGroup( - shooterStateHandler.setState(ShooterState.SHOOT), + shooterStateHandler.setDumbState( + TurretConstants.DUMB_MODE_TURRET_PASS_POSITION, + HoodConstants.DUMB_MODE_HOOD_PASS_POSITION, + FlywheelConstants.DUMB_MODE_FLYWHEEL_PASS_VELOCITY + ), new RepeatCommand( new SequentialCommandGroup( new ParallelCommandGroup( diff --git a/src/main/java/frc/robot/statemachine/shooterstatehandler/ShooterStateHandler.java b/src/main/java/frc/robot/statemachine/shooterstatehandler/ShooterStateHandler.java index 14bc64cd4a..588d2ec5af 100644 --- a/src/main/java/frc/robot/statemachine/shooterstatehandler/ShooterStateHandler.java +++ b/src/main/java/frc/robot/statemachine/shooterstatehandler/ShooterStateHandler.java @@ -1,5 +1,6 @@ package frc.robot.statemachine.shooterstatehandler; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.*; import frc.robot.Robot; import frc.robot.statemachine.ShootingCalculations; @@ -107,6 +108,27 @@ private Command resetSubsystems() { ); } + public Command setDumbState(Rotation2d turretTargetPosition, Rotation2d hoodTargetPosition, Rotation2d flywheelTargetVelocityRPS) { + Command command = new ParallelCommandGroup( + turret.asSubsystemCommand( + new TurretSafeMoveToPosition( + turret, + () -> turretTargetPosition, + () -> Rotation2d.kZero, + logPath + ), + "Safe move to position" + ), + hood.getCommandsBuilder().setTargetPosition(() -> hoodTargetPosition), + flyWheel.getCommandBuilder().setVelocityAsSupplier(() -> flywheelTargetVelocityRPS) + ); + return new ParallelCommandGroup( + new InstantCommand(() -> Logger.recordOutput(logPath + "/CurrentState", ShooterState.SHOOT.name())), + new InstantCommand(() -> currentState = ShooterState.SHOOT), + command + ); + } + private Command calibration() { return new ParallelCommandGroup( turret.getCommandsBuilder().setTargetPosition(() -> ShootingCalculations.getShootingParams().targetTurretPosition()), From 7ed215f50e9d115a729768068dcf9f7994d186f7 Mon Sep 17 00:00:00 2001 From: dolev Date: Wed, 1 Jul 2026 19:22:04 +0300 Subject: [PATCH 7/7] antigravity fix --- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/subsystems/arm/Arm.java | 4 ++++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 40350b2b88..462781cadb 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -309,7 +309,7 @@ private void updateAllSubsystems() { } public boolean isTurretMoveLegal() { - return TurretCalculations.isTurretMoveLegal(ShootingCalculations.getShootingParams().targetTurretPosition(), turret.getPosition()); + return TurretCalculations.isTurretMoveLegal(turret.getPositionRequest().getSetPoint(), turret.getPosition()); } public void periodic() { diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index b431a8c944..0a47ca9f0d 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -40,6 +40,10 @@ public Arm( commandBuilder = new ArmCommandBuilder(this); setDefaultCommand(commandBuilder.stayInPlace()); } + + public IFeedForwardRequest getPositionRequest() { + return positionRequest; + } public ArmCommandBuilder getCommandsBuilder() { return commandBuilder;