Merge branch 'master' into auto-programming

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

@@ -5,16 +5,18 @@
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
-package frc4388.robot.commands.storage;
+package frc4388.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
-public class StorageIntakeFinal extends CommandBase {
+public class InterruptSubystem extends CommandBase {
   /**
-   * Creates a new StorageIntakeFinal.
+   * Creates a new InterruptSubystem.
    */
-  public StorageIntakeFinal() {
+  public InterruptSubystem(SubsystemBase subsystem) {
     // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(subsystem);
   }
 
   // Called when the command is initially scheduled.
@@ -35,6 +37,6 @@ public class StorageIntakeFinal extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return false;
+    return true;
   }
 }

src/main/java/frc4388/robot/commands/shooter/PrepChecker.java

@@ -1,75 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package frc4388.robot.commands.shooter;
-
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc4388.robot.subsystems.Shooter;
-import frc4388.robot.subsystems.ShooterAim;
-import frc4388.robot.subsystems.ShooterHood;
-import frc4388.robot.subsystems.Storage;
-
-public class PrepChecker extends CommandBase {
-  Shooter m_shooter;
-  ShooterAim m_shooterAim;
-  ShooterHood m_shooterHood;
-  Storage m_storage;
-
-  boolean m_isDrumReady = false;
-  boolean m_isAimReady = false;
-  boolean m_isHoodReady = false;
-  boolean m_isStorageReady = false;
-  
-  /**
-   * Creates a new PrepChecker.
-   * @param shooter used to read all shooter subsystems. Not used as a requirement so don't expect it to interrupt other commands.
-   * @param storage reads storage in a similar way to shooter. Not used as a requirement.
-   */
-  public PrepChecker(Shooter shooter, Storage storage) {
-    m_shooter = shooter;
-    m_shooterAim = m_shooter.m_shooterAimSubsystem;
-    m_shooterHood = m_shooter.m_shooterHoodSubsystem;
-    m_storage = storage;
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {
-    m_isDrumReady = false;
-    m_isAimReady = false;
-    m_isHoodReady = false;
-    m_isStorageReady = false;
-  }
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-    m_isDrumReady = m_shooter.m_isDrumReady;              SmartDashboard.putBoolean("ShooterVelocityPID Finished", m_isDrumReady);
-    m_isAimReady = m_shooterAim.m_isAimReady;             SmartDashboard.putBoolean("TrackTarget Finished", m_isAimReady);
-    m_isHoodReady = m_shooterHood.m_isHoodReady;          SmartDashboard.putBoolean("HoodPosition Finished", m_isHoodReady);
-    m_isStorageReady = m_storage.m_isStorageReadyToFire;  SmartDashboard.putBoolean("StoragePrepAim Finished", m_isStorageReady);
-  }
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {
-    m_shooter.m_isDrumReady = false;
-    m_shooterAim.m_isAimReady = false;
-    m_shooterHood.m_isHoodReady = false;
-    m_storage.m_isStorageReadyToFire = false;
-  }
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    if (m_isDrumReady && m_isAimReady && m_isHoodReady && m_isStorageReady) {
-      return true;
-    }
-    return false;
-  }
-}

src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java

@@ -1,36 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package frc4388.robot.commands.shooter;
-
-import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
-import edu.wpi.first.wpilibj2.command.RunCommand;
-import frc4388.robot.commands.storage.StorageRun;
-import frc4388.robot.subsystems.Shooter;
-import frc4388.robot.subsystems.ShooterAim;
-import frc4388.robot.subsystems.ShooterHood;
-import frc4388.robot.subsystems.Storage;
-
-// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
-// information, see:
-// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
-public class ShootFireGroup extends ParallelRaceGroup {
-  /**
-   * Fires the shooter
-   * @param m_shooter The Shooter subsytem
-   * @param m_shooterAim The ShooterAim subsystem
-   * @param m_storage The Storage subsytem
-   */
-  public ShootFireGroup(Shooter m_shooter, ShooterAim m_shooterAim, ShooterHood m_shooterHood, Storage m_storage) {
-    addCommands(
-      new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel()), m_shooter),
-      new RunCommand(() -> m_shooterHood.runAngleAdjustPID(m_shooterHood.addFireAngle()), m_shooterHood),
-      new TrackTarget(m_shooterAim)
-      //new StorageRun(m_storage)
-    );
-  }
-}

src/main/java/frc4388/robot/commands/shooter/ShootFullGroup.java

@@ -1,32 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package frc4388.robot.commands.shooter;
-
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import frc4388.robot.subsystems.Shooter;
-import frc4388.robot.subsystems.ShooterAim;
-import frc4388.robot.subsystems.ShooterHood;
-import frc4388.robot.subsystems.Storage;
-
-// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
-// information, see:
-// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
-public class ShootFullGroup extends SequentialCommandGroup {
-  /**
-   * Preps and Fires the Shooter
-   * @param m_shooter The Shooter subsytem
-   * @param m_shooterAim The ShooterAim subsystem
-   * @param m_storage The Storage subsytem
-   */
-  public ShootFullGroup(Shooter m_shooter, ShooterAim m_shooterAim, ShooterHood m_shooterHood, Storage m_storage) {
-    addCommands(
-      new ShootPrepGroup(m_shooter, m_shooterAim, m_shooterHood, m_storage), 
-      new ShootFireGroup(m_shooter, m_shooterAim, m_shooterHood, m_storage)
-    );
-  }
-}

src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java

@@ -8,7 +8,7 @@
 package frc4388.robot.commands.shooter;
 
 import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
-import frc4388.robot.commands.storage.StoragePrepAim;
+import frc4388.robot.commands.storage.StoragePrep;
 import frc4388.robot.subsystems.Shooter;
 import frc4388.robot.subsystems.ShooterAim;
 import frc4388.robot.subsystems.ShooterHood;
@@ -26,11 +26,9 @@ public class ShootPrepGroup extends ParallelDeadlineGroup {
    */
   public ShootPrepGroup(Shooter m_shooter, ShooterAim m_shooterAim, ShooterHood m_shooterHood, Storage m_storage) { 
     super(
-      new PrepChecker(m_shooter, m_storage),
       new TrackTarget(m_shooterAim),
       new ShooterVelocityControlPID(m_shooter),
-      new HoodPositionPID(m_shooterHood),
-      new StoragePrepAim(m_storage)
+      new HoodPositionPID(m_shooterHood)
     );
   }
 }

src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java

@@ -34,10 +34,7 @@ public class ShooterVelocityControlPID extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    m_shooter.runDrumShooterVelocityPID(/*m_shooter.addFireVel()*/13000);
-    //m_shooterHood.runAngleAdjustPID(m_shooterHood.addFireAngle());
-    //SmartDashboard.putNumber("Fire Velocity", m_shooter.addFireVel());
-    //SmartDashboard.putNumber("Fire Angle", m_shooter.addFireAngle());
+    m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel());
   }
 
   // Called once the command ends or is interrupted.

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

@@ -12,6 +12,7 @@ import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.robot.Constants.VisionConstants;
+import frc4388.robot.subsystems.LimeLight;
 import frc4388.robot.subsystems.Shooter;
 import frc4388.robot.subsystems.ShooterAim;
 import frc4388.robot.subsystems.ShooterHood;
@@ -31,6 +32,7 @@ public class TrackTarget extends CommandBase {
   double yAngle = 0;
   double target = 0;
   public double distance;
+  public double realDistance;
   public static double fireVel;
   public static double fireAngle;
 
@@ -48,6 +50,7 @@ public class TrackTarget extends CommandBase {
     m_shooterHood = m_shooter.m_shooterHoodSubsystem;
     addRequirements(m_shooterAim);
     NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
+    NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0);
   }
 
   // Called when the command is initially scheduled.
@@ -65,8 +68,14 @@ public class TrackTarget extends CommandBase {
     xAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0);
     yAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0);
 
+    // Finding Distance
+    distance = VisionConstants.TARGET_HEIGHT / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180));
+    realDistance = (1.09 * distance) - 12.8;
+
+
     if (target == 1.0) { // If target in view
       // Aiming Left/Right
+      xAngle = xAngle + m_shooter.m_shooterTable.getCenterDisplacement(realDistance);
       turnAmount = (xAngle / VisionConstants.FOV) * VisionConstants.TURN_P_VALUE;
       if (Math.abs(xAngle) < VisionConstants.X_ANGLE_ERROR) {
         turnAmount = 0;
@@ -79,10 +88,8 @@ public class TrackTarget extends CommandBase {
       }
       m_shooterAim.runShooterWithInput(-turnAmount);// - m_shooter.shooterTrims.m_turretTrim);
 
-      // Finding Distance
-      distance = VisionConstants.TARGET_HEIGHT / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180));
-      SmartDashboard.putNumber("Distance to Target", distance);
-
+      SmartDashboard.putNumber("Distance to Target", realDistance);
+      SmartDashboard.putNumber("Center Displacement", m_shooter.m_shooterTable.getCenterDisplacement(realDistance));
         //START Equation Code
       double yVel = Math.sqrt(2 * VisionConstants.GRAV * VisionConstants.TARGET_HEIGHT);
       double xVel = (distance * VisionConstants.GRAV) / (yVel);
@@ -92,8 +99,8 @@ public class TrackTarget extends CommandBase {
         //END Equation Code
 
         //START CSV Code
-      fireVel = m_shooter.m_shooterTable.getVelocity(distance);
-      fireAngle = m_shooter.m_shooterTable.getHood(distance); //Note: Ensure to follow because units are different
+      fireVel = m_shooter.m_shooterTable.getVelocity(realDistance);
+      fireAngle = m_shooter.m_shooterTable.getHood(realDistance); //Note: Ensure to follow because units are different
       //fireAngle = 33;
         //END CSV Code
 
@@ -115,7 +122,7 @@ public class TrackTarget extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (xAngle < 1 && xAngle > -1 && target == 1)
+    if (xAngle < 0.5 && xAngle > -0.5 && target == 1)
     {
       m_shooterAim.m_isAimReady = true;
     } else {

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

@@ -0,0 +1,141 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot.commands.storage;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.Constants.StorageConstants;
+import frc4388.robot.subsystems.Storage;
+
+public class ManageStorage extends CommandBase {
+  Storage m_storage;
+
+  /* Timer */
+  long m_resetStartTime;
+
+  /* Keeps track of which beam breaks are pressed */
+  boolean m_isBallInIntake = false;
+  boolean m_isBallInStorage = false;
+  boolean m_isBallInUseless = false;
+  boolean m_isBallInShooter = false;
+
+  /* Used for intaking a ball. Keeps track off when the 2nd ball in storage has moved */
+  boolean m_isStorageEmpty = true;
+
+  public enum StorageMode{IDLE, INTAKE, RESET};
+  StorageMode m_storageMode = StorageMode.IDLE;
+  
+  /**
+   * Creates a new ManageStorage.
+   */
+  public ManageStorage(Storage m_robotStorage, StorageMode storageMode) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    m_storage = m_robotStorage;
+    m_storageMode = storageMode;
+    addRequirements(m_storage);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    m_isBallInIntake = !m_storage.getBeamIntake();
+    m_isBallInStorage = !m_storage.getBeamStorage();
+    m_isBallInUseless = !m_storage.getBeamUseless();
+    m_isBallInShooter = !m_storage.getBeamShooter();
+
+    m_isStorageEmpty = !m_isBallInStorage;
+
+    if (m_storageMode == StorageMode.RESET) {
+      m_resetStartTime = System.currentTimeMillis();
+    }
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_isBallInIntake = !m_storage.getBeamIntake();
+    m_isBallInStorage = !m_storage.getBeamStorage();
+    m_isBallInUseless = !m_storage.getBeamUseless();
+    m_isBallInShooter = !m_storage.getBeamShooter();
+
+    /// TODO: Delete/Comment these when done
+    SmartDashboard.putBoolean("!Ball in Intake!", m_isBallInIntake);
+    SmartDashboard.putBoolean("!Ball Storage!", m_isBallInStorage);
+    SmartDashboard.putBoolean("!Ball Shooter!", m_isBallInShooter);
+
+    if (m_storageMode == StorageMode.IDLE) {
+      runIdle();
+    } else if (m_storageMode == StorageMode.INTAKE) {
+      runIntake();
+    } else if (m_storageMode == StorageMode.RESET) {
+      runReset();
+    }
+  }
+
+  /**
+   * Intakes a ball.
+   * Runs until the storage ball has moved past the
+   * storage sensor and the intake ball has taken its place.
+   */
+  private void runIntake() {
+    if (!m_isBallInShooter) { // Intake balls as long as there is not a ball at the shooter
+      m_storage.runStorage(StorageConstants.STORAGE_SPEED);
+
+      if (!m_isStorageEmpty && !m_isBallInStorage) { // If ball moves out of storage, set storage to empty
+        m_isStorageEmpty = true;
+      }
+      if (m_isStorageEmpty && m_isBallInStorage) { // If Ball moves into storage, set storage to full and swtich to idle mode
+        m_isStorageEmpty = false;
+        m_storageMode = StorageMode.IDLE;
+      }
+    } else {
+      m_storageMode = StorageMode.IDLE;
+    }
+  }
+
+  /**
+   * Idles until a ball is in the intake.
+   * Also updates the status of the storage position
+   */
+  private void runIdle() {
+    m_storage.runStorage(0);
+
+    if (m_isBallInIntake) {
+      m_storageMode = StorageMode.INTAKE;
+    }
+    m_isStorageEmpty = !m_isBallInStorage;
+  }
+
+  /**
+   * Runs Storage out until the Intake Sensor is tripped.
+   * Then switches into intake mode. This effectively
+   * resets the position of the balls back to the bottom of the storage.
+   */
+  private void runReset() {
+    m_storage.runStorage(-StorageConstants.STORAGE_SPEED);
+
+    if (m_isBallInIntake) {
+      m_storageMode = StorageMode.INTAKE;
+    } else if (m_resetStartTime + StorageConstants.STORAGE_TIMEOUT < System.currentTimeMillis()) {
+      m_storageMode = StorageMode.IDLE;
+    }
+    m_isStorageEmpty = !m_isBallInStorage;
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    m_storageMode = StorageMode.RESET;
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc4388/robot/commands/storage/ManageStoragePID.java

@@ -0,0 +1,145 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot.commands.storage;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.Constants.StorageConstants;
+import frc4388.robot.subsystems.Storage;
+
+public class ManageStoragePID extends CommandBase {
+  Storage m_storage;
+
+  /* Timer */
+  long m_resetStartTime;
+
+  /* Start Positions */
+  double m_intakeStartPos;
+
+  /* Keeps track of which beam breaks are pressed */
+  boolean m_isBallInIntake = false;
+  boolean m_isBallInStorage = false;
+  boolean m_isBallInUseless = false;
+  boolean m_isBallInShooter = false;
+
+  /* Used for intaking a ball. Keeps track off when the 2nd ball in storage has moved */
+  boolean m_isStorageEmpty = true;
+
+  public enum StorageMode{IDLE, INTAKE, RESET};
+  StorageMode m_storageMode = StorageMode.IDLE;
+  
+  /**
+   * Creates a new ManageStorage.
+   */
+  public ManageStoragePID(Storage m_robotStorage, StorageMode storageMode) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    m_storage = m_robotStorage;
+    m_storageMode = storageMode;
+    addRequirements(m_storage);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    m_isBallInIntake = !m_storage.getBeamIntake();
+    m_isBallInStorage = !m_storage.getBeamStorage();
+    m_isBallInUseless = !m_storage.getBeamUseless();
+    m_isBallInShooter = !m_storage.getBeamShooter();
+
+    m_isStorageEmpty = !m_isBallInStorage;
+
+    m_intakeStartPos = m_storage.getEncoderPosInches();
+
+    if (m_storageMode == StorageMode.RESET) {
+      m_resetStartTime = System.currentTimeMillis();
+    }
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_isBallInIntake = !m_storage.getBeamIntake();
+    m_isBallInStorage = !m_storage.getBeamStorage();
+    m_isBallInUseless = !m_storage.getBeamUseless();
+    m_isBallInShooter = !m_storage.getBeamShooter();
+
+    /// TODO: Delete/Comment these when done
+    SmartDashboard.putBoolean("!Ball in Intake!", m_isBallInIntake);
+    SmartDashboard.putBoolean("!Ball Storage!", m_isBallInStorage);
+    SmartDashboard.putBoolean("!Ball Shooter!", m_isBallInShooter);
+
+    if (m_storageMode == StorageMode.IDLE) {
+      runIdle();
+    } else if (m_storageMode == StorageMode.INTAKE) {
+      runIntake();
+    } else if (m_storageMode == StorageMode.RESET) {
+      runReset();
+    }
+  }
+
+  /**
+   * Intakes a ball.
+   * Runs until the storage ball has moved past the
+   * storage sensor and the intake ball has taken its place.
+   */
+  private void runIntake() {
+    if (!m_isBallInShooter) { // Intake balls as long as there is not a ball at the shooter
+      m_storage.runStoragePositionPID(m_intakeStartPos + StorageConstants.STORAGE_FULL_BALL);
+
+      double error = (m_intakeStartPos + StorageConstants.STORAGE_FULL_BALL) - m_storage.getEncoderPosInches();
+      if (m_storage.getEncoderVel() == 0 && Math.abs(error) < 0.5) {
+        m_storageMode = StorageMode.IDLE;
+      }
+    } else {
+      m_storageMode = StorageMode.IDLE;
+    }
+  }
+
+  /**
+   * Idles until a ball is in the intake.
+   * Also updates the status of the storage position
+   */
+  private void runIdle() {
+    m_storage.runStorage(0);
+
+    if (m_isBallInIntake) {
+      m_storageMode = StorageMode.INTAKE;
+      m_intakeStartPos = m_storage.getEncoderPosInches();
+    }
+    m_isStorageEmpty = !m_isBallInStorage;
+  }
+
+  /**
+   * Runs Storage out until the Intake Sensor is tripped.
+   * Then switches into intake mode. This effectively
+   * resets the position of the balls back to the bottom of the storage.
+   */
+  private void runReset() {
+    m_storage.runStorage(-StorageConstants.STORAGE_SPEED);
+
+    if (m_isBallInIntake) {
+      m_storageMode = StorageMode.INTAKE;
+      m_intakeStartPos = m_storage.getEncoderPosInches();
+    } else if (m_resetStartTime + StorageConstants.STORAGE_TIMEOUT < System.currentTimeMillis()) {
+      m_storageMode = StorageMode.IDLE;
+    }
+    m_isStorageEmpty = !m_isBallInStorage;
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    m_storageMode = StorageMode.RESET;
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc4388/robot/commands/storage/StorageIntake.java

@@ -1,63 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package frc4388.robot.commands.storage;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc4388.robot.Constants.StorageConstants;
-import frc4388.robot.subsystems.Intake;
-import frc4388.robot.subsystems.Storage;
-
-public class StorageIntake extends CommandBase {
-  public Intake m_intake;
-  public Storage m_storage;
-  public boolean intook;
-  /**
-   * Runs the Storage in for intaking
-   * @param inSub The Intake subsystem
-   * @param storeSub The Storage subsytem
-   */
-  public StorageIntake(Intake inSub, Storage storeSub) {
-    m_intake = inSub;
-    m_storage = storeSub;
-    addRequirements(m_intake);
-    addRequirements(m_storage);
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {
-    intook = false;
-  }
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-
-    if (!m_storage.getBeam(StorageConstants.BEAM_SENSOR_STORAGE) && !m_storage.getBeam(StorageConstants.BEAM_SENSOR_INTAKE)){
-      m_storage.runStorage(StorageConstants.STORAGE_SPEED);
-      intook = true;
-    }
-    else{
-      m_storage.runStorage(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() {
-    if (m_storage.getBeam(StorageConstants.BEAM_SENSOR_STORAGE) && intook){
-      return true;
-    }
-    return false;
-  }
-}

src/main/java/frc4388/robot/commands/storage/StorageOutake.java

@@ -1,46 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package frc4388.robot.commands.storage;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc4388.robot.Constants.StorageConstants;
-import frc4388.robot.subsystems.Storage;
-
-public class StorageOutake extends CommandBase {
-  Storage m_storage;
-  /**
-   * Runs the Storage out for outaking
-   * @param storeSub The Storage subsystem
-   */
-  public StorageOutake(Storage storeSub) {
-    m_storage = storeSub;
-    addRequirements(m_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() {
-      m_storage.runStorage(-StorageConstants.STORAGE_SPEED);
-  }
-
-  // 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/storage/StoragePositionPID.java

@@ -1,52 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package frc4388.robot.commands.storage;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc4388.robot.Constants.StorageConstants;
-import frc4388.robot.subsystems.Storage;
-
-public class StoragePositionPID extends CommandBase {
-  public Storage m_storage;
-  double startPos;
-  /**
-   * Moves the storage a number of rotations
-   * @param subsystem The Storage subsystem
-   */
-  public StoragePositionPID(Storage subsystem) {
-    m_storage = subsystem;
-    addRequirements(m_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() {
-    m_storage.runStoragePositionPID(StorageConstants.STORAGE_FULL_BALL);
-  }
-
-  // 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() {
-    /*if (startPos + StorageConstants.STORAGE_FULL_BALL == m_storage.getEncoderPos())
-    {
-      return true;
-    }*/
-    return false;
-  }
-}

src/main/java/frc4388/robot/commands/storage/StoragePrep.java

@@ -7,19 +7,18 @@
 
 package frc4388.robot.commands.storage;
 
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.robot.Constants.StorageConstants;
 import frc4388.robot.subsystems.Storage;
 
-public class StoragePrepAim extends CommandBase {
+public class StoragePrep extends CommandBase {
   Storage m_storage;
-  double startTime;
+  double m_startTime;
   /**
    * Prepares the Storage for aiming
    * @param storeSub The Storage subsystem
    */
-  public StoragePrepAim(Storage storeSub) {
+  public StoragePrep(Storage storeSub) {
     m_storage = storeSub;
     addRequirements(m_storage);
   }
@@ -27,17 +26,18 @@ public class StoragePrepAim extends CommandBase {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    startTime = System.currentTimeMillis();
+    m_startTime = System.currentTimeMillis();
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if (m_storage.getBeam(1)){
-      //m_storage.runStorage(StorageConstants.STORAGE_SPEED);
-    }
-    else{
+    if ((m_storage.getBeamUseless() && m_storage.getBeamShooter()) || (m_startTime + StorageConstants.STORAGE_TIMEOUT) < System.currentTimeMillis()) {
+      m_storage.runStorage(StorageConstants.STORAGE_SPEED);
+      m_storage.m_isStorageReadyToFire = false;
+    } else {
       m_storage.runStorage(0);
+      m_storage.m_isStorageReadyToFire = true;
     }
   }
 
@@ -49,13 +49,6 @@ public class StoragePrepAim extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    /*if (!m_storage.getBeam(1) || startTime + StorageConstants.STORAGE_TIMEOUT <= System.currentTimeMillis()){
-      m_storage.m_isStorageReadyToFire = true;
-      return true;
-    } else {
-      m_storage.m_isStorageReadyToFire = false;
-      return false;
-    }*/
-    return true;
+    return false;
   }
 }

src/main/java/frc4388/robot/commands/storage/StoragePrepIntake.java

@@ -1,62 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package frc4388.robot.commands.storage;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc4388.robot.Constants.StorageConstants;
-import frc4388.robot.subsystems.Intake;
-import frc4388.robot.subsystems.Storage;
-
-public class StoragePrepIntake extends CommandBase {
-  public Intake m_intake;
-  public Storage m_storage;
-  public double startTime;
-
-  /**
-   * Prepares the Storage for intaking
-   * @param inSub The Intake subsystem
-   * @param storeSub the Storage subsystem
-   */
-  public StoragePrepIntake(Intake inSub, Storage storeSub) {
-    m_intake = inSub;
-    m_storage = storeSub;
-    addRequirements(m_intake);
-    addRequirements(m_storage);
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {
-    startTime = System.currentTimeMillis();
-  }
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-    if (m_storage.getBeam(StorageConstants.BEAM_SENSOR_STORAGE)){
-      m_storage.runStorage(-StorageConstants.STORAGE_SPEED);
-    }
-    else{
-      m_storage.runStorage(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() {
-    if (!m_storage.getBeam(StorageConstants.BEAM_SENSOR_INTAKE) || startTime + StorageConstants.STORAGE_TIMEOUT <= System.currentTimeMillis()){
-      return false;
-    }
-    return false;
-  }
-}

src/main/java/frc4388/robot/commands/storage/StorageRun.java

@@ -1,46 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package frc4388.robot.commands.storage;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc4388.robot.Constants.StorageConstants;
-import frc4388.robot.subsystems.Storage;
-
-public class StorageRun extends CommandBase {
-  Storage m_storage;
-  /**
-   * Runs the Storage at a speed
-   * @param storageSub The Storage subsytem
-   */
-  public StorageRun(Storage storageSub) {
-    m_storage = storageSub;
-  }
-
-  // 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() {
-    m_storage.runStorage(StorageConstants.STORAGE_SPEED);
-  }
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {
-    m_storage.runStorage(0);
-  }
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return false;
-  }
-}