functionality for path-mech parallels added

src/main/java/frc/robot/commands/Autos.java

@@ -17,6 +17,7 @@
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.subsystems.Swerve;
@@ -51,11 +52,18 @@ public static void runAutoCommand(AutoType auto) {
       () -> { Optional<DriverStation.Alliance> alliance = DriverStation.getAlliance();
         return alliance.isPresent() && alliance.get() == Alliance.Red; }, //decides whether or not the math should be mirrored (depends on alliance)
       s_Swerve);
-      commandsToSchedule.add(swerveCommand);
-      commandsToSchedule.add(auto.mechCommands[i]);
-      //CommandScheduler.getInstance().schedule(new SequentialCommandGroup(swerveCommand, auto.mechCommands[i]));
+
+      if(auto.parallelToPath[i]){
+        ParallelCommandGroup curr = new ParallelCommandGroup();
+        curr.addCommands(auto.mechCommands[i]);
+        curr.addCommands(swerveCommand);
+        commandsToSchedule.add(curr);
+      } else {
+        commandsToSchedule.add(swerveCommand);
+        commandsToSchedule.add(auto.mechCommands[i]);
+      }
     }
-    SequentialCommandGroup group = new SequentialCommandGroup(null);
+    SequentialCommandGroup group = new SequentialCommandGroup();
     for (Command i : commandsToSchedule) {
       group.addCommands(i);
     }
@@ -65,16 +73,28 @@ public static void runAutoCommand(AutoType auto) {
   }
 
   public enum AutoType {
+
+    //when writing enums, if you want multiple mechCommands to run before the next path, put them in a sequential command group
+    //if you want those mechCommands to run in parallel, put them in a parallelCommandGroup
+    //if you want to run a mechCommand or mechCommandGroup in parallel with a path, create a boolean array with true values corresponding to the mechCommands you want to run in parallel.
       TEST("test", new Command[]{new SetPivot(PivotState.GROUND), new SetPivot(PivotState.GROUND)}),
       ONEBALLAMP("one ball amp", new Command[]{}),
       BALLSPEAKER("ball speaker", new Command[]{});
 
       String name;
       Command[] mechCommands;
+      boolean[] parallelToPath;
+
+      private AutoType(String a, Command[] mechCommands, boolean[] parallelToPath){
+        name = a;
+        this.mechCommands = mechCommands;
+        this.parallelToPath = parallelToPath;
+      }
 
       private AutoType(String a, Command[] mechCommands){
         name = a;
         this.mechCommands = mechCommands;
+        parallelToPath = new boolean[mechCommands.length];
       }
   }
 }