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

probably decided to go with the CommandChooser solution, this shoudl work

Browse Source
This commit is contained in:
aarav18
2022-03-18 19:04:43 -06:00
parent 1fbf6bc9e9
commit f890dda9f7
2 changed files with 47 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+13 -11
View File
@@ -68,6 +68,7 @@ import frc4388.robot.Constants.OIConstants;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.Constants.StorageConstants;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.robot.commands.CommandChooser;
import frc4388.robot.commands.ButtonBoxCommands.RunMiddleSwitch;
import frc4388.robot.commands.ButtonBoxCommands.RunTurretOrClimberAuto;
import frc4388.robot.commands.ClimberCommands.RunClaw;
@@ -154,8 +155,8 @@ public class RobotContainer {
public static boolean softLimits = true;
// control mode switching
private enum ControlMode { SHOOTER, CLIMBER };
private ControlMode currentControlMode = ControlMode.SHOOTER;
public static enum ControlMode { SHOOTER, CLIMBER };
public static ControlMode currentControlMode = ControlMode.SHOOTER;
// turret mode switching
private enum TurretMode { MANUAL, AUTONOMOUS };
@@ -362,19 +363,20 @@ public class RobotContainer {
}))
// * custom Command inside InstantCommand
.whenPressed(new InstantCommand(() -> {
if (this.currentControlMode.equals(ControlMode.SHOOTER)) { new RunClimberPath(m_robotClimber, m_robotClaws, new Point[] {new Point()}); }
if (this.currentControlMode.equals(ControlMode.CLIMBER)) { new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry); }
})
.until(() -> this.currentClimberMode.equals(ClimberMode.MANUAL)))
// .whenPressed(new InstantCommand(() -> {
// if (this.currentControlMode.equals(ControlMode.CLIMBER)) { new RunClimberPath(m_robotClimber, m_robotClaws, new Point[] {new Point()}); }
// if (this.currentControlMode.equals(ControlMode.SHOOTER)) { new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry); }
// })
// .until(() -> this.currentClimberMode.equals(ClimberMode.MANUAL)))
// * external RunTurretOrClimberAutos command, which runs either one (conditionally) based on currentControlMode
.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
// .whenPressed(new RunClimberPath(m_robotClimber, m_robotClaws, new Point[] {new Point()}).until(() -> this.currentClimberMode.equals(ClimberMode.MANUAL)))
// .whenPressed(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry).until(() -> this.currentTurretMode.equals(TurretMode.MANUAL)))
.whenPressed(new CommandChooser(new RunClimberPath(m_robotClimber, m_robotClaws, new Point[] {new Point()}),
new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry),
() -> this.currentControlMode.equals(ControlMode.CLIMBER),
() -> this.currentControlMode.equals(ControlMode.SHOOTER)))
.whenReleased(new InstantCommand(() -> {
if (this.currentControlMode.equals(ControlMode.SHOOTER)) { this.currentTurretMode = TurretMode.MANUAL; }
src/main/java/frc4388/robot/commands/CommandChooser.java
+34 -7
View File
@@ -4,42 +4,69 @@
package frc4388.robot.commands;
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.Subsystem;
public class CommandChooser extends CommandBase {
private Command c1;
private Command c2;
private BooleanSupplier bs1;
private BooleanSupplier bs2;
private Boolean b1;
private Boolean b2;
private Command chosen;
/** Creates a new CommandChooser. */
public CommandChooser(Command c1, Command c2, BooleanSupplier bs1, BooleanSupplier bs2) {
// Use addRequirements() here to declare subsystem dependencies.
this.c1 = c1;
this.c2 = c2;
addRequirements(c1.getRequirements().toArray());
this.b1 = bs1.getAsBoolean();
this.b2 = bs2.getAsBoolean();
this.chosen = getChosen();
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() {}
public void initialize() {
this.chosen.initialize();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {}
public void execute() {
this.chosen.execute();
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}
public void end(boolean interrupted) {
this.chosen.end(interrupted);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
return this.chosen.isFinished();
}
}