From a856a6633976af5c72f2c28eeca7357f5b4e01a6 Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Sat, 7 Mar 2020 10:09:44 -0700
Subject: [PATCH 01/25] P val change

Co-Authored-By: Keenan D. Buckley <hfocus@users.noreply.github.com>
---
 src/main/java/frc4388/robot/Constants.java                 | 2 +-
 .../java/frc4388/robot/commands/shooter/TrackTarget.java   | 7 +++++--
 2 files changed, 6 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index b951b15..221430e 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -200,7 +200,7 @@ public final class Constants {
         public static final double FOV = 29.8; //Field of view of limelight
         public static final double TARGET_HEIGHT = 71;
         public static final double LIME_ANGLE = 25;
-        public static final double TURN_P_VALUE = 0.65;
+        public static final double TURN_P_VALUE = 0.6;
         public static final double X_ANGLE_ERROR = 1.3;
         public static final double MOTOR_DEAD_ZONE = 0.3;
         public static final double DISTANCE_ERROR_EQUATION_M = 1.1279;
diff --git a/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java
index 6d35f49..16b770a 100644
--- a/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java
+++ b/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;
@@ -48,6 +49,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.
@@ -82,7 +84,8 @@ public class TrackTarget extends CommandBase {
       // Finding Distance
       distance = VisionConstants.TARGET_HEIGHT / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180));
       SmartDashboard.putNumber("Distance to Target", distance);
-
+      SmartDashboard.putNumber("ts Skew or Rotation", NetworkTableInstance.getDefault().getTable("limelight").getEntry("ts").getDouble(0));
+      SmartDashboard.putNumber("ta Area", NetworkTableInstance.getDefault().getTable("limelight").getEntry("ta").getDouble(0));
         //START Equation Code
       double yVel = Math.sqrt(2 * VisionConstants.GRAV * VisionConstants.TARGET_HEIGHT);
       double xVel = (distance * VisionConstants.GRAV) / (yVel);
@@ -115,7 +118,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 {

From 9d4f9f7020a6b504337728cc0d3edbb74b55ab13 Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Sat, 7 Mar 2020 11:22:32 -0700
Subject: [PATCH 02/25] Lime distance within 5 in

Co-Authored-By: Keenan D. Buckley <hfocus@users.noreply.github.com>
---
 src/main/java/frc4388/robot/Constants.java                | 4 ++--
 .../java/frc4388/robot/commands/shooter/TrackTarget.java  | 8 +++++---
 2 files changed, 7 insertions(+), 5 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 221430e..e9ac88d 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -198,8 +198,8 @@ public final class Constants {
     
     public static final class VisionConstants {
         public static final double FOV = 29.8; //Field of view of limelight
-        public static final double TARGET_HEIGHT = 71;
-        public static final double LIME_ANGLE = 25;
+        public static final double TARGET_HEIGHT = 66.75;
+        public static final double LIME_ANGLE = 28.387;
         public static final double TURN_P_VALUE = 0.6;
         public static final double X_ANGLE_ERROR = 1.3;
         public static final double MOTOR_DEAD_ZONE = 0.3;
diff --git a/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java
index 16b770a..d4bb084 100644
--- a/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java
+++ b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java
@@ -32,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;
 
@@ -83,7 +84,8 @@ public class TrackTarget extends CommandBase {
 
       // Finding Distance
       distance = VisionConstants.TARGET_HEIGHT / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180));
-      SmartDashboard.putNumber("Distance to Target", distance);
+      realDistance = (1.13 * distance) - 14.3;
+      SmartDashboard.putNumber("Distance to Target", realDistance);
       SmartDashboard.putNumber("ts Skew or Rotation", NetworkTableInstance.getDefault().getTable("limelight").getEntry("ts").getDouble(0));
       SmartDashboard.putNumber("ta Area", NetworkTableInstance.getDefault().getTable("limelight").getEntry("ta").getDouble(0));
         //START Equation Code
@@ -95,8 +97,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
 

From 9787998778af1ce5c765b18aaba9fbb6a126bfd6 Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Sat, 7 Mar 2020 13:22:19 -0700
Subject: [PATCH 03/25] PID fix and Idle

---
 src/main/java/frc4388/robot/Constants.java      | 3 +--
 src/main/java/frc4388/robot/RobotContainer.java | 2 +-
 2 files changed, 2 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index e9ac88d..de9ae73 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -122,8 +122,7 @@ public final class Constants {
         public static final int SHOOTER_SLOT_IDX = 0;
         public static final int SHOOTER_PID_LOOP_IDX = 1;
         public static final int SHOOTER_TIMEOUT_MS = 30;
-        public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0005, 13, 0.05, 0, 1.0);
-        //public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.0, 0.0, 0.0, 0.05, 0, 1.0);
+        public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.55, 0.0, 100, 0.0, 0, 1.0);
         public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0);
         public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.3);
         public static final double SHOOTER_TURRET_MIN = -1.0;
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index c44884a..cc622d4 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -117,7 +117,7 @@ public class RobotContainer {
         // runs the turret with joystick
         m_robotShooterAim.setDefaultCommand(new RunCommand(() -> m_robotShooterAim.runShooterWithInput(-m_operatorXbox.getLeftXAxis()), m_robotShooterAim));
         // moves the drum not
-        m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooter(0), m_robotShooter));
+        m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(1500), m_robotShooter));
         // drives climber with input from triggers on the opperator controller
         m_robotClimber.setDefaultCommand(new RunClimberWithTriggers(m_robotClimber, getDriverController()));
         // drives the leveler with an axis input from the driver controller

From 7f9b8d9c9f49536128755d70d78b343bdc31ff6d Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Sat, 7 Mar 2020 17:38:14 -0700
Subject: [PATCH 04/25] Shooter Tuning

---
 src/main/deploy/Robot Data - Distances.csv    | 22 +++++++++++--------
 src/main/java/frc4388/robot/Constants.java    |  4 ++--
 .../robot/commands/shooter/TrackTarget.java   | 12 +++++-----
 .../java/frc4388/utility/ShooterTables.java   | 21 +++++++++++++++++-
 4 files changed, 42 insertions(+), 17 deletions(-)

diff --git a/src/main/deploy/Robot Data - Distances.csv b/src/main/deploy/Robot Data - Distances.csv
index 5a625da..b60700b 100644
--- a/src/main/deploy/Robot Data - Distances.csv	
+++ b/src/main/deploy/Robot Data - Distances.csv	
@@ -1,9 +1,13 @@
-Distance (in),Hood Ext. (u),Drum Velocity (u/ds)
-74,20,8000
-144,23,10000
-162,28,10000
-208,29,10000
-296,32,12500
-418,33,16000
-430,31,16000
-450,31,16000
\ No newline at end of file
+Distance (in),Hood Ext. (u),Drum Velocity (u/ds),Center Displacment (deg)
+1,10,7000,2
+70,20,8000,2, double check
+100,24,9000,2
+145,28,10000,1,Add a 200
+230,31,12000,0,Add a 270
+320,32,17000,0,change 17000 and mark them lower
+331,33,17000,0
+397,33,16000,0
+415,33,16250,0
+436,31,18000,0
+500,33,18000,0
+501,33,18000,0
\ No newline at end of file
diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index de9ae73..c0e3dbf 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -197,8 +197,8 @@ public final class Constants {
     
     public static final class VisionConstants {
         public static final double FOV = 29.8; //Field of view of limelight
-        public static final double TARGET_HEIGHT = 66.75;
-        public static final double LIME_ANGLE = 28.387;
+        public static final double TARGET_HEIGHT = 71.5;
+        public static final double LIME_ANGLE = 24.7;
         public static final double TURN_P_VALUE = 0.6;
         public static final double X_ANGLE_ERROR = 1.3;
         public static final double MOTOR_DEAD_ZONE = 0.3;
diff --git a/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java
index d4bb084..a1e688e 100644
--- a/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java
+++ b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java
@@ -68,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;
@@ -82,12 +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));
-      realDistance = (1.13 * distance) - 14.3;
       SmartDashboard.putNumber("Distance to Target", realDistance);
-      SmartDashboard.putNumber("ts Skew or Rotation", NetworkTableInstance.getDefault().getTable("limelight").getEntry("ts").getDouble(0));
-      SmartDashboard.putNumber("ta Area", NetworkTableInstance.getDefault().getTable("limelight").getEntry("ta").getDouble(0));
+      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);
diff --git a/src/main/java/frc4388/utility/ShooterTables.java b/src/main/java/frc4388/utility/ShooterTables.java
index 85244ce..62af09e 100644
--- a/src/main/java/frc4388/utility/ShooterTables.java
+++ b/src/main/java/frc4388/utility/ShooterTables.java
@@ -20,12 +20,13 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
  * Add your docs here.
  */
 public class ShooterTables {
-    double[][] m_distance = new double[50][3];
+    double[][] m_distance = new double[50][4];
     double[][] m_angle = new double[50][2];
 
     final int m_columnDistance = 0;
     final int m_columnHood = 1;
     final int m_columnVel = 2;
+    final int m_columnCenterDisplacement = 3;
     final int m_columnAngle = 0;
     final int m_columnDisplacement = 1;
 
@@ -55,6 +56,7 @@ public class ShooterTables {
                     m_distance[lineNum - 1][m_columnDistance] = Double.parseDouble(values[0]);
                     m_distance[lineNum - 1][m_columnHood] = Double.parseDouble(values[1]);
                     m_distance[lineNum - 1][m_columnVel] = Double.parseDouble(values[2]);
+                    m_distance[lineNum - 1][m_columnCenterDisplacement] = Double.parseDouble(values[3]);
                     
                     lineNum ++;
                 }
@@ -123,6 +125,23 @@ public class ShooterTables {
         }
     }
 
+    public double getCenterDisplacement(double distance) { //Degrees of Lime FOV
+        int i = 0;
+        while ((i < m_distanceLength) && (m_distance[i][m_columnDistance] < distance)) {
+            i ++;
+        }
+        if ((i < m_distanceLength) && (m_distance[i][m_columnDistance] == distance)) {
+            return m_distance[i][m_columnCenterDisplacement];
+        } else {
+            if (i >= m_distanceLength) {
+                i = m_distanceLength - 1;
+            }
+            return linearInterpolation(i, distance, m_columnCenterDisplacement, m_distance);
+        }
+    }
+
+    
+
     public double getAngleDisplacement(double angle) {
         int i = 0;
         while ((i < m_angleLength) && (m_angle[i][m_columnAngle] < angle)) {

From 6ad2a614131c1f2e6f5a0d3660bb7f5c0c60c771 Mon Sep 17 00:00:00 2001
From: "Keenan D. Buckley" <keenandbuckley@protonmail.com>
Date: Sun, 8 Mar 2020 10:24:40 -0600
Subject: [PATCH 05/25] Rename Related Commands

---
 .../java/frc4388/robot/commands/shooter/ShootFireGroup.java   | 4 ++--
 .../java/frc4388/robot/commands/shooter/ShootPrepGroup.java   | 4 ++--
 .../commands/storage/{StorageRun.java => StorageFire.java}    | 4 ++--
 .../storage/{StoragePrepAim.java => StoragePrep.java}         | 4 ++--
 4 files changed, 8 insertions(+), 8 deletions(-)
 rename src/main/java/frc4388/robot/commands/storage/{StorageRun.java => StorageFire.java} (93%)
 rename src/main/java/frc4388/robot/commands/storage/{StoragePrepAim.java => StoragePrep.java} (95%)

diff --git a/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java b/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java
index 15ee080..02d5799 100644
--- a/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java
+++ b/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java
@@ -9,7 +9,7 @@ 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.commands.storage.StorageFire;
 import frc4388.robot.subsystems.Shooter;
 import frc4388.robot.subsystems.ShooterAim;
 import frc4388.robot.subsystems.ShooterHood;
@@ -30,7 +30,7 @@ public class ShootFireGroup extends ParallelRaceGroup {
       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)
+      //new StorageFire(m_storage)
     );
   }
 }
\ No newline at end of file
diff --git a/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java b/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java
index 14accc3..d919d32 100644
--- a/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java
+++ b/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;
@@ -30,7 +30,7 @@ public class ShootPrepGroup extends ParallelDeadlineGroup {
       new TrackTarget(m_shooterAim),
       new ShooterVelocityControlPID(m_shooter),
       new HoodPositionPID(m_shooterHood),
-      new StoragePrepAim(m_storage)
+      new StoragePrep(m_storage)
     );
   }
 }
diff --git a/src/main/java/frc4388/robot/commands/storage/StorageRun.java b/src/main/java/frc4388/robot/commands/storage/StorageFire.java
similarity index 93%
rename from src/main/java/frc4388/robot/commands/storage/StorageRun.java
rename to src/main/java/frc4388/robot/commands/storage/StorageFire.java
index 11dbc4b..6413c1e 100644
--- a/src/main/java/frc4388/robot/commands/storage/StorageRun.java
+++ b/src/main/java/frc4388/robot/commands/storage/StorageFire.java
@@ -11,13 +11,13 @@ import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.robot.Constants.StorageConstants;
 import frc4388.robot.subsystems.Storage;
 
-public class StorageRun extends CommandBase {
+public class StorageFire extends CommandBase {
   Storage m_storage;
   /**
    * Runs the Storage at a speed
    * @param storageSub The Storage subsytem
    */
-  public StorageRun(Storage storageSub) {
+  public StorageFire(Storage storageSub) {
     m_storage = storageSub;
   }
 
diff --git a/src/main/java/frc4388/robot/commands/storage/StoragePrepAim.java b/src/main/java/frc4388/robot/commands/storage/StoragePrep.java
similarity index 95%
rename from src/main/java/frc4388/robot/commands/storage/StoragePrepAim.java
rename to src/main/java/frc4388/robot/commands/storage/StoragePrep.java
index 6d47abf..09850f8 100644
--- a/src/main/java/frc4388/robot/commands/storage/StoragePrepAim.java
+++ b/src/main/java/frc4388/robot/commands/storage/StoragePrep.java
@@ -12,14 +12,14 @@ 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;
   /**
    * Prepares the Storage for aiming
    * @param storeSub The Storage subsystem
    */
-  public StoragePrepAim(Storage storeSub) {
+  public StoragePrep(Storage storeSub) {
     m_storage = storeSub;
     addRequirements(m_storage);
   }

From a7558a751130ab92f06f651a54291cec02dec7f2 Mon Sep 17 00:00:00 2001
From: "Keenan D. Buckley" <keenandbuckley@protonmail.com>
Date: Sun, 8 Mar 2020 11:26:57 -0600
Subject: [PATCH 06/25] Add Manage Storage Command

---
 .../java/frc4388/robot/RobotContainer.java    |   2 -
 .../robot/commands/storage/ManageStorage.java | 123 ++++++++++++++++++
 2 files changed, 123 insertions(+), 2 deletions(-)
 create mode 100644 src/main/java/frc4388/robot/commands/storage/ManageStorage.java

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index bbe5ff6..7588412 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -42,8 +42,6 @@ import frc4388.robot.commands.shooter.ShootFullGroup;
 import frc4388.robot.commands.shooter.ShootPrepGroup;
 import frc4388.robot.commands.shooter.TrackTarget;
 import frc4388.robot.commands.shooter.TrimShooter;
-import frc4388.robot.commands.storage.StorageIntake;
-import frc4388.robot.commands.storage.StoragePrepIntake;
 import frc4388.robot.subsystems.Camera;
 import frc4388.robot.subsystems.Climber;
 import frc4388.robot.subsystems.Drive;
diff --git a/src/main/java/frc4388/robot/commands/storage/ManageStorage.java b/src/main/java/frc4388/robot/commands/storage/ManageStorage.java
new file mode 100644
index 0000000..3709946
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/storage/ManageStorage.java
@@ -0,0 +1,123 @@
+/*----------------------------------------------------------------------------*/
+/* 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;
+
+  /* Keeps track of which beam breaks are pressed */
+  boolean isBallInIntake = false;
+  boolean isBallInStorage = false;
+  boolean isBallInShooter = false;
+
+  /* Used for intaking a ball. Keeps track off when the 2nd ball in storage has moved */
+  boolean m_isStorageEmpty = true;
+
+  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() {
+    isBallInIntake = !m_storage.getBeam(StorageConstants.BEAM_SENSOR_INTAKE);
+    isBallInStorage = !m_storage.getBeam(StorageConstants.BEAM_SENSOR_STORAGE);
+    isBallInShooter = !m_storage.getBeam(StorageConstants.BEAM_SENSOR_SHOOTER);
+
+    m_isStorageEmpty = !isBallInStorage;
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    isBallInIntake = !m_storage.getBeam(StorageConstants.BEAM_SENSOR_INTAKE);
+    isBallInStorage = !m_storage.getBeam(StorageConstants.BEAM_SENSOR_STORAGE);
+    isBallInShooter = !m_storage.getBeam(StorageConstants.BEAM_SENSOR_SHOOTER);
+
+    SmartDashboard.putBoolean("Ball in Intake", isBallInIntake);
+    SmartDashboard.putBoolean("Ball in Storage", isBallInStorage);
+    SmartDashboard.putBoolean("Ball in Shooter", 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() {
+    m_storage.runStorage(StorageConstants.STORAGE_SPEED);
+
+    if (!m_isStorageEmpty && !isBallInStorage) {
+      m_isStorageEmpty = true;
+    }
+    if (m_isStorageEmpty && isBallInStorage) {
+      m_isStorageEmpty = false;
+      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 (isBallInIntake) {
+      m_storageMode = StorageMode.INTAKE;
+    }
+    m_isStorageEmpty = !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 (isBallInIntake) {
+      m_storageMode = StorageMode.INTAKE;
+    }
+    m_isStorageEmpty = !isBallInStorage;
+  }
+
+  // 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;
+  }
+}

From 0b452d3309498596b1728bf2bcca3560b97c859b Mon Sep 17 00:00:00 2001
From: "Keenan D. Buckley" <keenandbuckley@protonmail.com>
Date: Sun, 8 Mar 2020 12:14:24 -0600
Subject: [PATCH 07/25] Fix Storage Subsystem

- Also improved the Gains object to have two constructors depending on what kind of output limits you want.
---
 src/main/java/frc4388/robot/Constants.java    | 11 ++-
 .../robot/commands/storage/ManageStorage.java | 22 +++---
 .../robot/commands/storage/StoragePrep.java   |  2 +-
 .../frc4388/robot/subsystems/Storage.java     | 78 +++++++++++++------
 src/main/java/frc4388/utility/Gains.java      | 26 ++++++-
 5 files changed, 99 insertions(+), 40 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 096f898..8ff656b 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -164,21 +164,24 @@ public final class Constants {
         public static final double STORAGE_SPEED = 0.5;
         public static final double STORAGE_TIMEOUT = 2000;
 
+        /* Storage Characteristics */
+        public static final double MOTOR_ROTS_PER_STORAGE_ROT = 1; //For the first storage belt
+        public static final double INCHES_PER_STORAGE_ROT = 1; //Circumference of the first storage belt
+
         /* Ball Indexes */
         public static final int BEAM_SENSOR_SHOOTER = 1;
         public static final int BEAM_SENSOR_USELESS = 2;
         public static final int BEAM_SENSOR_STORAGE = 3;
         public static final int BEAM_SENSOR_INTAKE = 4;
 
+        /* PID Gains */
+        public static final Gains STORAGE_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0);
+
         /* PID Values */
         public static final int SLOT_DISTANCE = 0;
 
         /* PID Indexes */
         public static final int PID_PRIMARY = 0;
-
-        /* PID Gains */
-        public static final double STORAGE_MIN_OUTPUT = -1.0;
-        public static final Gains STORAGE_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0);
     }
 
     public static final class PneumaticsConstants {
diff --git a/src/main/java/frc4388/robot/commands/storage/ManageStorage.java b/src/main/java/frc4388/robot/commands/storage/ManageStorage.java
index 3709946..ea4aa19 100644
--- a/src/main/java/frc4388/robot/commands/storage/ManageStorage.java
+++ b/src/main/java/frc4388/robot/commands/storage/ManageStorage.java
@@ -18,6 +18,7 @@ public class ManageStorage extends CommandBase {
   /* Keeps track of which beam breaks are pressed */
   boolean isBallInIntake = false;
   boolean isBallInStorage = false;
+  boolean isBallInUseless = false;
   boolean isBallInShooter = false;
 
   /* Used for intaking a ball. Keeps track off when the 2nd ball in storage has moved */
@@ -39,9 +40,10 @@ public class ManageStorage extends CommandBase {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    isBallInIntake = !m_storage.getBeam(StorageConstants.BEAM_SENSOR_INTAKE);
-    isBallInStorage = !m_storage.getBeam(StorageConstants.BEAM_SENSOR_STORAGE);
-    isBallInShooter = !m_storage.getBeam(StorageConstants.BEAM_SENSOR_SHOOTER);
+    isBallInIntake = !m_storage.getBeamIntake();
+    isBallInStorage = !m_storage.getBeamStorage();
+    isBallInUseless = !m_storage.getBeamUseless();
+    isBallInShooter = !m_storage.getBeamShooter();
 
     m_isStorageEmpty = !isBallInStorage;
   }
@@ -49,13 +51,15 @@ public class ManageStorage extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    isBallInIntake = !m_storage.getBeam(StorageConstants.BEAM_SENSOR_INTAKE);
-    isBallInStorage = !m_storage.getBeam(StorageConstants.BEAM_SENSOR_STORAGE);
-    isBallInShooter = !m_storage.getBeam(StorageConstants.BEAM_SENSOR_SHOOTER);
+    isBallInIntake = !m_storage.getBeamIntake();
+    isBallInStorage = !m_storage.getBeamStorage();
+    isBallInUseless = !m_storage.getBeamUseless();
+    isBallInShooter = !m_storage.getBeamShooter();
 
-    SmartDashboard.putBoolean("Ball in Intake", isBallInIntake);
-    SmartDashboard.putBoolean("Ball in Storage", isBallInStorage);
-    SmartDashboard.putBoolean("Ball in Shooter", isBallInShooter);
+    /// TODO: Delete/Comment these when done
+    SmartDashboard.putBoolean("!Ball in Intake!", isBallInIntake);
+    SmartDashboard.putBoolean("!Ball Storage!", isBallInStorage);
+    SmartDashboard.putBoolean("!Ball Shooter!", isBallInShooter);
 
     if (m_storageMode == StorageMode.IDLE) {
       runIdle();
diff --git a/src/main/java/frc4388/robot/commands/storage/StoragePrep.java b/src/main/java/frc4388/robot/commands/storage/StoragePrep.java
index 09850f8..00c9bb5 100644
--- a/src/main/java/frc4388/robot/commands/storage/StoragePrep.java
+++ b/src/main/java/frc4388/robot/commands/storage/StoragePrep.java
@@ -33,7 +33,7 @@ public class StoragePrep extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if (m_storage.getBeam(1)){
+    if (m_storage.getBeamShooter()){
       //m_storage.runStorage(StorageConstants.STORAGE_SPEED);
     }
     else{
diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java
index 4caa519..175e499 100644
--- a/src/main/java/frc4388/robot/subsystems/Storage.java
+++ b/src/main/java/frc4388/robot/subsystems/Storage.java
@@ -21,7 +21,10 @@ import frc4388.utility.Gains;
 
 public class Storage extends SubsystemBase {
   public CANSparkMax m_storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless);
-  private DigitalInput[] m_beamSensors = new DigitalInput[6];
+  private DigitalInput m_beamShooter = new DigitalInput(StorageConstants.BEAM_SENSOR_SHOOTER);
+  private DigitalInput m_beamUseless = new DigitalInput(StorageConstants.BEAM_SENSOR_USELESS);
+  private DigitalInput m_beamStorage = new DigitalInput(StorageConstants.BEAM_SENSOR_STORAGE);
+  private DigitalInput m_beamIntake = new DigitalInput(StorageConstants.BEAM_SENSOR_INTAKE);
 
   CANPIDController m_storagePIDController = m_storageMotor.getPIDController();
 
@@ -38,10 +41,14 @@ public class Storage extends SubsystemBase {
    */
   public Storage() {
     resetEncoder();
-    m_beamSensors[1] = new DigitalInput(StorageConstants.BEAM_SENSOR_SHOOTER);
-    m_beamSensors[2] = new DigitalInput(StorageConstants.BEAM_SENSOR_USELESS);
-    m_beamSensors[3] = new DigitalInput(StorageConstants.BEAM_SENSOR_STORAGE);
-    m_beamSensors[4] = new DigitalInput(StorageConstants.BEAM_SENSOR_INTAKE);
+
+    // Set PID Coefficients
+    m_storagePIDController.setP(storageGains.m_kP);
+    m_storagePIDController.setI(storageGains.m_kI);
+    m_storagePIDController.setD(storageGains.m_kD);
+    m_storagePIDController.setIZone(storageGains.m_kIzone);
+    m_storagePIDController.setFF(storageGains.m_kF);
+    m_storagePIDController.setOutputRange(storageGains.m_kminOutput, storageGains.m_kmaxOutput);
   }
 
   @Override
@@ -55,9 +62,8 @@ public class Storage extends SubsystemBase {
   /**
    * Runs storage motor
    * 
-   * @param input the voltage to run motor at
+   * @param input the percent output to run motor at
    */
-  
   public void runStorage(double input) {
     m_storageMotor.set(input);
   }
@@ -66,36 +72,58 @@ public class Storage extends SubsystemBase {
     m_encoder.setPosition(0);
   }
 
-  public void testBeams(){
-    SmartDashboard.putBoolean("Beam 0", m_beamSensors[0].get());
-    SmartDashboard.putBoolean("Beam 1", m_beamSensors[1].get());
-  }
-
-  /* Storage PID Control */
+  /**
+   * Runs Storage to a particular position
+   * @param targetPos in inches
+   */
   public void runStoragePositionPID(double targetPos){
-    // Set PID Coefficients
-    m_storagePIDController.setP(storageGains.m_kP);
-    m_storagePIDController.setI(storageGains.m_kI);
-    m_storagePIDController.setD(storageGains.m_kD);
-    m_storagePIDController.setIZone(storageGains.m_kIzone);
-    m_storagePIDController.setFF(storageGains.m_kF);
-    m_storagePIDController.setOutputRange(StorageConstants.STORAGE_MIN_OUTPUT, storageGains.m_kmaxOutput);
-
     //SmartDashboard.putNumber("Storage Position PID Target", targetPos);
     //SmartDashboard.putNumber("Storage Position Pos", getEncoderPos());
+    targetPos = InchesToMotorRots(targetPos);
     m_storagePIDController.setReference(targetPos, ControlType.kPosition);
   }
 
+  /**
+   * Runs Storage to a particular position
+   * @param position in motor rotations
+   */
+  public void setStoragePID(double position){
+    m_storagePIDController.setReference(position, ControlType.kPosition);
+  }
 
   public double getEncoderPos(){
     return m_encoder.getPosition();
   }
 
-  public boolean getBeam(int id){
-    return m_beamSensors[id].get();
+  /**
+   * @param motorRots
+   * @return inches
+   */
+  public double motorRotsToInches(double motorRots) {
+    return motorRots * (1/StorageConstants.MOTOR_ROTS_PER_STORAGE_ROT) * (StorageConstants.INCHES_PER_STORAGE_ROT);
   }
 
-  public void setStoragePID(double position){
-    m_storagePIDController.setReference(position , ControlType.kPosition);
+  /**
+   * @param inches
+   * @return motorRots
+   */
+  public double InchesToMotorRots(double inches) {
+    return inches * (1/StorageConstants.INCHES_PER_STORAGE_ROT) * (StorageConstants.MOTOR_ROTS_PER_STORAGE_ROT);
+  }
+
+  public boolean getBeamShooter(){
+    return m_beamShooter.get();
+  }
+
+  public boolean getBeamUseless(){
+    return m_beamUseless.get();
+  }
+
+  public boolean getBeamStorage(){
+    return m_beamStorage.get();
+  }
+
+  public boolean getBeamIntake(){
+    return m_beamIntake.get();
   }
 }
diff --git a/src/main/java/frc4388/utility/Gains.java b/src/main/java/frc4388/utility/Gains.java
index f1b78f9..cfbb689 100644
--- a/src/main/java/frc4388/utility/Gains.java
+++ b/src/main/java/frc4388/utility/Gains.java
@@ -27,7 +27,7 @@ public class Gains {
      * @param kD The D value.
      * @param kF The F value.
      * @param kIzone The zone of the I value.
-     * @param kPeakOutput The peak output setting the motors to run the gains at.
+     * @param kPeakOutput The peak output setting the motors to run the gains at, in both forward and reverse directions. By default 1.0.
      */
     public Gains(double kP, double kI, double kD, double kF, int kIzone, double kPeakOutput)
     {
@@ -37,5 +37,29 @@ public class Gains {
         m_kF = kF;
         m_kIzone = kIzone;
         m_kPeakOutput = kPeakOutput;
+        m_kmaxOutput = m_kPeakOutput;
+        m_kminOutput = -m_kPeakOutput;
+    }
+
+    /**
+     * Creates Gains object for PIDs
+     * @param kP The P value.
+     * @param kI The I value.
+     * @param kD The D value.
+     * @param kF The F value.
+     * @param kIzone The zone of the I value.
+     * @param kMinOutput The lowest output setting to run the gains at, usually in the reverse direction. By default -1.0.
+     * @param kMaxOutput The highest output setting to run the gains at, usually in the forward direction. By default 1.0.
+     */
+    public Gains(double kP, double kI, double kD, double kF, int kIzone, double kMinOutput, double kMaxOutput)
+    {
+        m_kP = kP;
+        m_kI = kI;
+        m_kD = kD;
+        m_kF = kF;
+        m_kIzone = kIzone;
+        m_kminOutput = kMinOutput;
+        m_kmaxOutput = kMaxOutput;
+        m_kPeakOutput = (Math.abs(m_kminOutput) > Math.abs(m_kmaxOutput)) ? Math.abs(m_kminOutput) : Math.abs(m_kmaxOutput);
     }
 }

From 809616b1e3fb97762264b4c42e5393d678acf6fa Mon Sep 17 00:00:00 2001
From: "Keenan D. Buckley" <keenandbuckley@protonmail.com>
Date: Sun, 8 Mar 2020 12:27:03 -0600
Subject: [PATCH 08/25] Remove Unused Commands

I know its a bad idea but at least we still have the deltas in case we need them.
---
 .../robot/commands/storage/StorageIntake.java | 63 -------------------
 .../commands/storage/StorageIntakeFinal.java  | 40 ------------
 .../robot/commands/storage/StorageOutake.java | 46 --------------
 .../commands/storage/StoragePositionPID.java  | 52 ---------------
 .../commands/storage/StoragePrepIntake.java   | 62 ------------------
 5 files changed, 263 deletions(-)
 delete mode 100644 src/main/java/frc4388/robot/commands/storage/StorageIntake.java
 delete mode 100644 src/main/java/frc4388/robot/commands/storage/StorageIntakeFinal.java
 delete mode 100644 src/main/java/frc4388/robot/commands/storage/StorageOutake.java
 delete mode 100644 src/main/java/frc4388/robot/commands/storage/StoragePositionPID.java
 delete mode 100644 src/main/java/frc4388/robot/commands/storage/StoragePrepIntake.java

diff --git a/src/main/java/frc4388/robot/commands/storage/StorageIntake.java b/src/main/java/frc4388/robot/commands/storage/StorageIntake.java
deleted file mode 100644
index 47769c4..0000000
--- a/src/main/java/frc4388/robot/commands/storage/StorageIntake.java
+++ /dev/null
@@ -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;
-  }
-}
diff --git a/src/main/java/frc4388/robot/commands/storage/StorageIntakeFinal.java b/src/main/java/frc4388/robot/commands/storage/StorageIntakeFinal.java
deleted file mode 100644
index 0d8c447..0000000
--- a/src/main/java/frc4388/robot/commands/storage/StorageIntakeFinal.java
+++ /dev/null
@@ -1,40 +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;
-
-public class StorageIntakeFinal extends CommandBase {
-  /**
-   * Creates a new StorageIntakeFinal.
-   */
-  public StorageIntakeFinal() {
-    // 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() {
-  }
-
-  // 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;
-  }
-}
diff --git a/src/main/java/frc4388/robot/commands/storage/StorageOutake.java b/src/main/java/frc4388/robot/commands/storage/StorageOutake.java
deleted file mode 100644
index 658d8e5..0000000
--- a/src/main/java/frc4388/robot/commands/storage/StorageOutake.java
+++ /dev/null
@@ -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;
-  }
-}
diff --git a/src/main/java/frc4388/robot/commands/storage/StoragePositionPID.java b/src/main/java/frc4388/robot/commands/storage/StoragePositionPID.java
deleted file mode 100644
index 2a27b41..0000000
--- a/src/main/java/frc4388/robot/commands/storage/StoragePositionPID.java
+++ /dev/null
@@ -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;
-  }
-}
diff --git a/src/main/java/frc4388/robot/commands/storage/StoragePrepIntake.java b/src/main/java/frc4388/robot/commands/storage/StoragePrepIntake.java
deleted file mode 100644
index b9a9cf1..0000000
--- a/src/main/java/frc4388/robot/commands/storage/StoragePrepIntake.java
+++ /dev/null
@@ -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;
-  }
-}

From 32152080f2c033976134155998d19d03966c8680 Mon Sep 17 00:00:00 2001
From: "Keenan D. Buckley" <keenandbuckley@protonmail.com>
Date: Sun, 8 Mar 2020 12:35:14 -0600
Subject: [PATCH 09/25] Put Storage Beam Breaks on SmartDashboard

---
 src/main/java/frc4388/robot/subsystems/Storage.java | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java
index 175e499..6e30908 100644
--- a/src/main/java/frc4388/robot/subsystems/Storage.java
+++ b/src/main/java/frc4388/robot/subsystems/Storage.java
@@ -53,10 +53,10 @@ public class Storage extends SubsystemBase {
 
   @Override
   public void periodic() {
-    //SmartDashboard.putBoolean("Beam 0", m_beamSensors[0].get());
-    //SmartDashboard.putBoolean("Beam 1", m_beamSensors[1].get());
-    //SmartDashboard.putBoolean("Beam 0", m_beamSensors[0].get());
-    //SmartDashboard.putBoolean("Beam 1", m_beamSensors[1].get());
+    SmartDashboard.putBoolean("Intake Beam", getBeamIntake());
+    SmartDashboard.putBoolean("Storage Beam", getBeamStorage());
+    SmartDashboard.putBoolean("Upper Beam", getBeamUseless());
+    SmartDashboard.putBoolean("Shooter Beam", getBeamShooter());
   }
 
   /**

From 0db12332dcd38e519f6612ae115e9c5c63dfd9fc Mon Sep 17 00:00:00 2001
From: "Keenan D. Buckley" <keenandbuckley@protonmail.com>
Date: Sun, 8 Mar 2020 13:33:57 -0600
Subject: [PATCH 10/25] Improve Storage Commands

---
 src/main/java/frc4388/robot/Constants.java    |  2 +-
 .../robot/commands/storage/ManageStorage.java | 25 +++++++++++++------
 .../robot/commands/storage/StorageFire.java   |  1 -
 .../robot/commands/storage/StoragePrep.java   | 15 +++--------
 4 files changed, 22 insertions(+), 21 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 8ff656b..44e8f01 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -162,7 +162,7 @@ public final class Constants {
         public static final double STORAGE_PARTIAL_BALL = 2;
         public static final double STORAGE_FULL_BALL = 7;
         public static final double STORAGE_SPEED = 0.5;
-        public static final double STORAGE_TIMEOUT = 2000;
+        public static final double STORAGE_TIMEOUT = 3000;
 
         /* Storage Characteristics */
         public static final double MOTOR_ROTS_PER_STORAGE_ROT = 1; //For the first storage belt
diff --git a/src/main/java/frc4388/robot/commands/storage/ManageStorage.java b/src/main/java/frc4388/robot/commands/storage/ManageStorage.java
index ea4aa19..38a7670 100644
--- a/src/main/java/frc4388/robot/commands/storage/ManageStorage.java
+++ b/src/main/java/frc4388/robot/commands/storage/ManageStorage.java
@@ -15,6 +15,9 @@ 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 isBallInIntake = false;
   boolean isBallInStorage = false;
@@ -46,6 +49,10 @@ public class ManageStorage extends CommandBase {
     isBallInShooter = !m_storage.getBeamShooter();
 
     m_isStorageEmpty = !isBallInStorage;
+
+    if (m_storageMode == StorageMode.RESET) {
+      m_resetStartTime = System.currentTimeMillis();
+    }
   }
 
   // Called every time the scheduler runs while the command is scheduled.
@@ -76,13 +83,17 @@ public class ManageStorage extends CommandBase {
    * storage sensor and the intake ball has taken its place.
    */
   private void runIntake() {
-    m_storage.runStorage(StorageConstants.STORAGE_SPEED);
+    if (!isBallInShooter) { // Intake balls as long as there is not a ball at the shooter
+      m_storage.runStorage(StorageConstants.STORAGE_SPEED);
 
-    if (!m_isStorageEmpty && !isBallInStorage) {
-      m_isStorageEmpty = true;
-    }
-    if (m_isStorageEmpty && isBallInStorage) {
-      m_isStorageEmpty = false;
+      if (!m_isStorageEmpty && !isBallInStorage) { // If ball moves out of storage, set storage to empty
+        m_isStorageEmpty = true;
+      }
+      if (m_isStorageEmpty && 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;
     }
   }
@@ -108,7 +119,7 @@ public class ManageStorage extends CommandBase {
   private void runReset() {
     m_storage.runStorage(-StorageConstants.STORAGE_SPEED);
 
-    if (isBallInIntake) {
+    if (isBallInIntake || (m_resetStartTime + StorageConstants.STORAGE_TIMEOUT) < System.currentTimeMillis()) {
       m_storageMode = StorageMode.INTAKE;
     }
     m_isStorageEmpty = !isBallInStorage;
diff --git a/src/main/java/frc4388/robot/commands/storage/StorageFire.java b/src/main/java/frc4388/robot/commands/storage/StorageFire.java
index 6413c1e..61dda95 100644
--- a/src/main/java/frc4388/robot/commands/storage/StorageFire.java
+++ b/src/main/java/frc4388/robot/commands/storage/StorageFire.java
@@ -35,7 +35,6 @@ public class StorageFire extends CommandBase {
   // 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.
diff --git a/src/main/java/frc4388/robot/commands/storage/StoragePrep.java b/src/main/java/frc4388/robot/commands/storage/StoragePrep.java
index 00c9bb5..4e73870 100644
--- a/src/main/java/frc4388/robot/commands/storage/StoragePrep.java
+++ b/src/main/java/frc4388/robot/commands/storage/StoragePrep.java
@@ -7,7 +7,6 @@
 
 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;
@@ -33,12 +32,7 @@ public class StoragePrep extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if (m_storage.getBeamShooter()){
-      //m_storage.runStorage(StorageConstants.STORAGE_SPEED);
-    }
-    else{
-      m_storage.runStorage(0);
-    }
+    m_storage.runStorage(StorageConstants.STORAGE_SPEED);
   }
 
   // Called once the command ends or is interrupted.
@@ -49,13 +43,10 @@ public class StoragePrep 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;
+    if (!m_storage.getBeamShooter() || (startTime + StorageConstants.STORAGE_TIMEOUT) < System.currentTimeMillis()) {
       return true;
     } else {
-      m_storage.m_isStorageReadyToFire = false;
       return false;
-    }*/
-    return true;
+    }
   }
 }

From cc232cf574a9f070eea2c09ddad0fac7bf3fe18e Mon Sep 17 00:00:00 2001
From: "Keenan D. Buckley" <keenandbuckley@protonmail.com>
Date: Sun, 8 Mar 2020 13:50:39 -0600
Subject: [PATCH 11/25] Integrate Storage Commands into Fire Commands

---
 src/main/java/frc4388/robot/Constants.java         | 14 ++++++++------
 .../robot/commands/shooter/ShootFireGroup.java     |  4 ++--
 .../robot/commands/storage/StoragePrep.java        |  2 ++
 3 files changed, 12 insertions(+), 8 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 44e8f01..cbf0e96 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -105,12 +105,6 @@ public final class Constants {
         public static final double TICKS_PER_INCH_LOW = TICKS_PER_WHEEL_REV_LOW/INCHES_PER_WHEEL_REV;
         public static final double INCHES_PER_TICK_LOW = 1/TICKS_PER_INCH_LOW;
     }
-    
-    public static final class IntakeConstants {;
-        public static final double EXTENDER_SPEED = 0.3;
-        public static final int INTAKE_SPARK_ID = 12;
-        public static final int EXTENDER_SPARK_ID = 13;
-    }
   
     public static final class ShooterConstants {
         /* Motor IDs */
@@ -156,6 +150,14 @@ public final class Constants {
     public static final class LevelerConstants {
         public static final int LEVELER_CAN_ID = 15;
     }
+
+    public static final class IntakeConstants {;
+        public static final double EXTENDER_SPEED = 0.3;
+        public static final double INTAKE_SPEED = 1.0;
+
+        public static final int INTAKE_SPARK_ID = 12;
+        public static final int EXTENDER_SPARK_ID = 13;
+    }
   
     public static final class StorageConstants {
         public static final int STORAGE_CAN_ID = 11;
diff --git a/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java b/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java
index 02d5799..ce18d4b 100644
--- a/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java
+++ b/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java
@@ -29,8 +29,8 @@ public class ShootFireGroup extends ParallelRaceGroup {
     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 StorageFire(m_storage)
+      new TrackTarget(m_shooterAim),
+      new StorageFire(m_storage)
     );
   }
 }
\ No newline at end of file
diff --git a/src/main/java/frc4388/robot/commands/storage/StoragePrep.java b/src/main/java/frc4388/robot/commands/storage/StoragePrep.java
index 4e73870..0f39d25 100644
--- a/src/main/java/frc4388/robot/commands/storage/StoragePrep.java
+++ b/src/main/java/frc4388/robot/commands/storage/StoragePrep.java
@@ -44,8 +44,10 @@ public class StoragePrep extends CommandBase {
   @Override
   public boolean isFinished() {
     if (!m_storage.getBeamShooter() || (startTime + StorageConstants.STORAGE_TIMEOUT) < System.currentTimeMillis()) {
+      m_storage.m_isStorageReadyToFire = true;
       return true;
     } else {
+      m_storage.m_isStorageReadyToFire = false;
       return false;
     }
   }

From 5de8abf176bd39a2cfc700b15d74725bc9d681fe Mon Sep 17 00:00:00 2001
From: "Keenan D. Buckley" <keenandbuckley@protonmail.com>
Date: Sun, 8 Mar 2020 14:23:27 -0600
Subject: [PATCH 12/25] Integrate Storage Code with Firing Code

---
 src/main/java/frc4388/robot/Constants.java    |  2 +-
 .../java/frc4388/robot/RobotContainer.java    | 20 +++++++++++--------
 .../commands/shooter/ShootFireGroup.java      | 11 +++++-----
 .../commands/shooter/ShootPrepGroup.java      |  4 ++--
 .../robot/commands/storage/ManageStorage.java |  2 +-
 .../robot/commands/storage/StorageFire.java   |  5 ++++-
 .../robot/commands/storage/StoragePrep.java   | 10 +++++++---
 7 files changed, 33 insertions(+), 21 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index cbf0e96..cd211c6 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -163,7 +163,7 @@ public final class Constants {
         public static final int STORAGE_CAN_ID = 11;
         public static final double STORAGE_PARTIAL_BALL = 2;
         public static final double STORAGE_FULL_BALL = 7;
-        public static final double STORAGE_SPEED = 0.5;
+        public static final double STORAGE_SPEED = 1.0;
         public static final double STORAGE_TIMEOUT = 3000;
 
         /* Storage Characteristics */
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 7588412..b7a17ce 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -42,6 +42,8 @@ import frc4388.robot.commands.shooter.ShootFullGroup;
 import frc4388.robot.commands.shooter.ShootPrepGroup;
 import frc4388.robot.commands.shooter.TrackTarget;
 import frc4388.robot.commands.shooter.TrimShooter;
+import frc4388.robot.commands.storage.ManageStorage;
+import frc4388.robot.commands.storage.ManageStorage.StorageMode;
 import frc4388.robot.subsystems.Camera;
 import frc4388.robot.subsystems.Climber;
 import frc4388.robot.subsystems.Drive;
@@ -169,17 +171,19 @@ public class RobotContainer {
         /* Operator Buttons */
         // shoots until released
         new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
-            //.whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false)
+            .whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false);
+            //.whenReleased(new ManageStorage(m_robotStorage, StorageMode.RESET));
             //.whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
-            .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(-1), m_robotStorage))
-            .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage));
+            //.whenPressed(new RunCommand(() -> m_robotStorage.runStorage(-1), m_robotStorage))
+            //.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage));
 
         // shoots one ball
         new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON)
-            //.whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false)
+            .whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false);
+            //.whenReleased(new ManageStorage(m_robotStorage, StorageMode.RESET));
             //.whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
-            .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(1), m_robotStorage))
-            .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage));
+            //.whenPressed(new RunCommand(() -> m_robotStorage.runStorage(1), m_robotStorage))
+            //.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage));
 
         // extends or retracts the extender
         new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
@@ -199,7 +203,6 @@ public class RobotContainer {
         new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
             .whileHeld(new TrackTarget(m_robotShooterAim))
             .whileHeld(new RunCommand(() -> m_robotShooterHood.runAngleAdjustPID(m_robotShooterHood.addFireAngle())))
-            //.whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage))
             .whenReleased(new InstantCommand(() -> m_robotLime.limeOff()));
             //.whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(13000)));
             //.whileHeld(new HoldTarget(m_robotShooter, m_robotShooterAim))
@@ -226,7 +229,8 @@ public class RobotContainer {
 
         //Run drum
         new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON)
-            .whileHeld(new ShootFireGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false)
+            .whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false)
+            .whenReleased(new ManageStorage(m_robotStorage, StorageMode.RESET))
             .whenReleased(new InstantCommand(() -> m_robotLime.limeOff()));
     }
 
diff --git a/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java b/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java
index ce18d4b..40cbaa4 100644
--- a/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java
+++ b/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java
@@ -7,6 +7,7 @@
 
 package frc4388.robot.commands.shooter;
 
+import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
 import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import frc4388.robot.commands.storage.StorageFire;
@@ -18,7 +19,7 @@ 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 {
+public class ShootFireGroup extends ParallelDeadlineGroup {
   /**
    * Fires the shooter
    * @param m_shooter The Shooter subsytem
@@ -26,11 +27,11 @@ public class ShootFireGroup extends ParallelRaceGroup {
    * @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),
+    super(
+      new StorageFire(m_storage),
       new TrackTarget(m_shooterAim),
-      new StorageFire(m_storage)
+      new ShooterVelocityControlPID(m_shooter),
+      new HoodPositionPID(m_shooterHood)
     );
   }
 }
\ No newline at end of file
diff --git a/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java b/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java
index d919d32..b328f71 100644
--- a/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java
+++ b/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java
@@ -27,10 +27,10 @@ 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 StoragePrep(m_storage),
       new TrackTarget(m_shooterAim),
       new ShooterVelocityControlPID(m_shooter),
-      new HoodPositionPID(m_shooterHood),
-      new StoragePrep(m_storage)
+      new HoodPositionPID(m_shooterHood)
     );
   }
 }
diff --git a/src/main/java/frc4388/robot/commands/storage/ManageStorage.java b/src/main/java/frc4388/robot/commands/storage/ManageStorage.java
index 38a7670..4e019c7 100644
--- a/src/main/java/frc4388/robot/commands/storage/ManageStorage.java
+++ b/src/main/java/frc4388/robot/commands/storage/ManageStorage.java
@@ -27,7 +27,7 @@ public class ManageStorage extends CommandBase {
   /* Used for intaking a ball. Keeps track off when the 2nd ball in storage has moved */
   boolean m_isStorageEmpty = true;
 
-  enum StorageMode{IDLE, INTAKE, RESET};
+  public enum StorageMode{IDLE, INTAKE, RESET};
   StorageMode m_storageMode = StorageMode.IDLE;
   
   /**
diff --git a/src/main/java/frc4388/robot/commands/storage/StorageFire.java b/src/main/java/frc4388/robot/commands/storage/StorageFire.java
index 61dda95..261975e 100644
--- a/src/main/java/frc4388/robot/commands/storage/StorageFire.java
+++ b/src/main/java/frc4388/robot/commands/storage/StorageFire.java
@@ -14,7 +14,7 @@ import frc4388.robot.subsystems.Storage;
 public class StorageFire extends CommandBase {
   Storage m_storage;
   /**
-   * Runs the Storage at a speed
+   * Runs the Storage until shoot beam is empty, then ends
    * @param storageSub The Storage subsytem
    */
   public StorageFire(Storage storageSub) {
@@ -40,6 +40,9 @@ public class StorageFire extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
+    if (m_storage.getBeamShooter()) {
+      return true;
+    }
     return false;
   }
 }
diff --git a/src/main/java/frc4388/robot/commands/storage/StoragePrep.java b/src/main/java/frc4388/robot/commands/storage/StoragePrep.java
index 0f39d25..cee4b16 100644
--- a/src/main/java/frc4388/robot/commands/storage/StoragePrep.java
+++ b/src/main/java/frc4388/robot/commands/storage/StoragePrep.java
@@ -32,7 +32,11 @@ public class StoragePrep extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    m_storage.runStorage(StorageConstants.STORAGE_SPEED);
+    if (!m_storage.m_isStorageReadyToFire) {
+      m_storage.runStorage(StorageConstants.STORAGE_SPEED);
+    } else {
+      m_storage.runStorage(0);
+    }
   }
 
   // Called once the command ends or is interrupted.
@@ -45,10 +49,10 @@ public class StoragePrep extends CommandBase {
   public boolean isFinished() {
     if (!m_storage.getBeamShooter() || (startTime + StorageConstants.STORAGE_TIMEOUT) < System.currentTimeMillis()) {
       m_storage.m_isStorageReadyToFire = true;
-      return true;
     } else {
       m_storage.m_isStorageReadyToFire = false;
-      return false;
     }
+
+    return false;
   }
 }

From 09af79f1024f6320d8e2a1f390647ebdeb329e33 Mon Sep 17 00:00:00 2001
From: "Keenan D. Buckley" <keenandbuckley@protonmail.com>
Date: Sun, 8 Mar 2020 14:26:02 -0600
Subject: [PATCH 13/25] Close the File Readers

To suppress the warnings and practice good IO.
---
 src/main/java/frc4388/utility/ShooterTables.java | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/src/main/java/frc4388/utility/ShooterTables.java b/src/main/java/frc4388/utility/ShooterTables.java
index 85244ce..d8a4ac4 100644
--- a/src/main/java/frc4388/utility/ShooterTables.java
+++ b/src/main/java/frc4388/utility/ShooterTables.java
@@ -80,6 +80,9 @@ public class ShooterTables {
 
             m_angleLength = lineNum-1;
 
+            distanceReader.close();
+            angleReader.close();
+
         } catch (FileNotFoundException e) {
             e.printStackTrace();
         } catch (IOException e) {

From 2a2b594b9c38e44cb77ff70bf8273fe113464566 Mon Sep 17 00:00:00 2001
From: "Keenan D. Buckley" <keenandbuckley@protonmail.com>
Date: Sun, 8 Mar 2020 14:30:52 -0600
Subject: [PATCH 14/25] Change to reset everytime storage returns to the
 default command

---
 src/main/java/frc4388/robot/RobotContainer.java               | 4 ++--
 .../java/frc4388/robot/commands/storage/ManageStorage.java    | 1 +
 2 files changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index b7a17ce..17f28d0 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -126,7 +126,7 @@ public class RobotContainer {
         m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
         // runs the storage not
         //m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0), m_robotStorage));
-        m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0), m_robotStorage));
+        m_robotStorage.setDefaultCommand(new ManageStorage(m_robotStorage, StorageMode.IDLE));
         //m_robotLime.setDefaultCommand(new RunCommand(() -> m_robotLime.limeOff(), m_robotLime));
     }
 
@@ -230,7 +230,7 @@ public class RobotContainer {
         //Run drum
         new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON)
             .whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false)
-            .whenReleased(new ManageStorage(m_robotStorage, StorageMode.RESET))
+            //.whenReleased(new ManageStorage(m_robotStorage, StorageMode.RESET))
             .whenReleased(new InstantCommand(() -> m_robotLime.limeOff()));
     }
 
diff --git a/src/main/java/frc4388/robot/commands/storage/ManageStorage.java b/src/main/java/frc4388/robot/commands/storage/ManageStorage.java
index 4e019c7..0bf0519 100644
--- a/src/main/java/frc4388/robot/commands/storage/ManageStorage.java
+++ b/src/main/java/frc4388/robot/commands/storage/ManageStorage.java
@@ -128,6 +128,7 @@ public class ManageStorage extends CommandBase {
   // 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.

From 523be688cc1fe0355f9847178fcdabecb3d163ab Mon Sep 17 00:00:00 2001
From: "Keenan D. Buckley" <keenandbuckley@protonmail.com>
Date: Sun, 8 Mar 2020 14:35:46 -0600
Subject: [PATCH 15/25] Don't go into Intake mode if reset times out

---
 .../java/frc4388/robot/commands/storage/ManageStorage.java    | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc4388/robot/commands/storage/ManageStorage.java b/src/main/java/frc4388/robot/commands/storage/ManageStorage.java
index 0bf0519..4599017 100644
--- a/src/main/java/frc4388/robot/commands/storage/ManageStorage.java
+++ b/src/main/java/frc4388/robot/commands/storage/ManageStorage.java
@@ -119,8 +119,10 @@ public class ManageStorage extends CommandBase {
   private void runReset() {
     m_storage.runStorage(-StorageConstants.STORAGE_SPEED);
 
-    if (isBallInIntake || (m_resetStartTime + StorageConstants.STORAGE_TIMEOUT) < System.currentTimeMillis()) {
+    if (isBallInIntake) {
       m_storageMode = StorageMode.INTAKE;
+    } else if (m_resetStartTime + StorageConstants.STORAGE_TIMEOUT < System.currentTimeMillis()) {
+      m_storageMode = StorageMode.IDLE;
     }
     m_isStorageEmpty = !isBallInStorage;
   }

From 9ad9b76cc7e2d2017176f54d4e83ce2a29cd149e Mon Sep 17 00:00:00 2001
From: "Keenan D. Buckley" <keenandbuckley@protonmail.com>
Date: Sun, 8 Mar 2020 14:53:51 -0600
Subject: [PATCH 16/25] Add PID Versions of Storage Commands

---
 .../commands/storage/ManageStoragePID.java    | 145 ++++++++++++++++++
 .../commands/storage/StorageFirePID.java      |  52 +++++++
 2 files changed, 197 insertions(+)
 create mode 100644 src/main/java/frc4388/robot/commands/storage/ManageStoragePID.java
 create mode 100644 src/main/java/frc4388/robot/commands/storage/StorageFirePID.java

diff --git a/src/main/java/frc4388/robot/commands/storage/ManageStoragePID.java b/src/main/java/frc4388/robot/commands/storage/ManageStoragePID.java
new file mode 100644
index 0000000..6ed4fd3
--- /dev/null
+++ b/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;
+  }
+}
\ No newline at end of file
diff --git a/src/main/java/frc4388/robot/commands/storage/StorageFirePID.java b/src/main/java/frc4388/robot/commands/storage/StorageFirePID.java
new file mode 100644
index 0000000..2ee1ccc
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/storage/StorageFirePID.java
@@ -0,0 +1,52 @@
+/*----------------------------------------------------------------------------*/
+/* 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 StorageFirePID extends CommandBase {
+  Storage m_storage;
+  double m_intakeStartPos;
+  
+  /**
+   * Runs the Storage until shoot beam is empty, then ends
+   * @param storageSub The Storage subsytem
+   */
+  public StorageFirePID(Storage storageSub) {
+    m_storage = storageSub;
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    m_intakeStartPos = m_storage.getEncoderPosInches();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_storage.runStoragePositionPID(m_intakeStartPos + 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() {
+    double error = (m_intakeStartPos + StorageConstants.STORAGE_FULL_BALL) - m_storage.getEncoderPosInches();
+      if (m_storage.getEncoderVel() == 0 && Math.abs(error) < 0.5) {
+        return true;
+      }
+    return false;
+  }
+}

From e7d984241622f3b4e6f285c0146940c4506b1de9 Mon Sep 17 00:00:00 2001
From: "Keenan D. Buckley" <keenandbuckley@protonmail.com>
Date: Sun, 8 Mar 2020 14:54:15 -0600
Subject: [PATCH 17/25] Rename bad variables in Storage Commands

---
 .../robot/commands/storage/ManageStorage.java | 46 +++++++++----------
 .../robot/commands/storage/StoragePrep.java   |  6 +--
 .../frc4388/robot/subsystems/Storage.java     |  8 ++++
 3 files changed, 34 insertions(+), 26 deletions(-)

diff --git a/src/main/java/frc4388/robot/commands/storage/ManageStorage.java b/src/main/java/frc4388/robot/commands/storage/ManageStorage.java
index 4599017..68cfd71 100644
--- a/src/main/java/frc4388/robot/commands/storage/ManageStorage.java
+++ b/src/main/java/frc4388/robot/commands/storage/ManageStorage.java
@@ -19,10 +19,10 @@ public class ManageStorage extends CommandBase {
   long m_resetStartTime;
 
   /* Keeps track of which beam breaks are pressed */
-  boolean isBallInIntake = false;
-  boolean isBallInStorage = false;
-  boolean isBallInUseless = false;
-  boolean isBallInShooter = false;
+  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;
@@ -43,12 +43,12 @@ public class ManageStorage extends CommandBase {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    isBallInIntake = !m_storage.getBeamIntake();
-    isBallInStorage = !m_storage.getBeamStorage();
-    isBallInUseless = !m_storage.getBeamUseless();
-    isBallInShooter = !m_storage.getBeamShooter();
+    m_isBallInIntake = !m_storage.getBeamIntake();
+    m_isBallInStorage = !m_storage.getBeamStorage();
+    m_isBallInUseless = !m_storage.getBeamUseless();
+    m_isBallInShooter = !m_storage.getBeamShooter();
 
-    m_isStorageEmpty = !isBallInStorage;
+    m_isStorageEmpty = !m_isBallInStorage;
 
     if (m_storageMode == StorageMode.RESET) {
       m_resetStartTime = System.currentTimeMillis();
@@ -58,15 +58,15 @@ public class ManageStorage extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    isBallInIntake = !m_storage.getBeamIntake();
-    isBallInStorage = !m_storage.getBeamStorage();
-    isBallInUseless = !m_storage.getBeamUseless();
-    isBallInShooter = !m_storage.getBeamShooter();
+    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!", isBallInIntake);
-    SmartDashboard.putBoolean("!Ball Storage!", isBallInStorage);
-    SmartDashboard.putBoolean("!Ball Shooter!", isBallInShooter);
+    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();
@@ -83,13 +83,13 @@ public class ManageStorage extends CommandBase {
    * storage sensor and the intake ball has taken its place.
    */
   private void runIntake() {
-    if (!isBallInShooter) { // Intake balls as long as there is not a ball at the shooter
+    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 && !isBallInStorage) { // If ball moves out of storage, set storage to empty
+      if (!m_isStorageEmpty && !m_isBallInStorage) { // If ball moves out of storage, set storage to empty
         m_isStorageEmpty = true;
       }
-      if (m_isStorageEmpty && isBallInStorage) { // If Ball moves into storage, set storage to full and swtich to idle mode
+      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;
       }
@@ -105,10 +105,10 @@ public class ManageStorage extends CommandBase {
   private void runIdle() {
     m_storage.runStorage(0);
 
-    if (isBallInIntake) {
+    if (m_isBallInIntake) {
       m_storageMode = StorageMode.INTAKE;
     }
-    m_isStorageEmpty = !isBallInStorage;
+    m_isStorageEmpty = !m_isBallInStorage;
   }
 
   /**
@@ -119,12 +119,12 @@ public class ManageStorage extends CommandBase {
   private void runReset() {
     m_storage.runStorage(-StorageConstants.STORAGE_SPEED);
 
-    if (isBallInIntake) {
+    if (m_isBallInIntake) {
       m_storageMode = StorageMode.INTAKE;
     } else if (m_resetStartTime + StorageConstants.STORAGE_TIMEOUT < System.currentTimeMillis()) {
       m_storageMode = StorageMode.IDLE;
     }
-    m_isStorageEmpty = !isBallInStorage;
+    m_isStorageEmpty = !m_isBallInStorage;
   }
 
   // Called once the command ends or is interrupted.
diff --git a/src/main/java/frc4388/robot/commands/storage/StoragePrep.java b/src/main/java/frc4388/robot/commands/storage/StoragePrep.java
index cee4b16..d42c9b9 100644
--- a/src/main/java/frc4388/robot/commands/storage/StoragePrep.java
+++ b/src/main/java/frc4388/robot/commands/storage/StoragePrep.java
@@ -13,7 +13,7 @@ import frc4388.robot.subsystems.Storage;
 
 public class StoragePrep extends CommandBase {
   Storage m_storage;
-  double startTime;
+  double m_startTime;
   /**
    * Prepares the Storage for aiming
    * @param storeSub The Storage subsystem
@@ -26,7 +26,7 @@ public class StoragePrep 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.
@@ -47,7 +47,7 @@ public class StoragePrep extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (!m_storage.getBeamShooter() || (startTime + StorageConstants.STORAGE_TIMEOUT) < System.currentTimeMillis()) {
+    if (!m_storage.getBeamShooter() || (m_startTime + StorageConstants.STORAGE_TIMEOUT) < System.currentTimeMillis()) {
       m_storage.m_isStorageReadyToFire = true;
     } else {
       m_storage.m_isStorageReadyToFire = false;
diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java
index 6e30908..403de25 100644
--- a/src/main/java/frc4388/robot/subsystems/Storage.java
+++ b/src/main/java/frc4388/robot/subsystems/Storage.java
@@ -95,6 +95,14 @@ public class Storage extends SubsystemBase {
     return m_encoder.getPosition();
   }
 
+  public double getEncoderPosInches(){
+    return motorRotsToInches(getEncoderPos());
+  }
+
+  public double getEncoderVel(){
+    return m_encoder.getVelocity();
+  }
+
   /**
    * @param motorRots
    * @return inches

From 8ba12987d3647e5448d7f123bf2c91f753c1b045 Mon Sep 17 00:00:00 2001
From: "Keenan D. Buckley" <keenandbuckley@protonmail.com>
Date: Sun, 8 Mar 2020 15:09:19 -0600
Subject: [PATCH 18/25] ewwww

fixed ugly indents
---
 .../java/frc4388/robot/commands/storage/StorageFirePID.java | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc4388/robot/commands/storage/StorageFirePID.java b/src/main/java/frc4388/robot/commands/storage/StorageFirePID.java
index 2ee1ccc..6026452 100644
--- a/src/main/java/frc4388/robot/commands/storage/StorageFirePID.java
+++ b/src/main/java/frc4388/robot/commands/storage/StorageFirePID.java
@@ -44,9 +44,9 @@ public class StorageFirePID extends CommandBase {
   @Override
   public boolean isFinished() {
     double error = (m_intakeStartPos + StorageConstants.STORAGE_FULL_BALL) - m_storage.getEncoderPosInches();
-      if (m_storage.getEncoderVel() == 0 && Math.abs(error) < 0.5) {
-        return true;
-      }
+    if (m_storage.getEncoderVel() == 0 && Math.abs(error) < 0.5) {
+      return true;
+    }
     return false;
   }
 }

From 8c2c6f1eced03ae0bc533890e0db7bff4ea46b0b Mon Sep 17 00:00:00 2001
From: Kyra <101209@psdschools.org>
Date: Mon, 9 Mar 2020 17:56:52 -0600
Subject: [PATCH 19/25] button box fix LED

add funcion to increment led
---
 .../java/frc4388/robot/subsystems/LED.java    | 35 ++++++++++++++++++-
 1 file changed, 34 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java
index cbad21c..a86e369 100644
--- a/src/main/java/frc4388/robot/subsystems/LED.java
+++ b/src/main/java/frc4388/robot/subsystems/LED.java
@@ -17,6 +17,7 @@ import frc4388.utility.LEDPatterns;
 public class LED extends SubsystemBase {
 
   public static float currentLED;
+  public static float defaultLED;
   public static Spark LEDController;
 
   /**
@@ -25,7 +26,8 @@ public class LED extends SubsystemBase {
    */
   public LED(){
     LEDController = new Spark(LEDConstants.LED_SPARK_ID);
-    setPattern(LEDConstants.DEFAULT_PATTERN);
+    defaultLED = LEDConstants.DEFAULT_PATTERN.getValue();
+    runDefaultLED();
     LEDController.set(currentLED);
     System.err.println("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good.");
   }
@@ -38,6 +40,37 @@ public class LED extends SubsystemBase {
     LEDController.set(currentLED);
   }
 
+  /**
+   * 
+   */
+  public void runDefaultLED() {
+    setPattern(defaultLED);
+  }
+
+  /**
+   * Changes the default LED by an amount
+   * @param amount the amount to increment led by
+   */
+  public void incrementLED(float amount) {
+    defaultLED += amount;
+    if (defaultLED > 1) {
+      defaultLED -= 2;
+    }
+    if (defaultLED < -1) {
+      defaultLED += 2;
+    }
+  }
+
+  /**
+   * Sets the current LED pattern. This method should only be run
+   * whenever you want to change the current LED.
+   * @param pattern LEDPattern to set the Blinkin to.
+   */
+  public void setPattern(float pattern){
+    currentLED = pattern;
+    LEDController.set(pattern);
+  }
+
   /**
    * Sets the current LED pattern. This method should only be run
    * whenever you want to change the current LED.

From b182cd74f960448f87fcf0fe46d2e97c66820947 Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Mon, 9 Mar 2020 19:07:51 -0600
Subject: [PATCH 20/25] Debug Storage

---
 src/main/java/frc4388/robot/Constants.java    |  8 ++--
 .../java/frc4388/robot/RobotContainer.java    | 16 ++++---
 .../robot/commands/InterruptSubystem.java     | 42 +++++++++++++++++++
 .../robot/commands/shooter/PrepChecker.java   |  2 +-
 .../commands/shooter/ShootPrepGroup.java      |  2 +-
 .../robot/commands/storage/StoragePrep.java   | 10 ++---
 6 files changed, 61 insertions(+), 19 deletions(-)
 create mode 100644 src/main/java/frc4388/robot/commands/InterruptSubystem.java

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index cd211c6..4782ff4 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -171,10 +171,10 @@ public final class Constants {
         public static final double INCHES_PER_STORAGE_ROT = 1; //Circumference of the first storage belt
 
         /* Ball Indexes */
-        public static final int BEAM_SENSOR_SHOOTER = 1;
-        public static final int BEAM_SENSOR_USELESS = 2;
-        public static final int BEAM_SENSOR_STORAGE = 3;
-        public static final int BEAM_SENSOR_INTAKE = 4;
+        public static final int BEAM_SENSOR_SHOOTER = 11;
+        public static final int BEAM_SENSOR_USELESS = 12;
+        public static final int BEAM_SENSOR_STORAGE = 13;
+        public static final int BEAM_SENSOR_INTAKE = 14;
 
         /* PID Gains */
         public static final Gains STORAGE_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0);
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 17f28d0..fd9a56c 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -27,6 +27,7 @@ import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc4388.robot.Constants.DriveConstants;
 import frc4388.robot.Constants.OIConstants;
+import frc4388.robot.commands.InterruptSubystem;
 import frc4388.robot.commands.auto.AutoPath1FromCenter;
 import frc4388.robot.commands.auto.Wait;
 import frc4388.robot.commands.climber.DisengageRachet;
@@ -43,6 +44,7 @@ import frc4388.robot.commands.shooter.ShootPrepGroup;
 import frc4388.robot.commands.shooter.TrackTarget;
 import frc4388.robot.commands.shooter.TrimShooter;
 import frc4388.robot.commands.storage.ManageStorage;
+import frc4388.robot.commands.storage.StoragePrep;
 import frc4388.robot.commands.storage.ManageStorage.StorageMode;
 import frc4388.robot.subsystems.Camera;
 import frc4388.robot.subsystems.Climber;
@@ -171,19 +173,19 @@ public class RobotContainer {
         /* Operator Buttons */
         // shoots until released
         new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
-            .whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false);
+            //.whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false);
             //.whenReleased(new ManageStorage(m_robotStorage, StorageMode.RESET));
             //.whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
-            //.whenPressed(new RunCommand(() -> m_robotStorage.runStorage(-1), m_robotStorage))
-            //.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage));
+            .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(-1), m_robotStorage))
+            .whenReleased(new InterruptSubystem(m_robotStorage));
 
         // shoots one ball
         new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON)
-            .whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false);
+            //.whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false);
             //.whenReleased(new ManageStorage(m_robotStorage, StorageMode.RESET));
             //.whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
-            //.whenPressed(new RunCommand(() -> m_robotStorage.runStorage(1), m_robotStorage))
-            //.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage));
+            .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(1), m_robotStorage))
+            .whenReleased(new InterruptSubystem(m_robotStorage));
 
         // extends or retracts the extender
         new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
@@ -203,6 +205,8 @@ public class RobotContainer {
         new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
             .whileHeld(new TrackTarget(m_robotShooterAim))
             .whileHeld(new RunCommand(() -> m_robotShooterHood.runAngleAdjustPID(m_robotShooterHood.addFireAngle())))
+            //.whenPressed(new StoragePrep(m_robotStorage))
+            //.whenReleased(new InterruptSubystem(m_robotStorage))
             .whenReleased(new InstantCommand(() -> m_robotLime.limeOff()));
             //.whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(13000)));
             //.whileHeld(new HoldTarget(m_robotShooter, m_robotShooterAim))
diff --git a/src/main/java/frc4388/robot/commands/InterruptSubystem.java b/src/main/java/frc4388/robot/commands/InterruptSubystem.java
new file mode 100644
index 0000000..5eae65f
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/InterruptSubystem.java
@@ -0,0 +1,42 @@
+/*----------------------------------------------------------------------------*/
+/* 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;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class InterruptSubystem extends CommandBase {
+  /**
+   * Creates a new InterruptSubystem.
+   */
+  public InterruptSubystem(SubsystemBase subsystem) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(subsystem);
+  }
+
+  // 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 true;
+  }
+}
diff --git a/src/main/java/frc4388/robot/commands/shooter/PrepChecker.java b/src/main/java/frc4388/robot/commands/shooter/PrepChecker.java
index c0ef7a6..f76fd3e 100644
--- a/src/main/java/frc4388/robot/commands/shooter/PrepChecker.java
+++ b/src/main/java/frc4388/robot/commands/shooter/PrepChecker.java
@@ -68,7 +68,7 @@ public class PrepChecker extends CommandBase {
   @Override
   public boolean isFinished() {
     if (m_isDrumReady && m_isAimReady && m_isHoodReady && m_isStorageReady) {
-      return true;
+      //return true;
     }
     return false;
   }
diff --git a/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java b/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java
index b328f71..d669dd1 100644
--- a/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java
+++ b/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java
@@ -27,7 +27,7 @@ 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 StoragePrep(m_storage),
+      //new StoragePrep(m_storage),
       new TrackTarget(m_shooterAim),
       new ShooterVelocityControlPID(m_shooter),
       new HoodPositionPID(m_shooterHood)
diff --git a/src/main/java/frc4388/robot/commands/storage/StoragePrep.java b/src/main/java/frc4388/robot/commands/storage/StoragePrep.java
index d42c9b9..9fcee06 100644
--- a/src/main/java/frc4388/robot/commands/storage/StoragePrep.java
+++ b/src/main/java/frc4388/robot/commands/storage/StoragePrep.java
@@ -32,10 +32,12 @@ public class StoragePrep extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if (!m_storage.m_isStorageReadyToFire) {
+    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;
     }
   }
 
@@ -47,12 +49,6 @@ public class StoragePrep extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (!m_storage.getBeamShooter() || (m_startTime + StorageConstants.STORAGE_TIMEOUT) < System.currentTimeMillis()) {
-      m_storage.m_isStorageReadyToFire = true;
-    } else {
-      m_storage.m_isStorageReadyToFire = false;
-    }
-
     return false;
   }
 }

From 3f11fbd979d494bd161ce2f176b95c628acb249c Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Mon, 9 Mar 2020 20:05:47 -0600
Subject: [PATCH 21/25] Fix Shooter Velocity Control

& collect more data points
---
 src/main/deploy/Robot Data - Distances.csv                 | 7 ++++---
 .../robot/commands/shooter/ShooterVelocityControlPID.java  | 5 +----
 2 files changed, 5 insertions(+), 7 deletions(-)

diff --git a/src/main/deploy/Robot Data - Distances.csv b/src/main/deploy/Robot Data - Distances.csv
index b60700b..a63fa24 100644
--- a/src/main/deploy/Robot Data - Distances.csv	
+++ b/src/main/deploy/Robot Data - Distances.csv	
@@ -1,9 +1,10 @@
 Distance (in),Hood Ext. (u),Drum Velocity (u/ds),Center Displacment (deg)
-1,10,7000,2
-70,20,8000,2, double check
+1,10,8000,2
+70,21,8000,2, check
 100,24,9000,2
-145,28,10000,1,Add a 200
+145,28,10000,1
 230,31,12000,0,Add a 270
+246,32,15000,0
 320,32,17000,0,change 17000 and mark them lower
 331,33,17000,0
 397,33,16000,0
diff --git a/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java
index 215b6bd..75a6896 100644
--- a/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java
+++ b/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.

From 9e59e34314d0b22aefd41421ae960ad7954f1207 Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Mon, 9 Mar 2020 20:06:30 -0600
Subject: [PATCH 22/25] Remove Old Unusable Commands

---
 src/main/java/frc4388/robot/RobotContainer.java | 2 --
 1 file changed, 2 deletions(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 122edeb..1a0365e 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -38,8 +38,6 @@ import frc4388.robot.commands.drive.DriveWithJoystick;
 import frc4388.robot.commands.drive.TurnDegrees;
 import frc4388.robot.commands.intake.RunIntakeWithTriggers;
 import frc4388.robot.commands.shooter.CalibrateShooter;
-import frc4388.robot.commands.shooter.ShootFireGroup;
-import frc4388.robot.commands.shooter.ShootFullGroup;
 import frc4388.robot.commands.shooter.ShootPrepGroup;
 import frc4388.robot.commands.shooter.TrackTarget;
 import frc4388.robot.commands.shooter.TrimShooter;

From 725b2c00790c00fbbf3a0ceb33ed29d6632cf275 Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Mon, 9 Mar 2020 20:07:45 -0600
Subject: [PATCH 23/25] Remove Old Unusable Commands Pt 2

---
 .../robot/commands/shooter/PrepChecker.java   | 75 -------------------
 .../commands/shooter/ShootFireGroup.java      | 37 ---------
 .../commands/shooter/ShootFullGroup.java      | 32 --------
 .../commands/shooter/ShootPrepGroup.java      |  2 -
 .../robot/commands/storage/StorageFire.java   | 48 ------------
 .../commands/storage/StorageFirePID.java      | 52 -------------
 6 files changed, 246 deletions(-)
 delete mode 100644 src/main/java/frc4388/robot/commands/shooter/PrepChecker.java
 delete mode 100644 src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java
 delete mode 100644 src/main/java/frc4388/robot/commands/shooter/ShootFullGroup.java
 delete mode 100644 src/main/java/frc4388/robot/commands/storage/StorageFire.java
 delete mode 100644 src/main/java/frc4388/robot/commands/storage/StorageFirePID.java

diff --git a/src/main/java/frc4388/robot/commands/shooter/PrepChecker.java b/src/main/java/frc4388/robot/commands/shooter/PrepChecker.java
deleted file mode 100644
index f76fd3e..0000000
--- a/src/main/java/frc4388/robot/commands/shooter/PrepChecker.java
+++ /dev/null
@@ -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;
-  }
-}
diff --git a/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java b/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java
deleted file mode 100644
index 40cbaa4..0000000
--- a/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java
+++ /dev/null
@@ -1,37 +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.ParallelDeadlineGroup;
-import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
-import edu.wpi.first.wpilibj2.command.RunCommand;
-import frc4388.robot.commands.storage.StorageFire;
-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 ParallelDeadlineGroup {
-  /**
-   * 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) {
-    super(
-      new StorageFire(m_storage),
-      new TrackTarget(m_shooterAim),
-      new ShooterVelocityControlPID(m_shooter),
-      new HoodPositionPID(m_shooterHood)
-    );
-  }
-}
\ No newline at end of file
diff --git a/src/main/java/frc4388/robot/commands/shooter/ShootFullGroup.java b/src/main/java/frc4388/robot/commands/shooter/ShootFullGroup.java
deleted file mode 100644
index 8863636..0000000
--- a/src/main/java/frc4388/robot/commands/shooter/ShootFullGroup.java
+++ /dev/null
@@ -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)
-    );
-  }
-}
diff --git a/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java b/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java
index d669dd1..df92eb4 100644
--- a/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java
+++ b/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java
@@ -26,8 +26,6 @@ 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 StoragePrep(m_storage),
       new TrackTarget(m_shooterAim),
       new ShooterVelocityControlPID(m_shooter),
       new HoodPositionPID(m_shooterHood)
diff --git a/src/main/java/frc4388/robot/commands/storage/StorageFire.java b/src/main/java/frc4388/robot/commands/storage/StorageFire.java
deleted file mode 100644
index 261975e..0000000
--- a/src/main/java/frc4388/robot/commands/storage/StorageFire.java
+++ /dev/null
@@ -1,48 +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 StorageFire extends CommandBase {
-  Storage m_storage;
-  /**
-   * Runs the Storage until shoot beam is empty, then ends
-   * @param storageSub The Storage subsytem
-   */
-  public StorageFire(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) {
-  }
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    if (m_storage.getBeamShooter()) {
-      return true;
-    }
-    return false;
-  }
-}
diff --git a/src/main/java/frc4388/robot/commands/storage/StorageFirePID.java b/src/main/java/frc4388/robot/commands/storage/StorageFirePID.java
deleted file mode 100644
index 6026452..0000000
--- a/src/main/java/frc4388/robot/commands/storage/StorageFirePID.java
+++ /dev/null
@@ -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 StorageFirePID extends CommandBase {
-  Storage m_storage;
-  double m_intakeStartPos;
-  
-  /**
-   * Runs the Storage until shoot beam is empty, then ends
-   * @param storageSub The Storage subsytem
-   */
-  public StorageFirePID(Storage storageSub) {
-    m_storage = storageSub;
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {
-    m_intakeStartPos = m_storage.getEncoderPosInches();
-  }
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-    m_storage.runStoragePositionPID(m_intakeStartPos + 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() {
-    double error = (m_intakeStartPos + StorageConstants.STORAGE_FULL_BALL) - m_storage.getEncoderPosInches();
-    if (m_storage.getEncoderVel() == 0 && Math.abs(error) < 0.5) {
-      return true;
-    }
-    return false;
-  }
-}

From af6fc658a4a27786c4ee3b2a9bbeef816fbfa8ca Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Mon, 9 Mar 2020 20:15:41 -0600
Subject: [PATCH 24/25] Put Shooter Status on the SmartDashboard

---
 src/main/java/frc4388/robot/subsystems/Shooter.java    | 2 ++
 src/main/java/frc4388/robot/subsystems/ShooterAim.java | 2 ++
 2 files changed, 4 insertions(+)

diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java
index d8bb022..1b7a0c2 100644
--- a/src/main/java/frc4388/robot/subsystems/Shooter.java
+++ b/src/main/java/frc4388/robot/subsystems/Shooter.java
@@ -92,6 +92,8 @@ public class Shooter extends SubsystemBase {
     SmartDashboard.putNumber("Shooter Current", m_shooterFalcon.getSupplyCurrent());
 
     //SmartDashboard.putNumber("Hood Position", m_shooter.getAnglePosition());
+
+    SmartDashboard.putBoolean("Drum Ready" , m_isDrumReady);
     }
 
     catch(Exception e)
diff --git a/src/main/java/frc4388/robot/subsystems/ShooterAim.java b/src/main/java/frc4388/robot/subsystems/ShooterAim.java
index a34d59f..424175c 100644
--- a/src/main/java/frc4388/robot/subsystems/ShooterAim.java
+++ b/src/main/java/frc4388/robot/subsystems/ShooterAim.java
@@ -17,6 +17,7 @@ import com.revrobotics.CANSparkMax.SoftLimitDirection;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 import com.revrobotics.ControlType;
 
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.ShooterConstants;
 import frc4388.utility.Gains;
@@ -57,6 +58,7 @@ public class ShooterAim extends SubsystemBase {
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
+    SmartDashboard.putBoolean("Turret Aimed" , m_isAimReady);
   }
 
   /**

From ba715805d3767eeb91cdc362cf88f3364d1c8196 Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Tue, 10 Mar 2020 18:36:16 -0600
Subject: [PATCH 25/25] Added driver station config stuff

---
 .../driverStation/GOAT DRIVERSTATION.json     | 691 ++++++++++++++++++
 .../themes/Ridgbotics/ridgeboticstheme.css    |   9 +
 2 files changed, 700 insertions(+)
 create mode 100644 src/main/deploy/driverStation/GOAT DRIVERSTATION.json
 create mode 100644 src/main/deploy/driverStation/themes/Ridgbotics/ridgeboticstheme.css

diff --git a/src/main/deploy/driverStation/GOAT DRIVERSTATION.json b/src/main/deploy/driverStation/GOAT DRIVERSTATION.json
new file mode 100644
index 0000000..d28b08f
--- /dev/null
+++ b/src/main/deploy/driverStation/GOAT DRIVERSTATION.json	
@@ -0,0 +1,691 @@
+{
+  "tabPane": [
+    {
+      "title": "SmartDashboard",
+      "autoPopulate": true,
+      "autoPopulatePrefix": "SmartDashboard/",
+      "widgetPane": {
+        "gridSize": 128.0,
+        "showGrid": true,
+        "hgap": 16.0,
+        "vgap": 16.0,
+        "tiles": {
+          "0,0": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Text View",
+              "_source0": "network_table:///SmartDashboard/Shooter Current",
+              "_title": "Shooter Current"
+            }
+          },
+          "1,0": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Text View",
+              "_source0": "network_table:///SmartDashboard/Delta Time",
+              "_title": "Delta Time"
+            }
+          },
+          "2,0": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Text View",
+              "_source0": "network_table:///SmartDashboard/Time Seconds",
+              "_title": "Time Seconds"
+            }
+          },
+          "3,0": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Text View",
+              "_source0": "network_table:///SmartDashboard/Shooter Temp C",
+              "_title": "Shooter Temp C"
+            }
+          },
+          "4,0": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Text View",
+              "_source0": "network_table:///SmartDashboard/Auto?",
+              "_title": "Auto?"
+            }
+          },
+          "5,0": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Text View",
+              "_source0": "network_table:///SmartDashboard/Fire Angle CSV",
+              "_title": "Fire Angle CSV"
+            }
+          },
+          "6,0": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Text View",
+              "_source0": "network_table:///SmartDashboard/Drum Velocity",
+              "_title": "Drum Velocity"
+            }
+          },
+          "7,0": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Text View",
+              "_source0": "network_table:///SmartDashboard/Drum Velocity CSV",
+              "_title": "Drum Velocity CSV"
+            }
+          },
+          "8,0": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Text View",
+              "_source0": "network_table:///SmartDashboard/limelight_PipelineName",
+              "_title": "limelight_PipelineName"
+            }
+          },
+          "9,0": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Text View",
+              "_source0": "network_table:///SmartDashboard/limelight_Interface",
+              "_title": "limelight_Interface"
+            }
+          },
+          "0,1": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Text View",
+              "_source0": "network_table:///SmartDashboard/limelight_Stream",
+              "_title": "limelight_Stream"
+            }
+          },
+          "1,1": {
+            "size": [
+              2,
+              2
+            ],
+            "content": {
+              "_type": "Gyro",
+              "_source0": "network_table:///SmartDashboard/Pigeon Gyro",
+              "_title": "Pigeon Gyro",
+              "Visuals/Major tick spacing": 45.0,
+              "Visuals/Starting angle": 180.0,
+              "Visuals/Show tick mark ring": true,
+              "Visuals/Counter clockwise": false
+            }
+          },
+          "3,1": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Boolean Box",
+              "_source0": "network_table:///SmartDashboard/Climber Safety",
+              "_title": "Climber Safety",
+              "Colors/Color when true": "#7CFC00FF",
+              "Colors/Color when false": "#8B0000FF"
+            }
+          },
+          "4,1": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Boolean Box",
+              "_source0": "network_table:///SmartDashboard/Rachet",
+              "_title": "Rachet",
+              "Colors/Color when true": "#7CFC00FF",
+              "Colors/Color when false": "#8B0000FF"
+            }
+          },
+          "5,1": {
+            "size": [
+              3,
+              2
+            ],
+            "content": {
+              "_type": "Differential Drivebase",
+              "_source0": "network_table:///SmartDashboard/Drive Train",
+              "_title": "Drive Train",
+              "Wheels/Number of wheels": 4,
+              "Wheels/Wheel diameter": 80.0,
+              "Visuals/Show velocity vectors": true
+            }
+          },
+          "8,1": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Boolean Box",
+              "_source0": "network_table:///SmartDashboard/Gear Shift",
+              "_title": "Gear Shift",
+              "Colors/Color when true": "#7CFC00FF",
+              "Colors/Color when false": "#8B0000FF"
+            }
+          }
+        }
+      }
+    },
+    {
+      "title": "LiveWindow",
+      "autoPopulate": true,
+      "autoPopulatePrefix": "LiveWindow/",
+      "widgetPane": {
+        "gridSize": 128.0,
+        "showGrid": true,
+        "hgap": 16.0,
+        "vgap": 16.0,
+        "tiles": {
+          "0,0": {
+            "size": [
+              2,
+              4
+            ],
+            "content": {
+              "_type": "Subsystem Layout",
+              "_source0": "network_table:///LiveWindow/Climber",
+              "_title": "Climber",
+              "Layout/Label position": "BOTTOM",
+              "_children": []
+            }
+          },
+          "2,0": {
+            "size": [
+              2,
+              4
+            ],
+            "content": {
+              "_type": "Subsystem Layout",
+              "_source0": "network_table:///LiveWindow/Ungrouped",
+              "_title": "Ungrouped",
+              "Layout/Label position": "BOTTOM",
+              "_children": [
+                {
+                  "_type": "Speed Controller",
+                  "_source0": "network_table:///LiveWindow/Ungrouped/Talon FX [5]",
+                  "_title": "Talon FX [5]",
+                  "Visuals/Orientation": "HORIZONTAL"
+                },
+                {
+                  "_type": "Differential Drivebase",
+                  "_source0": "network_table:///LiveWindow/Ungrouped/DifferentialDrive[1]",
+                  "_title": "DifferentialDrive[1]",
+                  "Wheels/Number of wheels": 4,
+                  "Wheels/Wheel diameter": 80.0,
+                  "Visuals/Show velocity vectors": true
+                },
+                {
+                  "_type": "Speed Controller",
+                  "_source0": "network_table:///LiveWindow/Ungrouped/Talon FX [3]",
+                  "_title": "Talon FX [3]",
+                  "Visuals/Orientation": "HORIZONTAL"
+                },
+                {
+                  "_type": "Speed Controller",
+                  "_source0": "network_table:///LiveWindow/Ungrouped/Spark[0]",
+                  "_title": "Spark[0]",
+                  "Visuals/Orientation": "HORIZONTAL"
+                },
+                {
+                  "_type": "Speed Controller",
+                  "_source0": "network_table:///LiveWindow/Ungrouped/Talon FX [8]",
+                  "_title": "Talon FX [8]",
+                  "Visuals/Orientation": "HORIZONTAL"
+                },
+                {
+                  "_type": "Text View",
+                  "_source0": "network_table:///LiveWindow/Ungrouped/Servo[4]/Value",
+                  "_title": "Servo[4]/Value"
+                },
+                {
+                  "_type": "Boolean Box",
+                  "_source0": "network_table:///LiveWindow/Ungrouped/DigitalInput[3]/Value",
+                  "_title": "DigitalInput[3]/Value",
+                  "Colors/Color when true": "#7CFC00FF",
+                  "Colors/Color when false": "#8B0000FF"
+                },
+                {
+                  "_type": "Boolean Box",
+                  "_source0": "network_table:///LiveWindow/Ungrouped/DigitalInput[1]/Value",
+                  "_title": "DigitalInput[1]/Value",
+                  "Colors/Color when true": "#7CFC00FF",
+                  "Colors/Color when false": "#8B0000FF"
+                },
+                {
+                  "_type": "Text View",
+                  "_source0": "network_table:///LiveWindow/Ungrouped/DoubleSolenoid[7,0]/Value",
+                  "_title": "DoubleSolenoid[7,0]/Value"
+                },
+                {
+                  "_type": "Boolean Box",
+                  "_source0": "network_table:///LiveWindow/Ungrouped/DigitalInput[4]/Value",
+                  "_title": "DigitalInput[4]/Value",
+                  "Colors/Color when true": "#7CFC00FF",
+                  "Colors/Color when false": "#8B0000FF"
+                },
+                {
+                  "_type": "Boolean Box",
+                  "_source0": "network_table:///LiveWindow/Ungrouped/DigitalInput[2]/Value",
+                  "_title": "DigitalInput[2]/Value",
+                  "Colors/Color when true": "#7CFC00FF",
+                  "Colors/Color when false": "#8B0000FF"
+                },
+                {
+                  "_type": "Text View",
+                  "_source0": "network_table:///LiveWindow/Ungrouped/DoubleSolenoid[7,3]/Value",
+                  "_title": "DoubleSolenoid[7,3]/Value"
+                }
+              ]
+            }
+          },
+          "4,0": {
+            "size": [
+              2,
+              4
+            ],
+            "content": {
+              "_type": "Subsystem Layout",
+              "_source0": "network_table:///LiveWindow/LimeLight",
+              "_title": "LimeLight",
+              "Layout/Label position": "BOTTOM",
+              "_children": []
+            }
+          },
+          "6,0": {
+            "size": [
+              2,
+              4
+            ],
+            "content": {
+              "_type": "Subsystem Layout",
+              "_source0": "network_table:///LiveWindow/Leveler",
+              "_title": "Leveler",
+              "Layout/Label position": "BOTTOM",
+              "_children": []
+            }
+          },
+          "8,0": {
+            "size": [
+              2,
+              4
+            ],
+            "content": {
+              "_type": "Subsystem Layout",
+              "_source0": "network_table:///LiveWindow/Drive",
+              "_title": "Drive",
+              "Layout/Label position": "BOTTOM",
+              "_children": []
+            }
+          },
+          "0,4": {
+            "size": [
+              2,
+              4
+            ],
+            "content": {
+              "_type": "Subsystem Layout",
+              "_source0": "network_table:///LiveWindow/Intake",
+              "_title": "Intake",
+              "Layout/Label position": "BOTTOM",
+              "_children": []
+            }
+          },
+          "2,4": {
+            "size": [
+              2,
+              4
+            ],
+            "content": {
+              "_type": "Subsystem Layout",
+              "_source0": "network_table:///LiveWindow/ShooterHood",
+              "_title": "ShooterHood",
+              "Layout/Label position": "BOTTOM",
+              "_children": []
+            }
+          },
+          "4,4": {
+            "size": [
+              2,
+              4
+            ],
+            "content": {
+              "_type": "Subsystem Layout",
+              "_source0": "network_table:///LiveWindow/LED",
+              "_title": "LED",
+              "Layout/Label position": "BOTTOM",
+              "_children": []
+            }
+          },
+          "6,4": {
+            "size": [
+              2,
+              4
+            ],
+            "content": {
+              "_type": "Subsystem Layout",
+              "_source0": "network_table:///LiveWindow/Shooter",
+              "_title": "Shooter",
+              "Layout/Label position": "BOTTOM",
+              "_children": [
+                {
+                  "_type": "Subsystem Layout",
+                  "_source0": "network_table:///LiveWindow/ShooterHood",
+                  "_title": "LiveWindow/ShooterHood",
+                  "Layout/Label position": "BOTTOM",
+                  "_children": []
+                },
+                {
+                  "_type": "Subsystem Layout",
+                  "_source0": "network_table:///LiveWindow/ShooterAim",
+                  "_title": "LiveWindow/ShooterAim",
+                  "Layout/Label position": "BOTTOM",
+                  "_children": []
+                }
+              ]
+            }
+          },
+          "8,4": {
+            "size": [
+              2,
+              4
+            ],
+            "content": {
+              "_type": "Subsystem Layout",
+              "_source0": "network_table:///LiveWindow/Camera",
+              "_title": "Camera",
+              "Layout/Label position": "BOTTOM",
+              "_children": []
+            }
+          }
+        }
+      }
+    },
+    {
+      "title": "Driver Station",
+      "autoPopulate": false,
+      "autoPopulatePrefix": "",
+      "widgetPane": {
+        "gridSize": 128.0,
+        "showGrid": true,
+        "hgap": 16.0,
+        "vgap": 16.0,
+        "tiles": {
+          "8,6": {
+            "size": [
+              2,
+              2
+            ],
+            "content": {
+              "_type": "Gyro",
+              "_title": "Gyro",
+              "Visuals/Major tick spacing": 45.0,
+              "Visuals/Starting angle": 180.0,
+              "Visuals/Show tick mark ring": true,
+              "Visuals/Counter clockwise": false
+            }
+          },
+          "8,8": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Boolean Box",
+              "_title": "Boolean Box",
+              "Colors/Color when true": "#7CFC00FF",
+              "Colors/Color when false": "#8B0000FF"
+            }
+          },
+          "9,8": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Boolean Box",
+              "_title": "Boolean Box",
+              "Colors/Color when true": "#7CFC00FF",
+              "Colors/Color when false": "#8B0000FF"
+            }
+          },
+          "8,5": {
+            "size": [
+              2,
+              1
+            ],
+            "content": {
+              "_type": "Simple Dial",
+              "_title": "Simple Dial",
+              "Range/Min": 0.0,
+              "Range/Max": 100.0,
+              "Visuals/Show value": true
+            }
+          },
+          "6,0": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Boolean Box",
+              "_title": "Boolean Box",
+              "Colors/Color when true": "#7CFC00FF",
+              "Colors/Color when false": "#8B0000FF"
+            }
+          },
+          "4,0": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Boolean Box",
+              "_title": "Boolean Box",
+              "Colors/Color when true": "#7CFC00FF",
+              "Colors/Color when false": "#8B0000FF"
+            }
+          },
+          "5,0": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Boolean Box",
+              "_title": "Boolean Box",
+              "Colors/Color when true": "#7CFC00FF",
+              "Colors/Color when false": "#8B0000FF"
+            }
+          },
+          "7,0": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Boolean Box",
+              "_title": "Boolean Box",
+              "Colors/Color when true": "#7CFC00FF",
+              "Colors/Color when false": "#8B0000FF"
+            }
+          },
+          "8,0": {
+            "size": [
+              2,
+              1
+            ],
+            "content": {
+              "_type": "Simple Dial",
+              "_title": "Simple Dial",
+              "Range/Min": 0.0,
+              "Range/Max": 100.0,
+              "Visuals/Show value": true
+            }
+          },
+          "0,0": {
+            "size": [
+              4,
+              4
+            ],
+            "content": {
+              "_type": "Camera Stream",
+              "_source0": "camera_server://front",
+              "_title": "front",
+              "Crosshair/Show crosshair": true,
+              "Crosshair/Crosshair color": "#FFFFFFFF",
+              "Controls/Show controls": true,
+              "Controls/Rotation": "NONE",
+              "compression": -1.0,
+              "fps": -1,
+              "imageWidth": 0,
+              "imageHeight": 0
+            }
+          },
+          "3,4": {
+            "size": [
+              5,
+              5
+            ],
+            "content": {
+              "_type": "Camera Stream",
+              "_source0": "camera_server://limelight",
+              "_title": "limelight",
+              "Crosshair/Show crosshair": true,
+              "Crosshair/Crosshair color": "#FFFFFFFF",
+              "Controls/Show controls": true,
+              "Controls/Rotation": "NONE",
+              "compression": -1.0,
+              "fps": -1,
+              "imageWidth": -1,
+              "imageHeight": -1
+            }
+          },
+          "8,4": {
+            "size": [
+              2,
+              1
+            ],
+            "content": {
+              "_type": "Simple Dial",
+              "_source0": "network_table:///SmartDashboard/Drum Velocity",
+              "_title": "SmartDashboard/Drum Velocity",
+              "Range/Min": 0.0,
+              "Range/Max": 20000.0,
+              "Visuals/Show value": true
+            }
+          },
+          "7,1": {
+            "size": [
+              3,
+              3
+            ],
+            "content": {
+              "_type": "Gyro",
+              "_source0": "network_table:///SmartDashboard/Pigeon Gyro",
+              "_title": "SmartDashboard/Pigeon Gyro",
+              "Visuals/Major tick spacing": 45.0,
+              "Visuals/Starting angle": 180.0,
+              "Visuals/Show tick mark ring": true,
+              "Visuals/Counter clockwise": false
+            }
+          },
+          "4,3": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Boolean Box",
+              "_source0": "network_table:///SmartDashboard/Gear Shift",
+              "_title": "Gear Shift",
+              "Colors/Color when true": "#7CFC00FF",
+              "Colors/Color when false": "#8B0000FF"
+            }
+          },
+          "5,3": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Boolean Box",
+              "_source0": "network_table:///SmartDashboard/Rachet",
+              "_title": "Rachet",
+              "Colors/Color when true": "#7CFC00FF",
+              "Colors/Color when false": "#8B0000FF"
+            }
+          },
+          "6,3": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Boolean Box",
+              "_source0": "network_table:///SmartDashboard/Climber Safety",
+              "_title": "Climber Safety",
+              "Colors/Color when true": "#7CFC00FF",
+              "Colors/Color when false": "#8B0000FF"
+            }
+          },
+          "4,1": {
+            "size": [
+              3,
+              2
+            ],
+            "content": {
+              "_type": "Differential Drivebase",
+              "_source0": "network_table:///SmartDashboard/Drive Train",
+              "_title": "SmartDashboard/Drive Train",
+              "Wheels/Number of wheels": 4,
+              "Wheels/Wheel diameter": 80.0,
+              "Visuals/Show velocity vectors": true
+            }
+          }
+        }
+      }
+    }
+  ],
+  "windowGeometry": {
+    "x": 47.20000076293945,
+    "y": 148.8000030517578,
+    "width": 1553.5999755859375,
+    "height": 1448.0
+  }
+}
\ No newline at end of file
diff --git a/src/main/deploy/driverStation/themes/Ridgbotics/ridgeboticstheme.css b/src/main/deploy/driverStation/themes/Ridgbotics/ridgeboticstheme.css
new file mode 100644
index 0000000..cb762ec
--- /dev/null
+++ b/src/main/deploy/driverStation/themes/Ridgbotics/ridgeboticstheme.css
@@ -0,0 +1,9 @@
+@import "/edu/wpi/first/shuffleboard/app/midnight.css";
+
+.root{
+	-swatch-100: #66FF66;
+	-swatch-200: #4DFF4D;
+	-swatch-300: #1AFF1A;
+	-swatch-400: #00CC00;
+	-swatch-500: #009900;
+}
\ No newline at end of file