Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 00:38:05 -06:00
Code Issues Packages Projects Releases Wiki Activity

Made better commandChooser

Browse Source
This commit is contained in:
66945
2022-03-19 11:10:04 -06:00
parent d47cf27633
commit db66cf8d55
2 changed files with 32 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+11 -4
View File
@@ -19,9 +19,12 @@ import java.time.format.DateTimeFormatter;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Comparator;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
import java.util.Objects;
import java.util.Optional;
import java.util.function.BooleanSupplier;
import java.util.function.Function;
import java.util.logging.Level;
import java.util.logging.Logger;
@@ -379,10 +382,14 @@ public class RobotContainer {
// .whenPressed(new RunTurretOrClimberAuto(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry, m_robotClimber, m_robotClaws))
// * CommandChooser with BooleanSuppliers
.whenPressed(new CommandChooser(new RunClimberPath(m_robotClimber, m_robotClaws, new Point[] {new Point()}),
new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry),
() -> this.currentControlMode.equals(SubsystemMode.CLIMBER),
() -> this.currentControlMode.equals(SubsystemMode.SHOOTER)))
.whenPressed(new CommandChooser(new HashMap<Command, BooleanSupplier>() {{
put(new RunClimberPath(m_robotClimber, m_robotClaws, new Point[] {new Point()}), () -> currentControlMode.equals(SubsystemMode.CLIMBER));
put(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry), () -> currentControlMode.equals(SubsystemMode.SHOOTER));
}}))
// .whenPressed(new CommandChooser(new RunClimberPath(m_robotClimber, m_robotClaws, new Point[] {new Point()}),
// new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry),
// () -> this.currentControlMode.equals(SubsystemMode.CLIMBER),
// () -> this.currentControlMode.equals(SubsystemMode.SHOOTER)))
.whenReleased(new InstantCommand(() -> {
if (this.currentControlMode.equals(SubsystemMode.SHOOTER)) { this.currentTurretMode = ControlMode.AUTONOMOUS; }
src/main/java/frc4388/robot/commands/CommandChooser.java
+21 -28
View File
@@ -4,69 +4,62 @@
package frc4388.robot.commands;
import java.util.Collections;
import java.util.HashMap;
import java.util.Set;
import java.util.function.BooleanSupplier;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj2.command.CommandGroupBase;
import edu.wpi.first.wpilibj2.command.Subsystem;
public class CommandChooser extends CommandBase {
private Command c1;
private Command c2;
private Boolean b1;
private Boolean b2;
private HashMap<Command, BooleanSupplier> commandMap;
private Command chosen;
// // private Command chosen;
/** Creates a new CommandChooser. */
public CommandChooser(Command c1, Command c2, BooleanSupplier bs1, BooleanSupplier bs2) {
// Use addRequirements() here to declare subsystem dependencies.
public CommandChooser(HashMap<Command, BooleanSupplier> commandMap) {
this.commandMap = commandMap;
this.c1 = c1;
this.c2 = c2;
Set<Subsystem> allReqs = Collections.emptySet();
this.b1 = bs1.getAsBoolean();
this.b2 = bs2.getAsBoolean();
for(Command command : commandMap.keySet())
allReqs.addAll(command.getRequirements());
Set<Subsystem> allReqs = c1.getRequirements();
allReqs.addAll(c2.getRequirements());
addRequirements((Subsystem[]) allReqs.toArray());
}
public Command getChosen() {
if (this.b1) {
return this.c1;
} else {
return this.c2;
}
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
this.chosen = getChosen();
this.chosen.initialize();
for(Command command : commandMap.keySet())
if(commandMap.get(command).getAsBoolean()) command.initialize();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
this.chosen.execute();
for(Command command : commandMap.keySet())
if(commandMap.get(command).getAsBoolean()) command.execute();
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
this.chosen.end(interrupted);
for(Command command : commandMap.keySet())
if(commandMap.get(command).getAsBoolean()) command.end(interrupted);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return this.chosen.isFinished();
boolean finished = true;
for(Command command : commandMap.keySet())
if(commandMap.get(command).getAsBoolean()) finished &= command.isFinished();
return finished;
}
}