Merge branch 'testRoboReveal' into Climber

src/main/java/frc4388/robot/commands/ButtonBoxCommands/RunMiddleSwitch.java

@@ -0,0 +1,61 @@
+// 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 edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class RunMiddleSwitch extends CommandBase {
+  
+  private static boolean manual = false;
+  private boolean newManual = false;
+  private boolean changes = false;
+
+  /** Creates a new RunMiddleSwitch. */
+  public RunMiddleSwitch() {
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // 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() {
+
+    changes = (newManual == manual);
+
+    if (manual) {
+      printManual();
+    } else {
+      printNormal();
+    }
+
+    newManual = manual;
+  }
+
+  public void printNormal(){
+    System.out.println("Normal");
+  }
+
+  public void printManual(){
+    System.out.println("Manual");
+  }
+
+  public static void setManual(boolean set)
+  {
+    manual = set;
+  }
+
+  // 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 changes;
+  }
+}

src/main/java/frc4388/robot/commands/ButtonBoxCommands/TurretManual.java

@@ -0,0 +1,62 @@
+// 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 edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.subsystems.Turret;
+
+public class TurretManual extends CommandBase {
+
+  // subsystems
+  private Turret turret;
+
+  // booleans
+  private static boolean manual = false;
+  private boolean newManual = false;
+  private boolean changes = false;
+
+  /** Creates a new TurretManual. */
+  public TurretManual(Turret turret) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    this.turret = turret;
+
+    addRequirements(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() {
+    changes = (newManual != manual);
+
+    if (manual) {
+      System.out.println("Manual Turret");  // TODO: turret manual controls
+    } else {
+      System.out.println("Auto Turret");    // TODO: turret auto controls;
+    }
+
+    newManual = manual;
+  }
+
+  public static void setManual(boolean set)
+  {
+    manual = set;
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    manual = !manual;
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return changes;
+  }
+}

src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/ExtenderIntakeGroup.java

@@ -0,0 +1,34 @@
+// 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.ExtenderIntakeCommands;
+
+import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
+import frc4388.robot.subsystems.Extender;
+import frc4388.robot.subsystems.Intake;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
+public class ExtenderIntakeGroup extends ParallelRaceGroup {
+  
+  public static int direction;
+
+  /** Creates a new RunExtenderAndIntake. */
+  public ExtenderIntakeGroup(Intake intake, Extender extender) {
+    // Add your commands in the addCommands() call, e.g.
+    // addCommands(new FooCommand(), new BarCommand());
+    
+    ExtenderIntakeGroup.direction = 1; // assume extender starts retracted completely
+    addCommands(new RunIntakeConditionally(intake), new RunExtender(extender));
+  }
+
+  public static void setDirectionToOut() {
+    ExtenderIntakeGroup.direction = 1;
+  }
+
+  public static void changeDirection() { // Never implemented?
+    ExtenderIntakeGroup.direction *= -1;
+  }
+}

src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/RunExtender.java

@@ -0,0 +1,66 @@
+// 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.ExtenderIntakeCommands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.Constants.ExtenderConstants;
+import frc4388.robot.subsystems.Extender;
+import frc4388.robot.subsystems.Intake;
+
+public class RunExtender extends CommandBase {
+
+  private Extender extender;
+
+  private double error;
+  private double tolerance;
+
+  /** Creates a new RunExtender. */
+  public RunExtender(Extender extender) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    this.extender = extender;
+    
+    updateError();
+    tolerance = 5.0;
+    
+    addRequirements(this.extender);
+  }
+  
+  public void updateError() {
+    if (ExtenderIntakeGroup.direction > 0) {
+      this.error = Math.abs(this.extender.getPosition() - ExtenderConstants.EXTENDER_FORWARD_LIMIT);
+    } else {
+      this.error = Math.abs(this.extender.getPosition() - ExtenderConstants.EXTENDER_REVERSE_LIMIT);
+    }
+  }
+  
+  // 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() {
+    System.out.println("RunExtender is working");
+    this.extender.runExtender(ExtenderIntakeGroup.direction * 1.0);
+    updateError();
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    ExtenderIntakeGroup.changeDirection();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    if (error < tolerance) {
+      System.out.println("RunExtender finished");
+      this.extender.runExtender(0.0);
+      return true;
+    }
+    return false;
+  }
+}

src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/RunIntakeConditionally.java

@@ -0,0 +1,47 @@
+// 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.ExtenderIntakeCommands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.Constants.IntakeConstants;
+import frc4388.robot.subsystems.Intake;
+
+public class RunIntakeConditionally extends CommandBase {
+
+  private Intake intake;
+
+  /** Creates a new RunIntakeConditionally. */
+  public RunIntakeConditionally(Intake intake) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    
+    this.intake = intake;
+
+    addRequirements(this.intake);
+  }
+
+  // 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() {
+    if (ExtenderIntakeGroup.direction > 0) {
+      this.intake.runAtOutput(-1);
+    } else {
+      this.intake.runAtOutput(0);
+    }
+  }
+
+  // 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/ShooterCommands/AimToCenter.java

@@ -2,7 +2,7 @@
 // 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;
+package frc4388.robot.commands.ShooterCommands;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.robot.Constants.ShooterConstants;
@@ -43,7 +43,7 @@ public class AimToCenter extends CommandBase {
   @Override
   public void execute() {
     m_targetAngle = angleToCenter(x, y, m_drive.getRegGyro().getDegrees());
-    m_turret.runshooterRotatePID(m_targetAngle);
+    m_turret.runShooterRotatePID(m_targetAngle);
 
     // Check if limelight is within range (comment out to disable vision odo)
     if (Math.abs(m_turret.getBoomBoomAngleDegrees() - m_targetAngle) < VisionConstants.RANGE){
@@ -61,21 +61,15 @@ public class AimToCenter extends CommandBase {
   }
 
   /**
-   * Checks if in hardware deadzone (due to mechanical limitations).
+   * Checks if in deadzone.
    * @param angle Angle to check.
-   * @return True if in hardware deadzone.
+   * @return True if in deadzone.
    */
-  public static boolean isHardwareDeadzone(double angle) {
-    return ((ShooterConstants.HARD_DEADZONE_LEFT > angle) || (angle > ShooterConstants.HARD_DEADZONE_RIGHT));
-  }
-
-  /**
-   * Checks if in digital deadzone (due to climber).
-   * @param angle Angle to check.
-   * @return True if in digital deadzone.
-   */
-  public static boolean isDigitalDeadzone(double angle) {
-    return ((ShooterConstants.DIG_DEADZONE_LEFT < angle) && (angle < ShooterConstants.DIG_DEADZONE_RIGHT));
+  public static boolean isDeadzone(double angle) {
+    if (angle == Double.NaN) {
+      return false;
+    }
+    return !((ShooterConstants.TURRET_REVERSE_SOFT_LIMIT <= angle) && (angle <= ShooterConstants.TURRET_FORWARD_SOFT_LIMIT));
   }
 
   // Called once the command ends or is interrupted.

src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java

@@ -2,14 +2,14 @@
 // 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;
+package frc4388.robot.commands.ShooterCommands;
 
-import edu.wpi.first.hal.simulation.SimulatorJNI;
-import edu.wpi.first.wpilibj.simulation.LinearSystemSim;
-import edu.wpi.first.wpilibj.simulation.SimHooks;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.robot.Constants.ShooterConstants;
+import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.robot.subsystems.BoomBoom;
 import frc4388.robot.subsystems.Hood;
 import frc4388.robot.subsystems.SwerveDrive;
@@ -35,21 +35,25 @@ public class Shoot extends CommandBase {
   public double m_targetVel;
   public double m_targetHood;
   public double m_targetAngle;
-  public double m_driveTargetAngle;
+  public Pose2d m_targetPoint;
 
   // pid
   public double error;
   public double prevError;
-  public Gains shootGains = ShooterConstants.SHOOT_GAINS;
+  public Gains gains = ShooterConstants.SHOOT_GAINS;
   public double kP, kI, kD;
   public double proportional, integral, derivative;
   public double time;
   public double output;
+  public double normOutput;
   public double tolerance;
   public boolean isAimedInTolerance;
+  public int inverted;
 
   // testing
-  public DummySensor dummy = new DummySensor(0);
+  public boolean simMode = true;
+  public DummySensor driveDummy;
+  public DummySensor turretDummy;
 
   /**
    * Creates a new shoot command, allowing the robot to aim and be ready to fire a ball
@@ -68,9 +72,9 @@ public class Shoot extends CommandBase {
     
     addRequirements(m_swerve, m_boomBoom, m_turret, m_hood);
 
-    kP = shootGains.kP;
-    kI = shootGains.kI;
-    kD = shootGains.kD;
+    kP = gains.kP;
+    kI = gains.kI;
+    kD = gains.kD;
 
     proportional = 0;
     integral = 0;
@@ -80,24 +84,37 @@ public class Shoot extends CommandBase {
     tolerance = 5.0;
     isAimedInTolerance = false;
 
-    DummySensor.resetAll();
+    if (simMode) {
+      driveDummy = new DummySensor(180);
+      turretDummy = new DummySensor(180);
+
+      DummySensor.resetAll();
+    }
   }
 
   /**
    * Updates error for custom PID.
    */
   public void updateError() {
-    error = (m_targetAngle - m_turret.getBoomBoomAngleDegrees() + 360) % 360;
+    m_targetPoint = SwerveDriveConstants.HUB_POSE;
+    m_targetAngle = AimToCenter.angleToCenter(m_odoX, m_odoY, driveDummy.get());
+    // m_targetAngle = AimToCenter.angleToCenter(m_odoX, m_odoY, m_swerve.getRegGyro().getDegrees());
+    error = (m_targetAngle - turretDummy.get() + 360) % 360;
+    // error = (m_targetAngle - m_turret.getBoomBoomAngleDegrees() + 360) % 360;
     isAimedInTolerance = (Math.abs(error) <= tolerance);
-    SmartDashboard.putBoolean("isAimed?", isAimedInTolerance);
+
+    if (simMode) {
+      SmartDashboard.putBoolean("isAimed?", isAimedInTolerance);
+      System.out.println("Target Angle: " + m_targetAngle);
+      System.out.println("Error: " + error);
+    }
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    m_odoX = m_swerve.getOdometry().getX();
-    m_odoY = m_swerve.getOdometry().getY();
-    m_distance = Math.sqrt(Math.pow(m_odoX, 2) + Math.pow(m_odoY, 2));
+    m_odoX = 0;//m_swerve.getOdometry().getX();
+    m_odoY = -1;//m_swerve.getOdometry().getY();
 
     m_gyroAngle = m_swerve.getRegGyro().getDegrees();
 
@@ -105,22 +122,10 @@ public class Shoot extends CommandBase {
     m_targetVel = m_boomBoom.getVelocity(m_distance);
     m_targetHood = m_boomBoom.getHood(m_distance);
 
-    // target angle tests
-    m_gyroAngle = 0;
-    m_odoX = -1;
-    m_odoY = 1;
-
     m_targetAngle = ((Math.atan2(m_odoY, m_odoX) * (180./Math.PI) - m_gyroAngle) + 180. + 360.) % 360.;
 
     // deadzone processing
-    if (AimToCenter.isHardwareDeadzone(m_targetAngle)) {
-      m_targetAngle = m_targetAngle + 20;
-    }
-
-    if (AimToCenter.isDigitalDeadzone(m_targetAngle)) {
-      // this should rotate the entire swerve drive by 20 degrees, so shoot can now proceed like normal. idk if this will work
-      m_swerve.driveWithInput(0, 0, Math.cos(m_gyroAngle + 20), Math.sin(m_gyroAngle + 20), true);
-    }
+    if (AimToCenter.isDeadzone(m_targetAngle)) {}
     
     // initial error
     updateError();
@@ -131,6 +136,13 @@ public class Shoot extends CommandBase {
    * Run custom PID.
    */
   public void runPID() {
+    if (error > 180){
+      error = 360 - error;
+      inverted = -1;
+    }
+    else{
+      inverted = 1;
+    }
     prevError = error;
     updateError();
     
@@ -138,20 +150,34 @@ public class Shoot extends CommandBase {
     integral = integral + error * time;
     derivative = (error - prevError) / time;
     output = kP * proportional + kI * integral + kD * derivative;
+    normOutput = output/360 * inverted;
   }
-  
+
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-
-    // custom pid
+    if (simMode) {
+      System.out.println("Normalized Output: " + normOutput);
+    }
+      // custom pid
     runPID();
-    // m_swerve.driveWithInput(0, 0, output, true); // i have no idea if this is how you rotate the
+
+    if (simMode) {
+      driveDummy.apply(normOutput);
+      System.out.println("Drive Dummy: " + driveDummy.get());
+    }
+    m_swerve.driveWithInput(0, 0, normOutput, true); // i have no idea if this is how you rotate the
                                                  // entire swerve drive or its the line below
-    m_swerve.driveWithInput(0, 0, Math.cos(output), Math.sin(output), true);
+    // m_swerve.driveWithInput(0, 0, Math.cos(output), Math.sin(output), true);
     
     m_hood.runAngleAdjustPID(m_targetHood);
     m_boomBoom.runDrumShooterVelocityPID(m_targetVel);
+
+    if (simMode) {
+      turretDummy.apply(normOutput);
+      System.out.println("Turret Dummy: " + turretDummy.get());
+    }
+    m_turret.m_boomBoomRotateMotor.set(normOutput);
   }
 
   // Called once the command ends or is interrupted.
@@ -161,6 +187,9 @@ public class Shoot extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return false;
+    // if (simMode) {
+    return isAimedInTolerance;
+    // }
+    // return false;
   }
 }

src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java

@@ -0,0 +1,168 @@
+// 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.ShooterCommands;
+
+import java.util.ArrayList;
+
+import org.opencv.core.Point;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+import frc4388.robot.Constants.VisionConstants;
+import frc4388.robot.subsystems.BoomBoom;
+import frc4388.robot.subsystems.Hood;
+import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.robot.subsystems.Turret;
+import frc4388.robot.subsystems.Vision;
+import frc4388.robot.subsystems.VisionOdometry;
+import frc4388.utility.Vector2D;
+import frc4388.utility.desmos.DesmosServer;
+
+public class TrackTarget extends CommandBase {
+  /** Creates a new TrackTarget. */
+  Turret m_turret;
+  VisionOdometry m_visionOdometry;
+  BoomBoom m_boomBoom;
+  Hood m_hood;
+
+  // use odometry to find x and y later
+  double x;
+  double y;
+  double distance;
+  double vel;
+  double hood;
+  double average;
+  double output;
+  Pose2d pos = new Pose2d();
+  ArrayList<Point> points = new ArrayList<>();
+
+  private boolean targetLocked = false;
+  private double velocityTolerance = 100.0;
+  private double hoodTolerance = 5.0;
+
+  double m=0;
+  double b=0;
+  boolean isExecuted = false;
+
+  // public static Gains m_aimGains;
+
+  public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    m_turret = turret;
+    m_boomBoom = boomBoom;
+    m_hood = hood;
+    m_visionOdometry = visionOdometry;
+
+    addRequirements(m_turret, m_boomBoom, m_hood, m_visionOdometry);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    x = 0;
+    y = 0;
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    //m_targetAngle = angleToCenter(x, y, m_drive.getRegGyro().getDegrees());
+    SmartDashboard.putBoolean("Target Locked", this.targetLocked);
+    
+    try {
+      m_visionOdometry.setLEDs(true);
+      points = m_visionOdometry.getTargetPoints();
+      
+      Point average = VisionOdometry.averagePoint(points);
+      
+      for(Point point : points) {
+        Vector2D difference = new Vector2D(point);
+        difference.subtract(new Vector2D(average));
+
+        if(difference.magnitude() < VisionConstants.POINTS_THRESHOLD)
+        points.remove(point);
+      }
+      
+      average = VisionOdometry.averagePoint(points);
+      DesmosServer.putPoint("average", average);
+      
+      for(int i = 0; i < points.size(); i++) {
+        DesmosServer.putPoint("Point" + i, points.get(i));
+      }
+      
+      output = (average.x - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS;
+      output *= 4;
+      // output *= 0.5;
+      DesmosServer.putDouble("output", output);
+      m_turret.runTurretWithInput(output);
+      
+      double y_rot = average.y / VisionConstants.LIME_VIXELS;
+      y_rot *= Math.toRadians(VisionConstants.V_FOV);
+      y_rot -= Math.toRadians(VisionConstants.V_FOV) / 2;
+      y_rot += Math.toRadians(VisionConstants.LIME_ANGLE);
+      
+      double distance = (VisionConstants.TARGET_HEIGHT - VisionConstants.LIME_HEIGHT) / Math.tan(y_rot);
+      DesmosServer.putDouble("distance", distance);
+      
+      updateRegressionDesmos();
+      double regressedDistance = distanceRegression(distance);
+      regressedDistance += VisionConstants.EDGE_TO_CENTER + VisionConstants.LIMELIGHT_RADIUS;
+      DesmosServer.putDouble("distanceReg", regressedDistance);
+      
+      //Vision odometry circle fit based pose estimate
+      // Point targetOffset = m_visionOdometry.getTargetOffset();
+      // DesmosServer.putPoint("targetOff", targetOffset);
+      
+      vel = m_boomBoom.getVelocity(regressedDistance + 30);
+      hood = m_boomBoom.getHood(regressedDistance + 30);
+      // m_boomBoom.runDrumShooter(vel);
+      m_boomBoom.runDrumShooterVelocityPID(vel);
+      m_hood.runAngleAdjustPID(hood);
+      
+      double currentDrumVel = this.m_boomBoom.m_shooterFalconLeft.getSelectedSensorVelocity();
+      double currentHood = this.m_hood.getEncoderPosition();
+  
+      this.targetLocked = (Math.abs(currentDrumVel - vel) < velocityTolerance) && (Math.abs(currentHood - hood) < hoodTolerance);
+
+      SmartDashboard.putNumber("Regressed Distance", regressedDistance);
+      SmartDashboard.putNumber("Distance", distance);
+      SmartDashboard.putNumber("Hood Target Angle Track", hood);
+      SmartDashboard.putNumber("Vel Target Track", vel);
+
+      // isExecuted = true;
+    }
+    catch (Exception e){
+      e.printStackTrace();
+      // System.err.println("Exception: " + e.toString() + ", Line 78 at TrackTarget.java");
+    }
+    // m_turret.runshooterRotatePID(m_targetAngle);
+  }
+
+  public final double distanceRegression(double distance) {
+    return (79.6078 * Math.pow(1.01343, distance) - 56.6671);
+  }
+
+  public void updateRegressionDesmos() {
+    m = DesmosServer.readDouble("m");
+    b = DesmosServer.readDouble("b");
+
+    DesmosServer.putArray("MB", m, b);
+  }
+
+  // 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;
+    // return isExecuted && Math.abs(output) < .1;
+  }
+}

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(); (accompanied by initialize and onFinished)
+      
+      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

@@ -0,0 +1,72 @@
+// 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.wpilibj2.command.CommandBase;
+import frc4388.robot.Constants.ShooterConstants;
+import frc4388.robot.subsystems.BoomBoom;
+import frc4388.robot.subsystems.Storage;
+import frc4388.robot.subsystems.Turret;
+
+public class SpitOutWrongColor extends CommandBase {
+
+  // subsystems
+  private Storage storage;
+  private BoomBoom drum;
+  private Turret turret;
+
+  // time (in milliseconds)
+  private long initialTime;
+  private long elapsedTime;
+  private long threshold;
+
+  private double initialTurret;
+  private int spitVelocity;
+
+  /** Creates a new SpitOutWrongColor. */
+  public SpitOutWrongColor(Storage storage, BoomBoom drum, Turret turret) {
+    // Use addRequirements() here to declare subsystem dependencies.
+
+    this.storage = storage;
+    this.drum = drum;
+    this.turret = turret;
+
+    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;
+  }
+  
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    elapsedTime = System.currentTimeMillis() - initialTime;
+
+    this.storage.runStorage(0.9);
+    this.turret.gotoMidpoint();
+    this.drum.runDrumShooterVelocityPID(spitVelocity);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    this.storage.runStorage(0.0);
+    this.turret.runShooterRotatePID(initialTurret * ShooterConstants.TURRET_DEGREES_PER_ROT);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return (elapsedTime >= threshold);
+  }
+}

src/main/java/frc4388/robot/commands/TrackTarget.java

@@ -1,121 +0,0 @@
-// 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.io.IOException;
-import java.util.ArrayList;
-import java.util.List;
-
-import org.opencv.core.Point;
-
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc4388.robot.Constants.ShooterConstants;
-import frc4388.robot.Constants.VisionConstants;
-import frc4388.robot.subsystems.BoomBoom;
-import frc4388.robot.subsystems.Hood;
-import frc4388.robot.subsystems.SwerveDrive;
-import frc4388.robot.subsystems.Turret;
-import frc4388.robot.subsystems.VisionOdometry;
-
-public class TrackTarget extends CommandBase {
-  /** Creates a new TrackTarget. */
-  Turret m_turret;
-  SwerveDrive m_drive;
-  VisionOdometry m_visionOdometry;
-  BoomBoom m_boomBoom;
-  Hood m_hood;
-
-  // use odometry to find x and y later
-  double x;
-  double y;
-  double distance;
-  double vel;
-  double hood;
-  double average;
-  double output;
-  Pose2d pos = new Pose2d();
-  ArrayList<Point> points = new ArrayList<>();
-
-  // public static Gains m_aimGains;
-
-  public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, SwerveDrive drive, VisionOdometry visionOdometry) {
-    // Use addRequirements() here to declare subsystem dependencies.
-    m_turret = turret;
-    m_drive = drive;
-    m_boomBoom = boomBoom;
-    m_hood = hood;
-    m_visionOdometry = visionOdometry;
-    addRequirements(m_turret, m_drive, m_visionOdometry);
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {
-    x = 0;
-    y = 0;
-  }
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-    //m_targetAngle = angleToCenter(x, y, m_drive.getRegGyro().getDegrees());
-    m_visionOdometry.setLEDs(true);
-    points = m_visionOdometry.getTargetPoints();
-    double pointTotal = 0;
-    for(Point point : points)
-    {
-      pointTotal = pointTotal + point.x;
-    }
-    average = pointTotal/points.size();
-    output = average/VisionConstants.LIME_HIXELS * VisionConstants.TURRET_kP;
-    m_turret.runTurretWithInput(output);
-    try{
-      pos = m_visionOdometry.getVisionOdometry();
-      distance = Math.hypot(pos.getX(), pos.getY());
-    }
-    catch (Exception e){
-    }
-    vel = m_boomBoom.getVelocity(distance);
-    hood = m_boomBoom.getHood(distance);
-    m_boomBoom.runDrumShooterVelocityPID(vel);
-    m_hood.runAngleAdjustPID(hood);
-    //m_turret.runshooterRotatePID(m_targetAngle);
-  }
-
- /* public static double angleToCenter(double x, double y, double gyro) {
-    double angle = ((Math.atan2(y, x) * (180./Math.PI) - gyro) + 180. + 360.) % 360.; // Finds the angle between the gyro of the robot and the target (positive x is gyro 0)
-    return angle;
-  }*/
-
-  /**
-   * Checks if in hardware deadzone (due to mechanical limitations).
-   * @param angle Angle to check.
-   * @return True if in hardware deadzone.
-   */
-  public static boolean isHardwareDeadzone(double angle) {
-    return ((ShooterConstants.HARD_DEADZONE_LEFT > angle) || (angle > ShooterConstants.HARD_DEADZONE_RIGHT));
-  }
-
-  /**
-   * Checks if in digital deadzone (due to climber).
-   * @param angle Angle to check.
-   * @return True if in digital deadzone.
-   */
-  public static boolean isDigitalDeadzone(double angle) {
-    return ((ShooterConstants.DIG_DEADZONE_LEFT < angle) && (angle < ShooterConstants.DIG_DEADZONE_RIGHT));
-  }
-
-  // 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;
-  }
-}