Dumb mode -> dis 1

src/main/java/frc/JoysticksBindings.java

@@ -137,6 +137,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) {

src/main/java/frc/RobotManager.java

@@ -73,7 +73,7 @@ public RobotManager() {
 			field2d.getObject("path").setPoses(robot.getAutonomousChooser().getChosenValue().getPath(!Field.isFieldConventionAlliance()));
 		});
 
-		robot.getAllLimelights().forEach(limelight -> limelight.setThrottleState(!DriverStationUtil.isMatch()));
+		robot.getLimelights().forEach(limelight -> limelight.setThrottleState(!DriverStationUtil.isMatch()));
 
 		alertsMessage = "Alerts: None";
 		Logger.recordOutput("AlertsMessage", alertsMessage);
@@ -85,7 +85,7 @@ public void disabledExit() {
 		if (!DriverStationUtil.isMatch()) {
 			BrakeStateManager.setBrakeMode(BrakeMode.BRAKE);
 		}
-		robot.getAllLimelights().forEach(limelight -> limelight.setThrottleState(false));
+		robot.getLimelights().forEach(limelight -> limelight.setThrottleState(false));
 	}
 
 	@Override
@@ -112,7 +112,7 @@ public void autonomousExit() {
 			autonomousCommand.cancel();
 		}
 		if (!DriverStationUtil.isMatch()) {
-			robot.getAllLimelights().forEach(limelight -> limelight.setThrottleState(true));
+			robot.getLimelights().forEach(limelight -> limelight.setThrottleState(true));
 		}
 		robot.getSwerve().setIsRunningIndependently(false);
 		Logger.recordOutput("averagePeriodPBS/Autonomous", robot.getAverageBPSForLastXSeconds(GamePeriodUtils.AUTONOMOUS_DURATION_SECONDS));
@@ -125,8 +125,8 @@ public void teleopInit() {
 
 	@Override
 	public void teleopExit() {
-		robot.getAllLimelights().forEach(limelight -> limelight.captureGivenTime(GamePeriodUtils.COMPLETE_GAME_TIME_SECONDS));
-		robot.getAllLimelights().forEach(limelight -> limelight.setThrottleState(true));
+		robot.getLimelights().forEach(limelight -> limelight.captureGivenTime(GamePeriodUtils.COMPLETE_GAME_TIME_SECONDS));
+		robot.getLimelights().forEach(limelight -> limelight.setThrottleState(true));
 	}
 
 	public static double getTeleopStartTimeSeconds() {

src/main/java/frc/robot/Robot.java

@@ -102,6 +102,7 @@ public class Robot {
 	private final Limelight limelightFront;
 	private final Limelight limelightRight;
 	private final Limelight limelightLeft;
+	private final List<Limelight> limelights;
 
 	private static double ballCounterIncludingPassing;
 	private static double ballCounterWithoutPassing;
@@ -177,26 +178,6 @@ public Robot() {
 			),
 			LimelightPipeline.APRIL_TAG
 		);
-
-		limelightFront.setMT1StdDevsCalculation(
-			LimelightStdDevCalculations.getMT1StdDevsCalculation(
-				limelightFront,
-				new StandardDeviations2D(0.5),
-				new StandardDeviations2D(0.15),
-				new StandardDeviations2D(0.4),
-				new StandardDeviations2D(0.011)
-			)
-		);
-		limelightFront.setMT1PoseFilter(
-			LimelightFilters.megaTag1Filter(
-				limelightFront,
-				timestamp -> poseEstimator.getEstimatedPoseAtTimestamp(timestamp).map(Pose2d::getRotation),
-				poseEstimator::isIMUOffsetCalibrated,
-				new Translation2d(0.1, 0.1),
-				Rotation2d.fromDegrees(10)
-			)
-		);
-
 		this.limelightRight = new Limelight(
 			"limelight-right",
 			"Vision",
@@ -206,25 +187,6 @@ public Robot() {
 			),
 			LimelightPipeline.APRIL_TAG
 		);
-		limelightRight.setMT1StdDevsCalculation(
-			LimelightStdDevCalculations.getMT1StdDevsCalculation(
-				limelightRight,
-				new StandardDeviations2D(0.5),
-				new StandardDeviations2D(0.15),
-				new StandardDeviations2D(0.4),
-				new StandardDeviations2D(0.011)
-			)
-		);
-		limelightRight.setMT1PoseFilter(
-			LimelightFilters.megaTag1Filter(
-				limelightRight,
-				timestamp -> poseEstimator.getEstimatedPoseAtTimestamp(timestamp).map(Pose2d::getRotation),
-				poseEstimator::isIMUOffsetCalibrated,
-				new Translation2d(0.1, 0.1),
-				Rotation2d.fromDegrees(10)
-			)
-		);
-
 		this.limelightLeft = new Limelight(
 			"limelight-left",
 			"Vision",
@@ -234,22 +196,28 @@ public Robot() {
 			),
 			LimelightPipeline.APRIL_TAG
 		);
-		limelightLeft.setMT1StdDevsCalculation(
-			LimelightStdDevCalculations.getMT1StdDevsCalculation(
-				limelightLeft,
-				new StandardDeviations2D(0.5),
-				new StandardDeviations2D(0.15),
-				new StandardDeviations2D(0.4),
-				new StandardDeviations2D(0.011)
+
+		this.limelights = List.of(limelightFront, limelightRight, limelightLeft);
+		limelights.forEach(
+			limelight -> limelight.setMT1StdDevsCalculation(
+				LimelightStdDevCalculations.getMT1StdDevsCalculation(
+					limelight,
+					new StandardDeviations2D(0.5),
+					new StandardDeviations2D(0.15),
+					new StandardDeviations2D(0.4),
+					new StandardDeviations2D(0.011)
+				)
 			)
 		);
-		limelightLeft.setMT1PoseFilter(
-			LimelightFilters.megaTag1Filter(
-				limelightLeft,
-				timestamp -> poseEstimator.getEstimatedPoseAtTimestamp(timestamp).map(Pose2d::getRotation),
-				poseEstimator::isIMUOffsetCalibrated,
-				new Translation2d(0.1, 0.1),
-				Rotation2d.fromDegrees(10)
+		limelights.forEach(
+			limelight -> limelight.setMT1PoseFilter(
+				LimelightFilters.megaTag1Filter(
+					limelight,
+					timestamp -> poseEstimator.getEstimatedPoseAtTimestamp(timestamp).map(Pose2d::getRotation),
+					poseEstimator::isIMUOffsetCalibrated,
+					new Translation2d(0.1, 0.1),
+					Rotation2d.fromDegrees(10)
+				)
 			)
 		);
 
@@ -341,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() {
@@ -351,17 +319,9 @@ public void periodic() {
 
 		poseEstimator.updateOdometry(swerve.getAllOdometryData());
 
-		limelightFront.updateHardwareInputs();
-		limelightRight.updateHardwareInputs();
-		limelightLeft.updateHardwareInputs();
-
-		limelightFront.updateMT1();
-		limelightRight.updateMT1();
-		limelightLeft.updateMT1();
-
-		limelightFront.getIndependentRobotPose().ifPresent(poseEstimator::updateVision);
-		limelightRight.getIndependentRobotPose().ifPresent(poseEstimator::updateVision);
-		limelightLeft.getIndependentRobotPose().ifPresent(poseEstimator::updateVision);
+		getLimelights().forEach(Limelight::updateHardwareInputs);
+		getLimelights().forEach(Limelight::updateMT1);
+		getLimelights().forEach(limelight -> limelight.getIndependentRobotPose().ifPresent(poseEstimator::updateVision));
 
 		poseEstimator.log();
 		ShootingCalculations
@@ -448,8 +408,8 @@ public Limelight getLimelightLeft() {
 		return limelightLeft;
 	}
 
-	public List<Limelight> getAllLimelights() {
-		return List.of(limelightFront, limelightRight, limelightLeft);
+	public List<Limelight> getLimelights() {
+		return limelights;
 	}
 
 	public RobotCommander getRobotCommander() {

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;
@@ -198,6 +203,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,
@@ -208,6 +222,15 @@ 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,
@@ -218,6 +241,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,
@@ -228,6 +260,15 @@ public boolean canContinuePassing() {
 		);
 	}
 
+	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(
 			robot,
@@ -271,6 +312,31 @@ public Command scoreSequence() {
 		);
 	}
 
+	public Command dumbScoreSequence() {
+		return new ParallelCommandGroup(
+			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(
+					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 +353,32 @@ public Command passSequence() {
 		);
 	}
 
+	public Command dumbPassSequence() {
+		return new ParallelCommandGroup(
+			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(
+						asSubsystemCommand(
+							funnelStateHandler.setState(FunnelState.PRE_SHOOT).until(this::isReadyToDumbPass),
+							RobotState.PRE_PASS
+						)
+					),
+					new ParallelCommandGroup(
+						asSubsystemCommand(
+							funnelStateHandler.setState(FunnelState.SHOOT).until(() -> !canContinueDumbPassing()),
+							RobotState.PASS
+						)
+					)
+				)
+			)
+		);
+	}
+
 	public Command calibrationScoreSequence() {
 		return new ParallelCommandGroup(
 			shooterStateHandler.setState(ShooterState.CALIBRATION),