diff --git a/src/main/java/frc4388/robot/commands/CommandChooser.java b/src/main/java/frc4388/robot/commands/CommandChooser.java index fa66a02..f6376fb 100644 --- a/src/main/java/frc4388/robot/commands/CommandChooser.java +++ b/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 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 consumer) { + Set 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();