one piece mobilities

StuyPulse · Oct 3, 2024 · f61a483 · f61a483
1 parent 29d5dc0
commit f61a483
Show file tree

Hide file tree

Showing 5 changed files with 48 additions and 17 deletions.
diff --git a/src/main/deploy/pathplanner/paths/Blue Amp Side Mobility.path b/src/main/deploy/pathplanner/paths/Blue Amp Side Mobility.path
@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 2.9026396514331507,
-        "y": 6.9956624528633675
+        "x": 2.496814123442796,
+        "y": 7.4598028796628215
       },
       "prevControl": {
-        "x": 1.9026396514331507,
-        "y": 6.9956624528633675
+        "x": 1.496814123442796,
+        "y": 7.4598028796628215
       },
       "nextControl": null,
       "isLocked": false,

diff --git a/src/main/deploy/pathplanner/paths/Red Amp Side Mobility.path b/src/main/deploy/pathplanner/paths/Red Amp Side Mobility.path
@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 2.9026396514331507,
-        "y": 6.9956624528633675
+        "x": 2.4851032868970147,
+        "y": 7.436381206571257
       },
       "prevControl": {
-        "x": 1.9026396514331507,
-        "y": 6.9956624528633675
+        "x": 1.4851032868970147,
+        "y": 7.436381206571257
       },
       "nextControl": null,
       "isLocked": false,

diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java
@@ -13,6 +13,7 @@
 import com.stuypulse.robot.commands.arm.ArmToSpeaker;
 import com.stuypulse.robot.commands.arm.ArmToSubwooferShot;
 import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget;
+import com.stuypulse.robot.commands.auton.CenterMobilityWithWait;
 import com.stuypulse.robot.commands.auton.DoNothingAuton;
 import com.stuypulse.robot.commands.auton.Mobility;
 import com.stuypulse.robot.commands.auton.ADEF.FivePieceADEF;
@@ -276,6 +277,11 @@ public void configureAutons() {
         AutonConfig MOBILITY_BLUE = new AutonConfig("Mobility", Mobility::new, "Mobility");
         AutonConfig MOBILITY_RED = new AutonConfig("Mobility", Mobility::new, "Mobility");
 
+        AutonConfig CENTER_MOBILITY_BLUE = new AutonConfig("Center Mobility", CenterMobilityWithWait::new, 
+            "Mobility");
+        AutonConfig CENTER_MOBILITY_RED = new AutonConfig("Center Mobility", CenterMobilityWithWait::new, 
+        "Mobility");
+
         // BCA
         AutonConfig BCA_BLUE = new AutonConfig("4 BCA", FourPieceBCA::new,
         "Blue Center to B", "Blue B to Center", "Blue Center to C", "Blue C to Shoot Before A", "Blue Center to A", "Blue A to Center");
@@ -294,19 +300,22 @@ public void configureAutons() {
         AutonConfig ADEF_RED = new AutonConfig("5 ADEF", FivePieceADEF::new,
         "Red Amp to A", "Red A to D", "Red D to Shoot", "Red D Shoot to E", "Red E to Shoot", "Red E Shoot to F");
 
-        // AutonConfig AMP_SIDE_ONE_PIECE_BLUE = new AutonConfig("Amp Side One Piece", OnePieceAmpSide::new, 
-        //     "Blue Amp Side Mobility");
-        // AutonConfig AMP_SIDE_ONE_PIECE_RED = new AutonConfig("Amp Side One Piece", OnePieceAmpSide::new, 
-        //     "Red Amp Side Mobility");
+        AutonConfig AMP_SIDE_ONE_PIECE_BLUE = new AutonConfig("Amp Side One Piece", OnePieceAmpSide::new, 
+            "Blue Amp Side Mobility");
+        AutonConfig AMP_SIDE_ONE_PIECE_RED = new AutonConfig("Amp Side One Piece", OnePieceAmpSide::new, 
+            "Red Amp Side Mobility");
 
         AutonConfig SOURCE_SIDE_ONE_PIECE_BLUE = new AutonConfig("Source Side One Piece", OnePieceSourceSide::new, 
         "Blue Source Side Mobility");
 
         AutonConfig SOURCE_SIDE_ONE_PIECE_RED = new AutonConfig("Source Side One Piece", OnePieceSourceSide::new, 
         "Red Source Side Mobility");
 
-        // AMP_SIDE_ONE_PIECE_BLUE.registerBlue(autonChooser);
-        // AMP_SIDE_ONE_PIECE_RED.registerRed(autonChooser);
+        AMP_SIDE_ONE_PIECE_BLUE.registerBlue(autonChooser);
+        AMP_SIDE_ONE_PIECE_RED.registerRed(autonChooser);
+
+        CENTER_MOBILITY_BLUE.registerBlue(autonChooser);
+        CENTER_MOBILITY_RED.registerRed(autonChooser);
 
         SOURCE_SIDE_ONE_PIECE_BLUE.registerBlue(autonChooser);
         SOURCE_SIDE_ONE_PIECE_RED.registerRed(autonChooser);

diff --git a/src/main/java/com/stuypulse/robot/commands/auton/CenterMobilityWithWait.java b/src/main/java/com/stuypulse/robot/commands/auton/CenterMobilityWithWait.java
@@ -0,0 +1,22 @@
+package com.stuypulse.robot.commands.auton;
+
+import com.pathplanner.lib.path.PathPlannerPath;
+import com.stuypulse.robot.commands.arm.ArmToFeed;
+import com.stuypulse.robot.commands.intake.IntakeSetAcquire;
+import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+
+public class CenterMobilityWithWait extends SequentialCommandGroup {
+
+    public CenterMobilityWithWait(PathPlannerPath... paths) {
+        addCommands(
+            ShootRoutine.fromSubwoofer(),
+            new ArmToFeed(),
+            new WaitCommand(10),
+            new IntakeSetAcquire(),
+            SwerveDrive.getInstance().followPathCommand(paths[0])
+        );
+    }
+}
diff --git a/src/main/java/com/stuypulse/robot/commands/auton/SideAutons/OnePieceAmpSide.java b/src/main/java/com/stuypulse/robot/commands/auton/SideAutons/OnePieceAmpSide.java
@@ -24,9 +24,9 @@ public OnePieceAmpSide(PathPlannerPath... paths) {
 
             new IntakeSetAcquire(),
             // Mobility
-            SwerveDrive.getInstance().followPathCommand(paths[0]),
-            new WaitUntilCommand(() -> Shooter.getInstance().hasNote()).andThen(ShootRoutine.fromAnywhere())
-                .onlyIf(() -> Intake.getInstance().hasNote() || Shooter.getInstance().hasNote())
+            SwerveDrive.getInstance().followPathCommand(paths[0])
+            // new WaitUntilCommand(() -> Shooter.getInstance().hasNote()).andThen(ShootRoutine.fromAnywhere())
+            //     .onlyIf(() -> Intake.getInstance().hasNote() || Shooter.getInstance().hasNote())
         );
     }