Working auto!

Prospect-Robotics · Nov 10, 2024 · 1156513 · 1156513
1 parent ff8b43d
commit 1156513
Show file tree

Hide file tree

Showing 2 changed files with 39 additions and 15 deletions.
diff --git a/SharpShooter/src/main/deploy/pathplanner/autos/SA-P-1F.auto b/SharpShooter/src/main/deploy/pathplanner/autos/SA-P-1F.auto
@@ -42,15 +42,35 @@
           }
         },
         {
-          "type": "named",
-          "data": {
-            "name": "Stop_Intake"
-          }
-        },
-        {
-          "type": "path",
+          "type": "parallel",
           "data": {
-            "pathName": "F1-Amp"
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "F1-Amp"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 1.0
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Stop_Intake"
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
           }
         },
         {

diff --git a/SharpShooter/src/main/java/com/team2813/RobotContainer.java b/SharpShooter/src/main/java/com/team2813/RobotContainer.java
@@ -28,6 +28,7 @@ public class RobotContainer {
     private final Elevator elevator = new Elevator();
     private final Drive drive = new Drive();
     private final SendableChooser<Command> autoChooser;
+    private boolean resetPosition = false;
 
     public RobotContainer() {
         elevator.setDefaultCommand(
@@ -39,11 +40,6 @@ public RobotContainer() {
                         () -> -modifyAxis(DRIVER_CONTROLLER.getLeftX()) * Drive.MAX_VELOCITY,
                         () -> -modifyAxis(DRIVER_CONTROLLER.getRightX()) * Drive.MAX_ROTATION));
 
-        autoChooser = AutoBuilder.buildAutoChooser();
-        SmartDashboard.putData(autoChooser);
-
-        configureBindings();
-
         NamedCommands.registerCommand(
                 "Amp",
                 new SequentialCommandGroup(
@@ -75,6 +71,11 @@ public RobotContainer() {
                         new InstantCommand(amp::stop, amp)
                 )
         );
+
+        autoChooser = AutoBuilder.buildAutoChooser();
+        SmartDashboard.putData(autoChooser);
+
+        configureBindings();
     }
 
     private static double deadband(double value, double deadband) {
@@ -192,9 +193,12 @@ private void configureBindings() {
 
         RESET_POSITIONING.whileTrue(
                 new SequentialCommandGroup(
-                        new WaitCommand(1),
+                        new WaitCommand(0.25),
                         new ParallelCommandGroup(
-                                new InstantCommand(() -> SmartDashboard.putBoolean("reset-position", true)),
+                                new InstantCommand(() -> {
+                                    resetPosition = !resetPosition;
+                                    SmartDashboard.putBoolean("reset-position", resetPosition);
+                                }),
                                 new InstantCommand(drive::resetRotation, drive)
                         )