diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 1d3cc08..93eb66e 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -69,6 +69,7 @@ import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.StorageConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.commands.ButtonBoxCommands.RunMiddleSwitch; +import frc4388.robot.commands.ButtonBoxCommands.RunTurretOrClimberAuto; import frc4388.robot.commands.ClimberCommands.RunClaw; import frc4388.robot.commands.ClimberCommands.RunClimberPath; // import frc4388.robot.commands.ButtonBoxCommands.TurretManual; @@ -353,22 +354,32 @@ public class RobotContainer { .whenReleased(new InstantCommand(() -> this.currentControlMode = ControlMode.SHOOTER)); // .whenReleased(EnableClimber())); - // control turret manual mode - // new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) - // .whileHeld(new RunCommand(() -> TurretManual.setManual(true))) - // .whenReleased(new RunCommand(() -> TurretManual.setManual(false))); + new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) - // new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) - // .whenPressed(new InstantCommand(() -> { - // this.currentClimberMode = ClimberMode.AUTONOMOUS; - // })) - // .whenPressed(new RunClimberPath(m_robotClimber, m_robotClaws, new Point[] {new Point()}).until(() -> this.currentClimberMode.equals(ClimberMode.MANUAL))) - // .whenReleased(new InstantCommand(() -> this.currentClimberMode = ClimberMode.MANUAL)); + .whenPressed(new InstantCommand(() -> { + if (this.currentControlMode.equals(ControlMode.SHOOTER)) { this.currentTurretMode = TurretMode.AUTONOMOUS; } + if (this.currentControlMode.equals(ControlMode.CLIMBER)) { this.currentClimberMode = ClimberMode.AUTONOMOUS; } + })) - // new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) - // .whenPressed(new InstantCommand(() -> this.currentTurretMode = TurretMode.AUTONOMOUS)) - // .whenPressed(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry).until(() -> this.currentTurretMode.equals(TurretMode.MANUAL))) - // .whenReleased(new InstantCommand(() -> this.currentTurretMode = TurretMode.MANUAL)); + // * 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))) + + // * external RunTurretOrClimberAutos command, which runs either one (conditionally) based on currentControlMode + .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))) + + .whenReleased(new InstantCommand(() -> { + if (this.currentControlMode.equals(ControlMode.SHOOTER)) { this.currentTurretMode = TurretMode.MANUAL; } + if (this.currentControlMode.equals(ControlMode.CLIMBER)) { this.currentClimberMode = ClimberMode.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/ButtonBoxCommands/RunTurretOrClimberAuto.java b/src/main/java/frc4388/robot/commands/ButtonBoxCommands/RunTurretOrClimberAuto.java new file mode 100644 index 0000000..87be8e6 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/ButtonBoxCommands/RunTurretOrClimberAuto.java @@ -0,0 +1,65 @@ +// 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.ButtonBoxCommands; + +import org.opencv.core.Point; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.RobotContainer; +import frc4388.robot.commands.ClimberCommands.RunClimberPath; +import frc4388.robot.commands.ShooterCommands.AimToCenter; +import frc4388.robot.subsystems.Claws; +import frc4388.robot.subsystems.Climber; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.Turret; +import frc4388.robot.subsystems.VisionOdometry; + +public class RunTurretOrClimberAuto extends CommandBase { + + private Turret turret; + private SwerveDrive swerveDrive; + private VisionOdometry visionOdometry; + private Climber climber; + private Claws claws; + + /** Creates a new RunTurretOrClimberAuto. */ + public RunTurretOrClimberAuto(Turret turret, SwerveDrive swerveDrive, VisionOdometry visionOdometry, Climber climber, Claws claws) { + // Use addRequirements() here to declare subsystem dependencies. + + this.turret = turret; + this.swerveDrive = swerveDrive; + this.visionOdometry = visionOdometry; + this.climber = climber; + this.claws = claws; + + addRequirements(this.turret, this.climber); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + // check control mode here. if shooter control mode, run turret auto command. if climber control mode, run climber auto command. + if (RobotContainer.currentControlMode.equals(RobotContainer.ControlMode.SHOOTER)) { + new AimToCenter(this.turret, this.swerveDrive, this.visionOdometry); + } + if (RobotContainer.currentControlMode.equals(RobotContainer.ControlMode.CLIMBER)) { + new RunClimberPath(this.climber, this.claws, new Point[] {new Point()}); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc4388/robot/commands/CommandChooser.java b/src/main/java/frc4388/robot/commands/CommandChooser.java new file mode 100644 index 0000000..f6c5fa0 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/CommandChooser.java @@ -0,0 +1,45 @@ +// 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.function.BooleanSupplier; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class CommandChooser extends CommandBase { + + private Command c1; + private Command c2; + + private BooleanSupplier bs1; + private BooleanSupplier bs2; + + /** Creates a new CommandChooser. */ + public CommandChooser(Command c1, Command c2, BooleanSupplier bs1, BooleanSupplier bs2) { + // Use addRequirements() here to declare subsystem dependencies. + + + addRequirements(c1.getRequirements().toArray()); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +}