From 80f0f3c76d2e0cf5ef77c28bfc32b9683cd67394 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sun, 20 Mar 2022 21:32:35 -0600 Subject: [PATCH] basic is back baby --- .../java/frc4388/robot/RobotContainer.java | 28 ++++---- .../robot/commands/BasicCommandChooser.java | 68 +++++++++++++++++++ ...hooser.java => ComplexCommandChooser.java} | 4 +- 3 files changed, 84 insertions(+), 16 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/BasicCommandChooser.java rename src/main/java/frc4388/robot/commands/{CommandChooser.java => ComplexCommandChooser.java} (94%) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index db5d677..4acc96a 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -71,7 +71,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.ComplexCommandChooser; import frc4388.robot.commands.ButtonBoxCommands.RunMiddleSwitch; import frc4388.robot.commands.ButtonBoxCommands.RunTurretOrClimberAuto; import frc4388.robot.commands.ClimberCommands.RunClaw; @@ -362,14 +362,14 @@ public class RobotContainer { // toggle manual mode and autonomous mode based on the current control mode new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) - .whenPressed(new InstantCommand(() -> { - if (this.currentControlMode.equals(SubsystemMode.CLIMBER)) { m_robotTurret.gotoZero(); } - }, m_robotTurret)) + // .whenPressed(new InstantCommand(() -> { + // if (this.currentControlMode.equals(SubsystemMode.CLIMBER)) { m_robotTurret.gotoZero(); } + // }, m_robotTurret)) - .whenPressed(new InstantCommand(() -> { - if (this.currentControlMode.equals(SubsystemMode.SHOOTER)) { this.currentTurretMode = ControlMode.MANUAL; } - if (this.currentControlMode.equals(SubsystemMode.CLIMBER)) { this.currentClimberMode = ControlMode.AUTONOMOUS; } - })) + // .whenPressed(new InstantCommand(() -> { + // if (this.currentControlMode.equals(SubsystemMode.SHOOTER)) { this.currentTurretMode = ControlMode.MANUAL; } + // if (this.currentControlMode.equals(SubsystemMode.CLIMBER)) { this.currentClimberMode = ControlMode.AUTONOMOUS; } + // })) // * custom Command inside InstantCommand // .whenPressed(new InstantCommand(() -> { @@ -382,19 +382,19 @@ public class RobotContainer { // .whenPressed(new RunTurretOrClimberAuto(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry, m_robotClimber, m_robotClaws)) // * CommandChooser with BooleanSuppliers - .whenPressed(new CommandChooser(new HashMap() {{ + .whenPressed(new ComplexCommandChooser(new HashMap() {{ 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; } - if (this.currentControlMode.equals(SubsystemMode.CLIMBER)) { this.currentClimberMode = ControlMode.MANUAL; } - })); + // .whenReleased(new InstantCommand(() -> { + // if (this.currentControlMode.equals(SubsystemMode.SHOOTER)) { this.currentTurretMode = ControlMode.AUTONOMOUS; } + // if (this.currentControlMode.equals(SubsystemMode.CLIMBER)) { this.currentClimberMode = ControlMode.MANUAL; } + // })); new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftButton.value) .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) diff --git a/src/main/java/frc4388/robot/commands/BasicCommandChooser.java b/src/main/java/frc4388/robot/commands/BasicCommandChooser.java new file mode 100644 index 0000000..e1532fe --- /dev/null +++ b/src/main/java/frc4388/robot/commands/BasicCommandChooser.java @@ -0,0 +1,68 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. +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 BasicCommandChooser extends CommandBase { + + private Command c1; + private Command c2; + private Boolean b1; + private Boolean b2; + + private Command theChosenOne; + + /** Creates a new CommandChooser. */ + public BasicCommandChooser(Command c1, Command c2, BooleanSupplier bs1, BooleanSupplier bs2) { + // Use addRequirements() here to declare subsystem dependencies. + this.c1 = c1; + this.c2 = c2; + this.b1 = bs1.getAsBoolean(); + this.b2 = bs2.getAsBoolean(); + + Set allReqs = c1.getRequirements(); + allReqs.addAll(c2.getRequirements()); + + addRequirements((Subsystem[]) allReqs.toArray()); + } + + public Command getTheChosenOne() { + if (this.b1) { + return this.c1; + } else { + return this.c2; + } + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + this.theChosenOne = getTheChosenOne(); + this.theChosenOne.initialize(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + this.theChosenOne.execute(); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + this.theChosenOne.end(interrupted); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return this.theChosenOne.isFinished(); + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/CommandChooser.java b/src/main/java/frc4388/robot/commands/ComplexCommandChooser.java similarity index 94% rename from src/main/java/frc4388/robot/commands/CommandChooser.java rename to src/main/java/frc4388/robot/commands/ComplexCommandChooser.java index f6376fb..e9c5443 100644 --- a/src/main/java/frc4388/robot/commands/CommandChooser.java +++ b/src/main/java/frc4388/robot/commands/ComplexCommandChooser.java @@ -14,7 +14,7 @@ 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 { +public class ComplexCommandChooser extends CommandBase { private HashMap commandMap; @@ -24,7 +24,7 @@ public class CommandChooser extends CommandBase { * @author Aarav Shah * @author Daniel Thomas McGrath */ - public CommandChooser(HashMap commandMap) { + public ComplexCommandChooser(HashMap commandMap) { this.commandMap = commandMap; Set allReqs = Collections.emptySet();