feat: add cube confirm

flamingchickens1540 · Oct 20, 2023 · dd66936 · dd66936
1 parent 51183a5
commit dd66936
Showing 1 changed file with 3 additions and 10 deletions.
diff --git a/src/main/java/org/team1540/robot2023/commands/auto/AutoCube.java b/src/main/java/org/team1540/robot2023/commands/auto/AutoCube.java
@@ -24,18 +24,11 @@ public AutoCube(Drivetrain drivetrain, Arm arm, GridScoreData positions, Wheeled
                 new ConditionalCommand(
                         Commands.sequence(
                                 new AutoGridAlign(drivetrain, positions, shouldAlign),
-                                //                            new ProxiedGridDriveCommand(drivetrain, positions),
-                                Commands.deadline(
-                                        new SetArmPosition(arm, positions.approach),
-                                        Commands.sequence(
-                                                new ProxyCommand(() -> new WaitCommand((arm.timeToState(positions.approach)-150)/1000)),
-
-                                                new GrabberOuttakeCommand(intake)
-
-                                        )
-                                ),
+                                new SetArmPosition(arm, positions.approach),
+                                new WaitUntilCommand(() -> controller.getLeftTriggerAxis() > 0.95).unless(() -> controller == null),
                                 Commands.deadline(
                                         Commands.sequence(
+                                                new WaitCommand(0.1),
                                                 new SetArmPosition(arm, AutoHybrid.catchNull(positions.retreat)).unless(() -> positions.retreat == null),
                                                 new ResetArmPositionCommand(arm, false),
                                                 new ResetArmPositionCommand(arm, true)