broken

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)
-    // 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)
+      .whenPressed(new InstantCommand(() -> {
-    //   .whenPressed(new InstantCommand(() -> {
+        if (this.currentControlMode.equals(ControlMode.SHOOTER)) { this.currentTurretMode = TurretMode.AUTONOMOUS; } 
-    //     this.currentClimberMode = ClimberMode.AUTONOMOUS;
+        if (this.currentControlMode.equals(ControlMode.CLIMBER)) { 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));
 
-    // new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value)
+      // * custom Command inside InstantCommand
-    //   .whenPressed(new InstantCommand(() -> this.currentTurretMode = TurretMode.AUTONOMOUS))
+      .whenPressed(new InstantCommand(() -> {
-    //   .whenPressed(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry).until(() -> this.currentTurretMode.equals(TurretMode.MANUAL)))
+        if (this.currentControlMode.equals(ControlMode.SHOOTER)) { new RunClimberPath(m_robotClimber, m_robotClaws, new Point[] {new Point()}); }
-    //   .whenReleased(new InstantCommand(() -> this.currentTurretMode = TurretMode.MANUAL));
+        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))

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;
+  }
+}

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;
+  }
+}