Comp can be dirty.

at least auto works.
Patribots4738 · Apr 5, 2024 · aa0af84 · aa0af84
1 parent 79f3398
commit aa0af84
Show file tree

Hide file tree

Showing 4 changed files with 43 additions and 5 deletions.
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -116,6 +116,7 @@ public void disabledExit() {
     @Override   
     public void autonomousInit() {
         // Update "constants"
+        Monologue.updateAll();
         DriveConstants.MAX_SPEED_METERS_PER_SECOND = AutoConstants.MAX_SPEED_METERS_PER_SECOND;
         Robot.gameMode = GameMode.AUTONOMOUS;
         robotContainer.onEnabled();

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -222,8 +222,15 @@ public RobotContainer() {
                 () -> driver.getLeftTrigger() || (!OIConstants.SINGLE_DRIVER_MODE && operator.getLeftBumper())    
             )
         );
+
+        BooleanSupplier autoDecisionMaker;
+        if (FieldConstants.IS_SIMULATION) {
+            autoDecisionMaker = driver::getYButton;
+        } else {
+            autoDecisionMaker = colorSensor::hasNote;
+        }
+        pathPlannerStorage = new PathPlannerStorage(autoDecisionMaker, swerve, limelight3);
 
-        pathPlannerStorage = new PathPlannerStorage(colorSensor::hasNote, swerve, limelight3);
         initializeComponents();
         prepareNamedCommands();
 
@@ -734,15 +741,37 @@ public void updateNTGains() {
     }
 
     private void prepareNamedCommands() {
-        // TODO: prepare to shoot while driving (w1 - c1)
         NamedCommands.registerCommand("Intake", pieceControl.intakeAuto());
-        NamedCommands.registerCommand("ToIndexer", pieceControl.intakeUntilNote());
+        // If you can fix this formatting I salute you
+        NamedCommands.registerCommand("ToIndexer",
+            Commands.either(
+                pieceControl.intakeUntilNote(),
+                Commands.waitUntil(driver::getYButton),
+                () -> !FieldConstants.IS_SIMULATION
+            )
+        );
         NamedCommands.registerCommand("StopIntake", pieceControl.stopIntakeAndIndexer());
         NamedCommands.registerCommand("StopAll", pieceControl.stopAllMotors());
         NamedCommands.registerCommand("PrepareShooter", shooterCmds.prepareFireCommandAuto(swerve::getPose));
         NamedCommands.registerCommand("Shoot", pieceControl.noteToShoot(swerve::getPose, swerve::getRobotRelativeVelocity));
-        NamedCommands.registerCommand("ShootInstantly", pieceControl.noteToShootUsingSensor(swerve::getPose, swerve::getRobotRelativeVelocity));
-        NamedCommands.registerCommand("ShootInstantlyWhenReady", Commands.waitSeconds(.4).andThen(pieceControl.noteToShootUsingSensorWhenReady(swerve::getPose, swerve::getRobotRelativeVelocity)));
+        NamedCommands.registerCommand("ShootInstantly", 
+            Commands.either(
+                pieceControl.noteToShootUsingSensor(swerve::getPose, swerve::getRobotRelativeVelocity),
+                Commands.waitUntil(() -> !driver.getYButton()), 
+                () -> !FieldConstants.IS_SIMULATION
+            )
+        );
+        // This one too, what a necessary nightmare :(
+        NamedCommands.registerCommand("ShootInstantlyWhenReady",
+            Commands.waitSeconds(.4)
+                .andThen(
+                    Commands.either(
+                        pieceControl.noteToShootUsingSensorWhenReady(swerve::getPose, swerve::getRobotRelativeVelocity),
+                        Commands.waitUntil(() -> !driver.getYButton()), 
+                        () -> !FieldConstants.IS_SIMULATION
+                    )
+                )
+            );
         NamedCommands.registerCommand("ShootWhenReady", pieceControl.shootPreload());
         NamedCommands.registerCommand("RaiseElevator", elevator.toTopCommand());
         NamedCommands.registerCommand("LowerElevator", elevator.toBottomCommand());

diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java
@@ -207,6 +207,12 @@ public Rotation2d getGyroRotation2d() {
         return this.gyroRotation2d;
     }
 
+    public boolean insideOwnWing() {
+        return Robot.isBlueAlliance() 
+            ? getPose().getX() < FieldConstants.BLUE_WING_X 
+            : getPose().getX() > FieldConstants.RED_WING_X;
+    }
+
     public SwerveDrivePoseEstimator getPoseEstimator() {
         return poseEstimator;
     }

diff --git a/src/main/java/frc/robot/util/auto/PathPlannerStorage.java b/src/main/java/frc/robot/util/auto/PathPlannerStorage.java
@@ -268,6 +268,8 @@ private Command generateNonObjectDetectionCommand(int i, int endingNote, boolean
                         .onlyIf(() -> !colorSensorSupplier.getAsBoolean())))
                 .andThen(NamedCommands.getCommand("ShootInstantlyWhenReady"))
                 .deadlineWith(NamedCommands.getCommand("PrepareSWD"))
+                .raceWith(Commands.waitUntil(() -> !colorSensorSupplier.getAsBoolean() && swerve.insideOwnWing()))
+
                 .andThen(
                     Commands.race(
                         Commands.sequence(