feat: CenterLanePBc auto

flamingchickens1540 · Mar 6, 2024 · 76e2316 · 76e2316
1 parent a065f1b
commit 76e2316
Show file tree

Hide file tree

Showing 2 changed files with 51 additions and 7 deletions.
diff --git a/paths.chor b/paths.chor
@@ -11664,7 +11664,7 @@
                     "controlIntervalCount": 23
                 },
                 {
-                    "x": 2.462606430053711,
+                    "x": 2.5,
                     "y": 5.547596454620361,
                     "heading": 0,
                     "isInitialGuess": false,
@@ -12634,37 +12634,43 @@
                     "scope": [
                         "first"
                     ],
-                    "type": "StopPoint"
+                    "type": "StopPoint",
+                    "direction": 0
                 },
                 {
                     "scope": [
                         "last"
                     ],
-                    "type": "StopPoint"
+                    "type": "StopPoint",
+                    "direction": 0
                 },
                 {
                     "scope": [
                         0
                     ],
-                    "type": "StopPoint"
+                    "type": "StopPoint",
+                    "direction": 0
                 },
                 {
                     "scope": [
                         1
                     ],
-                    "type": "StopPoint"
+                    "type": "StopPoint",
+                    "direction": 0
                 },
                 {
                     "scope": [
                         2
                     ],
-                    "type": "StopPoint"
+                    "type": "StopPoint",
+                    "direction": 0
                 },
                 {
                     "scope": [
                         3
                     ],
-                    "type": "StopPoint"
+                    "type": "StopPoint",
+                    "direction": 0
                 }
             ],
             "usesControlIntervalGuessing": true,

diff --git a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePBc.java b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePBc.java
@@ -0,0 +1,38 @@
+package org.team1540.robot2024.commands.autos;
+
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import org.team1540.robot2024.commands.indexer.IntakeCommand;
+import org.team1540.robot2024.commands.shooter.ShootSequence;
+import org.team1540.robot2024.subsystems.drive.Drivetrain;
+import org.team1540.robot2024.subsystems.indexer.Indexer;
+import org.team1540.robot2024.subsystems.shooter.Shooter;
+import org.team1540.robot2024.util.auto.AutoCommand;
+import org.team1540.robot2024.util.auto.PathHelper;
+
+public class CenterLanePBc extends AutoCommand {
+    public CenterLanePBc(Drivetrain drivetrain, Shooter shooter, Indexer indexer) {
+        super("CenterLanePBc");
+
+        addPath(
+                PathHelper.fromChoreoPath("CenterLanePBc.1.traj", true, true),
+                PathHelper.fromChoreoPath("CenterLanePBc.2.traj"),
+                PathHelper.fromChoreoPath("CenterLanePBc.3.traj"),
+                PathHelper.fromChoreoPath("CenterLanePBc.4.traj")
+        );
+
+        addCommands(
+                getPath(0).getCommand(drivetrain),
+                new ShootSequence(shooter, indexer),
+                new ParallelCommandGroup(
+                        new IntakeCommand(indexer, ()->false, 1).withTimeout(3),
+                        getPath(1).getCommand(drivetrain)
+                ),
+                getPath(2).getCommand(drivetrain),
+                new ShootSequence(shooter, indexer),
+                new ParallelCommandGroup(
+                        new IntakeCommand(indexer, ()->false, 1).withTimeout(3),
+                        getPath(3).getCommand(drivetrain)
+                )
+        );
+    }
+}