feat: wamil day 1

flamingchickens1540 · Oct 29, 2023 · cc3e4ee · cc3e4ee
1 parent 9a10bbd
commit cc3e4ee
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 10 deletions.
diff --git a/src/main/java/org/team1540/robot2023/RobotContainer.java b/src/main/java/org/team1540/robot2023/RobotContainer.java
@@ -112,9 +112,9 @@ private void configureButtonBindings() {
         controlPanel.onButton(ButtonPanel.PanelButton.MIDDLE_LEFT  ).whileTrue(drivetrain.updateOdometryAnd(new AutoCone(drivetrain, arm,Constants.Auto.midCone.withPolePosition(PolePosition.LEFT),     intake, driver, false)));
         controlPanel.onButton(ButtonPanel.PanelButton.MIDDLE_CENTER).whileTrue(drivetrain.updateOdometryAnd(new AutoCube(drivetrain, arm,Constants.Auto.midCube.withPolePosition(PolePosition.CENTER),     intake, driver, false)));
         controlPanel.onButton(ButtonPanel.PanelButton.MIDDLE_RIGHT ).whileTrue(drivetrain.updateOdometryAnd(new AutoCone(drivetrain, arm,Constants.Auto.midCone.withPolePosition(PolePosition.RIGHT),     intake, driver, false)));
-        controlPanel.onButton(ButtonPanel.PanelButton.BOTTOM_LEFT  ).whileTrue(drivetrain.updateOdometryAnd(new AutoHybrid(drivetrain, arm,Constants.Auto.hybridNode.withPolePosition(PolePosition.LEFT),  intake, driver, false)));
-        controlPanel.onButton(ButtonPanel.PanelButton.BOTTOM_CENTER).whileTrue(drivetrain.updateOdometryAnd(new AutoHybrid(drivetrain, arm,Constants.Auto.middleHybridNode.withPolePosition(PolePosition.CENTER),  intake, driver, false)));
-        controlPanel.onButton(ButtonPanel.PanelButton.BOTTOM_RIGHT ).whileTrue(drivetrain.updateOdometryAnd(new AutoHybrid(drivetrain, arm,Constants.Auto.hybridNode.withPolePosition(PolePosition.RIGHT),  intake, driver, false)));
+        controlPanel.onButton(ButtonPanel.PanelButton.BOTTOM_LEFT  ).whileTrue(drivetrain.updateOdometryAnd(new AutoCone(drivetrain, arm,Constants.Auto.highCone.withPolePosition(PolePosition.LEFT),    intake, driver, true)));
+        controlPanel.onButton(ButtonPanel.PanelButton.BOTTOM_CENTER).whileTrue(drivetrain.updateOdometryAnd(new AutoCube(drivetrain, arm,Constants.Auto.highCube.withPolePosition(PolePosition.CENTER),    intake, driver, true)));
+        controlPanel.onButton(ButtonPanel.PanelButton.BOTTOM_RIGHT ).whileTrue(drivetrain.updateOdometryAnd(new AutoCone(drivetrain, arm,Constants.Auto.highCone.withPolePosition(PolePosition.RIGHT),    intake, driver, true)));
 
         // coop:button(A, Run Intake [PRESS],copilot)
         copilot.a().toggleOnTrue(intakeCommand);

diff --git a/...rg/team1540/robot2023/commands/auto/sequence/bottom/AutoBottomGrid2_5PieceTaxiVision.java b/...rg/team1540/robot2023/commands/auto/sequence/bottom/AutoBottomGrid2_5PieceTaxiVision.java
@@ -7,6 +7,7 @@
 import org.team1540.robot2023.commands.arm.PivotCommand;
 import org.team1540.robot2023.commands.arm.ResetArmPositionCommand;
 import org.team1540.robot2023.commands.arm.SetArmPosition;
+import org.team1540.robot2023.commands.auto.AutoCube;
 import org.team1540.robot2023.commands.drivetrain.Drivetrain;
 import org.team1540.robot2023.commands.grabber.GrabberIntakeCommand;
 import org.team1540.robot2023.commands.grabber.GrabberOuttakeCommand;
@@ -15,6 +16,7 @@
 import org.team1540.robot2023.commands.vision.TurnToGamePiece;
 import org.team1540.robot2023.utils.AutoCommand;
 import org.team1540.robot2023.utils.Limelight;
+import org.team1540.robot2023.utils.PolePosition;
 
 import java.util.List;
 
@@ -27,13 +29,7 @@ public AutoBottomGrid2_5PieceTaxiVision(Drivetrain drivetrain, Arm arm, WheeledG
         setName("BottomGrid2.5PieceTaxiVision");
         addCommands(
                 new InstantCommand(()-> limelight.setPipeline(Limelight.Pipeline.GAME_PIECE)),
-                Commands.deadline(
-                        new SetArmPosition(arm, Constants.Auto.highCube.approach),
-                        Commands.sequence(
-                                new ProxyCommand(() -> new WaitCommand((arm.timeToState(Constants.Auto.highCube.approach)-150)/1000)),
-                                new GrabberOuttakeCommand(intake)
-                        )
-                ),
+                new AutoCube(drivetrain, arm, Constants.Auto.highCube.withPolePosition(PolePosition.CENTER), intake, null, false),
                 Commands.parallel(
                         new GrabberIntakeCommand(intake),
                         Commands.sequence(