Changed req handling

src/main/java/frc4388/robot/commands/CommandChooser.java

@@ -8,6 +8,7 @@ import java.util.Collections;
 import java.util.HashMap;
 import java.util.Set;
 import java.util.function.BooleanSupplier;
+import java.util.function.Consumer;
 
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandBase;
@@ -19,7 +20,10 @@ public class CommandChooser extends CommandBase {
   
   // // private Command chosen;
 
-  /** Creates a new CommandChooser. */
+  /** Creates a new CommandChooser.
+   * @author Aarav Shah
+   * @author Daniel Thomas McGrath
+  */
   public CommandChooser(HashMap<Command, BooleanSupplier> commandMap) {
     this.commandMap = commandMap;
 
@@ -31,32 +35,42 @@ public class CommandChooser extends CommandBase {
     addRequirements((Subsystem[]) allReqs.toArray());
   }
 
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {
-    for(Command command : commandMap.keySet())
-      if(commandMap.get(command).getAsBoolean()) command.initialize();
+  /**
+   * Runs an operation on every command in the group
+   * 
+   * @param consumer operation to run
+   */
+  public void runCommands(Consumer<Command> consumer) {
+    Set<Subsystem> reqCheck = Collections.emptySet();
+    
+    for(Command command : commandMap.keySet()) {
+      boolean reqFree = true;
+      for(Subsystem req : (Subsystem[]) command.getRequirements().toArray())
+        reqFree &= !reqCheck.contains(req);
+
+      if(commandMap.get(command).getAsBoolean() && reqFree) {
+        consumer.accept(command);
+        reqCheck.addAll(command.getRequirements());
+      }
+    }
   }
 
-  // Called every time the scheduler runs while the command is scheduled.
   @Override
-  public void execute() {
-    for(Command command : commandMap.keySet())
-      if(commandMap.get(command).getAsBoolean()) command.execute();
-  }
+  public void initialize() { runCommands(c -> c.initialize()); }
 
-  // Called once the command ends or is interrupted.
   @Override
-  public void end(boolean interrupted) {
-    for(Command command : commandMap.keySet())
-      if(commandMap.get(command).getAsBoolean()) command.end(interrupted);
-  }
+  public void execute() { runCommands(c -> c.execute()); }
+
+  @Override
+  public void end(boolean interrupted) { runCommands(c -> c.end(interrupted)); }
 
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
     boolean finished = true;
 
+    // Checks that everything is finished
+    //! command will not finish if there is an unfinished command
     for(Command command : commandMap.keySet())
       if(commandMap.get(command).getAsBoolean()) finished &= command.isFinished();