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.ArrayList;
import java.util.Arrays; import java.util.Arrays;
import java.util.Comparator; import java.util.Comparator;
import java.util.HashMap;
import java.util.List; import java.util.List;
import java.util.Map;
import java.util.Objects; import java.util.Objects;
import java.util.Optional; import java.util.Optional;
import java.util.function.BooleanSupplier;
import java.util.function.Function; import java.util.function.Function;
import java.util.logging.Level; import java.util.logging.Level;
import java.util.logging.Logger; 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)) // .whenPressed(new RunTurretOrClimberAuto(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry, m_robotClimber, m_robotClaws))
// * CommandChooser with BooleanSuppliers // * CommandChooser with BooleanSuppliers
.whenPressed(new CommandChooser(new RunClimberPath(m_robotClimber, m_robotClaws, new Point[] {new Point()}), .whenPressed(new CommandChooser(new HashMap<Command, BooleanSupplier>() {{
new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry), put(new RunClimberPath(m_robotClimber, m_robotClaws, new Point[] {new Point()}), () -> currentControlMode.equals(SubsystemMode.CLIMBER));
() -> this.currentControlMode.equals(SubsystemMode.CLIMBER), put(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry), () -> currentControlMode.equals(SubsystemMode.SHOOTER));
() -> this.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(() -> { .whenReleased(new InstantCommand(() -> {
if (this.currentControlMode.equals(SubsystemMode.SHOOTER)) { this.currentTurretMode = ControlMode.AUTONOMOUS; } 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; package frc4388.robot.commands;
import java.util.Collections;
import java.util.HashMap;
import java.util.Set; import java.util.Set;
import java.util.function.BooleanSupplier; import java.util.function.BooleanSupplier;
import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj2.command.CommandGroupBase;
import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.Subsystem;
public class CommandChooser extends CommandBase { public class CommandChooser extends CommandBase {
private Command c1; private HashMap<Command, BooleanSupplier> commandMap;
private Command c2;
private Boolean b1; // // private Command chosen;
private Boolean b2;
private Command chosen;
/** Creates a new CommandChooser. */ /** Creates a new CommandChooser. */
public CommandChooser(Command c1, Command c2, BooleanSupplier bs1, BooleanSupplier bs2) { public CommandChooser(HashMap<Command, BooleanSupplier> commandMap) {
// Use addRequirements() here to declare subsystem dependencies. this.commandMap = commandMap;
this.c1 = c1; Set<Subsystem> allReqs = Collections.emptySet();
this.c2 = c2;
this.b1 = bs1.getAsBoolean(); for(Command command : commandMap.keySet())
this.b2 = bs2.getAsBoolean(); allReqs.addAll(command.getRequirements());
Set<Subsystem> allReqs = c1.getRequirements();
allReqs.addAll(c2.getRequirements());
addRequirements((Subsystem[]) allReqs.toArray()); addRequirements((Subsystem[]) allReqs.toArray());
} }
public Command getChosen() {
if (this.b1) {
return this.c1;
} else {
return this.c2;
}
}
// Called when the command is initially scheduled. // Called when the command is initially scheduled.
@Override @Override
public void initialize() { public void initialize() {
this.chosen = getChosen(); for(Command command : commandMap.keySet())
this.chosen.initialize(); if(commandMap.get(command).getAsBoolean()) command.initialize();
} }
// Called every time the scheduler runs while the command is scheduled. // Called every time the scheduler runs while the command is scheduled.
@Override @Override
public void execute() { 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. // Called once the command ends or is interrupted.
@Override @Override
public void end(boolean interrupted) { 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. // Returns true when the command should end.
@Override @Override
public boolean isFinished() { 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;
} }
} }