move duck to supplier

i love 🍝
FRC-4509-MechBulls · Feb 8, 2024 · 7ee616e · 7ee616e
1 parent ba5956f
commit 7ee616e
Show file tree

Hide file tree

Showing 2 changed files with 22 additions and 9 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -25,11 +25,13 @@
 public class RobotContainer {
   // The robot's subsystems and commands are defined here...
 
-  StateControllerSub stateController = new StateControllerSub(); //this MUST be created before any pathplanner commands
 
   CommandXboxController driver = new CommandXboxController(0);
   CommandXboxController operator = new CommandXboxController(1);
 
+  StateControllerSub stateController = new StateControllerSub(()->driver.getLeftTriggerAxis()>0.5 || driver.getRightTriggerAxis()>0.5); //this MUST be created before any pathplanner commands
+
+
   VisionSubsystem visionSub = new VisionSubsystem();
 
   SendableChooser<Command> autoChooser = new SendableChooser<>();
@@ -76,15 +78,17 @@ private void configureBindings() {
     operator.leftTrigger(0.5).onTrue(new InstantCommand(stateController::shootPressed));
 
 
-    driver.rightTrigger(0.5).onTrue(new InstantCommand(stateController::scheduleAlignmentCommand,swerveSubsystem));
-    driver.rightTrigger(0.5).onFalse(swerveSubsystem.getDefaultCommand());
+    driver.leftTrigger(0.5).onTrue(new InstantCommand(stateController::scheduleAlignmentCommand,swerveSubsystem));
+    driver.leftTrigger(0.5).onFalse(swerveSubsystem.getDefaultCommand());
+
+    driver.x().onTrue(new InstantCommand(stateController::shootPressed));
 
 
-    driver.leftTrigger(0.5).onTrue(new InstantCommand(()->stateController.setDuckMode(true)));
-    driver.leftTrigger(0.5).onFalse(new InstantCommand(()->stateController.setDuckMode(false)));
+   // driver.leftTrigger(0.5).onTrue(new InstantCommand(()->stateController.setDuckMode(true)));
+    //driver.leftTrigger(0.5).onFalse(new InstantCommand(()->stateController.setDuckMode(false)));
 
-    driver.rightTrigger(0.5).onTrue(new InstantCommand(()->stateController.setDuckMode(true)));
-    driver.rightTrigger(0.5).onFalse(new InstantCommand(()->stateController.setDuckMode(false)));
+   // driver.rightTrigger(0.5).onTrue(new InstantCommand(()->stateController.setDuckMode(true)));
+   // driver.rightTrigger(0.5).onFalse(new InstantCommand(()->stateController.setDuckMode(false)));
 
     driver.a().onTrue(new InstantCommand(stateController::toggleAlignWhenClose));
 

diff --git a/src/main/java/frc/robot/StateControllerSub.java b/src/main/java/frc/robot/StateControllerSub.java
@@ -12,6 +12,7 @@
 import frc.robot.util.MBUtils;
 
 import java.util.Optional;
+import java.util.function.Supplier;
 
 import static frc.robot.Constants.ShootingTables;
 
@@ -80,6 +81,7 @@ public void scheduleAlignmentCommand(){
 
     }
 
+    @Deprecated
     public void setDuckMode(boolean lowered){
         if(lowered)
             duckMode = DuckMode.DUCKING;
@@ -167,8 +169,10 @@ public void setArmState(ArmState state){
 
     public Objective getObjective(){return objective;}
 
+Supplier<Boolean> forceDuck;
 
-    public StateControllerSub(){
+
+    public StateControllerSub(Supplier<Boolean> forceDuck){
         NamedCommands.registerCommand("intakeMode",new InstantCommand(this::intakePressed));
         NamedCommands.registerCommand("holdMode",new InstantCommand(this::holdPressed));
         NamedCommands.registerCommand("setObjectiveSpeaker",new InstantCommand(this::speakerPressed));
@@ -181,7 +185,7 @@ public StateControllerSub(){
         SmartDashboard.putNumber("tuningFlywheelVel",10);
 
 
-
+        this.forceDuck = forceDuck;
     }
 
     double climbAngle = 0.0;
@@ -231,6 +235,11 @@ public void periodic(){
 
         SmartDashboard.putBoolean("alignToObjective",alignWhenClose);
 
+        if(forceDuck.get())
+            duckMode = DuckMode.DUCKING;
+        else
+            duckMode = DuckMode.NOT_LOWERED;
+
     }
 
     public boolean alignWhenClose() {