prepare to score logic change

Author: ryanknj5

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

@@ -160,7 +160,7 @@ private void configureBindings() {
         .driverController
         .rightTrigger()
         .onTrue(robotCommands.confirmShotCommand())
-        .onFalse(robotCommands.waitSpeakerCommand());
+        ;
     hardware.driverController.leftTrigger().onTrue(robotCommands.intakeCommand());
 
     hardware
@@ -181,8 +181,8 @@ private void configureBindings() {
     hardware
         .driverController
         .x()
-        .onTrue(robotCommands.subwooferCommand())
-        .onFalse(robotCommands.stopShootingCommand());
+        //TODO:snap
+        .onFalse(robotCommands.waitSubwooferCommand());
     hardware.driverController.b().onTrue(robotCommands.waitAmpCommand());
 
     hardware.driverController.a().onTrue(robotCommands.stowCommand());

src/main/java/frc/robot/config/CompConfig.java

@@ -156,7 +156,7 @@ class CompConfig {
                 speakerDistanceToAngle.put(123.0, 321.0);
               },
               -77.0,
-              90.0),
+              87.0),
           new VisionConfig(4, 0.4, 0.4, InterpolatedVisionDataset.HOME));
 
   // NOT TUNED

src/main/java/frc/robot/robot_manager/RobotCommands.java

@@ -8,6 +8,7 @@
 public class RobotCommands {
   private final RobotManager robot;
   private final Subsystem[] requirements;
+  
 
   public RobotCommands(RobotManager robot) {
     this.robot = robot;
@@ -66,8 +67,10 @@ public Command waitSpeakerCommand() {
   }
 
   public Command confirmShotCommand() {
+    
     return Commands.runOnce(robot::confirmShotRequest, requirements)
         .andThen(robot.waitForState(RobotState.IDLE_NO_GP));
+        
   }
 
   public Command ampCommand() {

src/main/java/frc/robot/robot_manager/RobotManager.java

@@ -1,5 +1,6 @@
 package frc.robot.robot_manager;
 
+import edu.wpi.first.wpilibj.DriverStation;
 import frc.robot.arm.ArmState;
 import frc.robot.arm.ArmSubsystem;
 import frc.robot.imu.ImuSubsystem;
@@ -26,6 +27,8 @@ public class RobotManager extends StateMachine<RobotState> {
   private final double distanceToFeedSpot = 0.0;
   private final double distanceToSpeaker = 0.0;
 
+  private boolean confirmShotActive = false;
+
   public RobotManager(
       ArmSubsystem arm,
       ShooterSubsystem shooter,
@@ -58,7 +61,8 @@ protected RobotState getNextState(RobotState currentState) {
               IDLE_WITH_GP,
               CLIMBING_1_LINEUP,
               CLIMBING_2_HANGING,
-              PODIUM_WAITING ->
+              PODIUM_WAITING,
+              OUTTAKING ->
           currentState;
       case SPEAKER_SCORING,
               AMP_SCORING,
@@ -69,7 +73,7 @@ protected RobotState getNextState(RobotState currentState) {
           queuer.hasNote() ? currentState : RobotState.IDLE_NO_GP;
 
       case SPEAKER_PREPARE_TO_SCORE ->
-          shooter.atGoal() && arm.atGoal() ? RobotState.SPEAKER_SCORING : currentState;
+          shooter.atGoal() && arm.atGoal() && if(DriverStation.isTeleop()){getConfirmShotActive()==true} ? RobotState.SPEAKER_SCORING : currentState;
 
       case AMP_PREPARE_TO_SCORE ->
           shooter.atGoal() && arm.atGoal() ? RobotState.AMP_SCORING : currentState;
@@ -85,7 +89,6 @@ protected RobotState getNextState(RobotState currentState) {
 
       case UNJAM -> currentState;
       case INTAKING -> queuer.hasNote() ? RobotState.IDLE_WITH_GP : currentState;
-      case OUTTAKING -> queuer.hasNote() ? currentState : RobotState.IDLE_NO_GP;
     };
   }
 
@@ -227,8 +230,18 @@ public void robotPeriodic() {
     }
   }
 
+  public void setConfirmShotActive(boolean newValue) {
+    confirmShotActive = newValue;
+    
+  }
+  public boolean getConfirmShotActive(){
+    return confirmShotActive;
+    
+  }
+
   public void confirmShotRequest() {
     switch (getState()) {
+      
       case CLIMBING_1_LINEUP, CLIMBING_2_HANGING -> {}
 
       case AMP_WAITING -> setStateFromRequest(RobotState.AMP_PREPARE_TO_SCORE);
@@ -270,6 +283,9 @@ public void unjamRequest() {
   public void intakeRequest() {
     switch (getState()) {
       case CLIMBING_1_LINEUP, CLIMBING_2_HANGING -> {}
+      case IDLE_WITH_GP->{if(!queuer.hasNote()){
+        setStateFromRequest(RobotState.INTAKING);
+      }}
       default -> setStateFromRequest(RobotState.INTAKING);
     }
   }