From cc3e4ee1e77d7292d85f7a4ec2d887de95ce66f7 Mon Sep 17 00:00:00 2001
From: Sana Shah <54493565+sanashah007@users.noreply.github.com>
Date: Sat, 28 Oct 2023 18:45:58 -0700
Subject: [PATCH] feat: wamil day 1

---
 .../java/org/team1540/robot2023/RobotContainer.java    |  6 +++---
 .../bottom/AutoBottomGrid2_5PieceTaxiVision.java       | 10 +++-------
 2 files changed, 6 insertions(+), 10 deletions(-)

diff --git a/src/main/java/org/team1540/robot2023/RobotContainer.java b/src/main/java/org/team1540/robot2023/RobotContainer.java
index 09ee762..90474a5 100644
--- 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/src/main/java/org/team1540/robot2023/commands/auto/sequence/bottom/AutoBottomGrid2_5PieceTaxiVision.java b/src/main/java/org/team1540/robot2023/commands/auto/sequence/bottom/AutoBottomGrid2_5PieceTaxiVision.java
index cc8ccc6..40fc293 100644
--- a/src/main/java/org/team1540/robot2023/commands/auto/sequence/bottom/AutoBottomGrid2_5PieceTaxiVision.java
+++ b/src/main/java/org/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(