manage storage + alliance (needs testing)

src/main/java/frc4388/robot/commands/StorageCommands/ManageStorage.java

@@ -0,0 +1,76 @@
+// 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.StorageCommands;
+
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.ScheduleCommand;
+import frc4388.robot.Robot;
+import frc4388.robot.subsystems.BoomBoom;
+import frc4388.robot.subsystems.Storage;
+import frc4388.robot.subsystems.Turret;
+
+public class ManageStorage extends CommandBase {
+
+  // subsystems
+  private Storage storage;
+  private BoomBoom drum;
+  private Turret turret;
+
+  private Alliance alliance;
+  private boolean rightColor;
+
+  /** Creates a new ManageStorage. */
+  public ManageStorage(Storage storage, BoomBoom drum, Turret turret) {
+    // Use addRequirements() here to declare subsystem dependencies.
+
+    this.storage = storage;
+    this.drum = drum;
+    this.turret = turret;
+
+    rightColor = true;
+
+    addRequirements(this.storage, this.drum, this.turret);
+  }
+
+  // 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() {
+    checkColor();
+    
+    if (rightColor) {
+      this.storage.manageStorage();
+    } else {
+
+      // * CommandScheduler.getInstance().schedule(new ExampleCommand());
+      // * new ExampleCommand().schedule();
+      // * new ExampleCommand().execute();
+      
+      new SpitOutWrongColor(this.storage, this.drum, this.turret); // ? is this how you run a command inside a command
+    }
+  }
+
+  private void checkColor() {
+    this.alliance = this.storage.getColor();
+    rightColor = this.alliance.equals(Robot.alliance);
+  }
+
+  // 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/StorageCommands/SpitOutWrongColor.java

@@ -2,21 +2,20 @@
 // 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.ShooterCommands;
+package frc4388.robot.commands.StorageCommands;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.robot.Constants.ShooterConstants;
 import frc4388.robot.subsystems.BoomBoom;
 import frc4388.robot.subsystems.Storage;
 import frc4388.robot.subsystems.Turret;
-import frc4388.utility.RobotTime;
 
 public class SpitOutWrongColor extends CommandBase {
 
   // subsystems
+  private Storage storage;
   private BoomBoom drum;
   private Turret turret;
-  private Storage storage;
 
   // time (in milliseconds)
   private long initialTime;
@@ -27,27 +26,27 @@ public class SpitOutWrongColor extends CommandBase {
   private int spitVelocity;
 
   /** Creates a new SpitOutWrongColor. */
-  public SpitOutWrongColor(BoomBoom drum, Turret turret, Storage storage) {
+  public SpitOutWrongColor(Storage storage, BoomBoom drum, Turret turret) {
     // Use addRequirements() here to declare subsystem dependencies.
 
+    this.storage = storage;
     this.drum = drum;
     this.turret = turret;
-    this.storage = storage;
 
+    addRequirements(this.storage, this.drum, this.turret);
+  }
+  
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
     initialTime = System.currentTimeMillis();
     elapsedTime = 0;
     threshold = 2000;
 
     initialTurret = this.turret.getEncoderPosition();
     spitVelocity = 2000;
-
-    addRequirements(this.drum, this.turret, this.storage);
   }
-
-  // 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() {