From f890dda9f78f400869435dccf330d4d053785730 Mon Sep 17 00:00:00 2001
From: aarav18 <aaravshah18@gmail.com>
Date: Fri, 18 Mar 2022 19:04:43 -0600
Subject: [PATCH] probably decided to go with the CommandChooser solution, this
 shoudl work

---
 .../java/frc4388/robot/RobotContainer.java    | 24 ++++++-----
 .../robot/commands/CommandChooser.java        | 41 +++++++++++++++----
 2 files changed, 47 insertions(+), 18 deletions(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 93eb66e..9cddcbf 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -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; }
diff --git a/src/main/java/frc4388/robot/commands/CommandChooser.java b/src/main/java/frc4388/robot/commands/CommandChooser.java
index f6c5fa0..ff984cc 100644
--- a/src/main/java/frc4388/robot/commands/CommandChooser.java
+++ b/src/main/java/frc4388/robot/commands/CommandChooser.java
@@ -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();
   }
 }