Merge branch 'master' into buff-driver-station

src/main/deploy/Robot Data - Distances.csv

@@ -1,9 +1,14 @@
-Distance (in),Hood Ext. (u),Drum Velocity (u/ds)
+Distance (in),Hood Ext. (u),Drum Velocity (u/ds),Center Displacment (deg)
-74,20,8000
+1,10,8000,2
-144,23,10000
+70,21,8000,2, check
-162,28,10000
+100,24,9000,2
-208,29,10000
+145,28,10000,1
-296,32,12500
+230,31,12000,0,Add a 270
-418,33,16000
+246,32,15000,0
-430,31,16000
+320,32,17000,0,change 17000 and mark them lower
-450,31,16000
+331,33,17000,0
+397,33,16000,0
+415,33,16250,0
+436,31,18000,0
+500,33,18000,0
+501,33,18000,0

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 */
@@ -122,8 +116,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.55, 0.0, 100, 0.0, 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 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;
@@ -160,29 +153,40 @@ 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;
         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 = 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
+        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_SHOOTER = 11;
-        public static final int BEAM_SENSOR_USELESS = 2;
+        public static final int BEAM_SENSOR_USELESS = 12;
-        public static final int BEAM_SENSOR_STORAGE = 3;
+        public static final int BEAM_SENSOR_STORAGE = 13;
-        public static final int BEAM_SENSOR_INTAKE = 4;
+        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);
 
         /* 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 {
@@ -202,9 +206,9 @@ 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 TARGET_HEIGHT = 71.5;
-        public static final double LIME_ANGLE = 25;
+        public static final double LIME_ANGLE = 24.7;
-        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;

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;
@@ -37,13 +38,12 @@ 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;
-import frc4388.robot.commands.storage.StorageIntake;
+import frc4388.robot.commands.storage.ManageStorage;
-import frc4388.robot.commands.storage.StoragePrepIntake;
+import frc4388.robot.commands.storage.StoragePrep;
+import frc4388.robot.commands.storage.ManageStorage.StorageMode;
 import frc4388.robot.subsystems.Camera;
 import frc4388.robot.subsystems.Climber;
 import frc4388.robot.subsystems.Drive;
@@ -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
@@ -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));
     }
 
@@ -171,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));
+            .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));
+            .whenReleased(new InterruptSubystem(m_robotStorage));
 
         // extends or retracts the extender
         new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
@@ -201,7 +203,8 @@ 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))
+            //.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))
@@ -228,7 +231,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()));
     }
 

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

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

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

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

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

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

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

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

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

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

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_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel());
-    //m_shooterHood.runAngleAdjustPID(m_shooterHood.addFireAngle());
-    //SmartDashboard.putNumber("Fire Velocity", m_shooter.addFireVel());
-    //SmartDashboard.putNumber("Fire Angle", m_shooter.addFireAngle());
   }
 
   // Called once the command ends or is interrupted.

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

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

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

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

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

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

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

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

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

@@ -1,46 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package frc4388.robot.commands.storage;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc4388.robot.Constants.StorageConstants;
-import frc4388.robot.subsystems.Storage;
-
-public class StorageOutake extends CommandBase {
-  Storage m_storage;
-  /**
-   * Runs the Storage out for outaking
-   * @param storeSub The Storage subsystem
-   */
-  public StorageOutake(Storage storeSub) {
-    m_storage = storeSub;
-    addRequirements(m_storage);
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {
-  }
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-      m_storage.runStorage(-StorageConstants.STORAGE_SPEED);
-  }
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {
-  }
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return false;
-  }
-}

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

@@ -1,52 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package frc4388.robot.commands.storage;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc4388.robot.Constants.StorageConstants;
-import frc4388.robot.subsystems.Storage;
-
-public class StoragePositionPID extends CommandBase {
-  public Storage m_storage;
-  double startPos;
-  /**
-   * Moves the storage a number of rotations
-   * @param subsystem The Storage subsystem
-   */
-  public StoragePositionPID(Storage subsystem) {
-    m_storage = subsystem;
-    addRequirements(m_storage);
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {
-
-  }
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-    m_storage.runStoragePositionPID(StorageConstants.STORAGE_FULL_BALL);
-  }
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {
-  }
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    /*if (startPos + StorageConstants.STORAGE_FULL_BALL == m_storage.getEncoderPos())
-    {
-      return true;
-    }*/
-    return false;
-  }
-}

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

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

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

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

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

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

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.

src/main/java/frc4388/robot/subsystems/Shooter.java

@@ -91,8 +91,11 @@ public class Shooter extends SubsystemBase {
 
       SmartDashboard.putNumber("Shooter Current", m_shooterFalcon.getSupplyCurrent());
 
-      SmartDashboard.putBoolean("Drum Ready", m_isDrumReady);
+      SmartDashboard.putBoolean("Drum Ready" , m_isDrumReady);
-    } catch(Exception e) {
+    }
+
+    catch(Exception e)
+    {
       
     }
   }

src/main/java/frc4388/robot/subsystems/ShooterAim.java

@@ -67,6 +67,8 @@ public class ShooterAim extends SubsystemBase {
     SmartDashboard.putBoolean("Aim Ready", m_isAimReady);
 
     SmartDashboard.putData("Turret Angle", m_turretGyro);
+    
+    SmartDashboard.putBoolean("Turret Aimed" , m_isAimReady);
   }
 
   /**

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,26 +41,29 @@ 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);
+    // Set PID Coefficients
-    m_beamSensors[3] = new DigitalInput(StorageConstants.BEAM_SENSOR_STORAGE);
+    m_storagePIDController.setP(storageGains.m_kP);
-    m_beamSensors[4] = new DigitalInput(StorageConstants.BEAM_SENSOR_INTAKE);
+    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
   public void periodic() {
-    //SmartDashboard.putBoolean("Beam 0", m_beamSensors[0].get());
+    SmartDashboard.putBoolean("Intake Beam", getBeamIntake());
-    //SmartDashboard.putBoolean("Beam 1", m_beamSensors[1].get());
+    SmartDashboard.putBoolean("Storage Beam", getBeamStorage());
-    //SmartDashboard.putBoolean("Beam 0", m_beamSensors[0].get());
+    SmartDashboard.putBoolean("Upper Beam", getBeamUseless());
-    //SmartDashboard.putBoolean("Beam 1", m_beamSensors[1].get());
+    SmartDashboard.putBoolean("Shooter Beam", getBeamShooter());
   }
 
   /**
    * 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,66 @@ public class Storage extends SubsystemBase {
     m_encoder.setPosition(0);
   }
 
-  public void testBeams(){
+  /**
-    SmartDashboard.putBoolean("Beam 0", m_beamSensors[0].get());
+   * Runs Storage to a particular position
-    SmartDashboard.putBoolean("Beam 1", m_beamSensors[1].get());
+   * @param targetPos in inches
-  }
+   */
-
-  /* Storage PID Control */
   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){
+  public double getEncoderPosInches(){
-    return m_beamSensors[id].get();
+    return motorRotsToInches(getEncoderPos());
   }
 
-  public void setStoragePID(double position){
+  public double getEncoderVel(){
-    m_storagePIDController.setReference(position , ControlType.kPosition);
+    return m_encoder.getVelocity();
+  }
+
+  /**
+   * @param motorRots
+   * @return inches
+   */
+  public double motorRotsToInches(double motorRots) {
+    return motorRots * (1/StorageConstants.MOTOR_ROTS_PER_STORAGE_ROT) * (StorageConstants.INCHES_PER_STORAGE_ROT);
+  }
+
+  /**
+   * @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();
   }
 }

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);
     }
 }

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 ++;
                 }
@@ -80,6 +82,9 @@ public class ShooterTables {
 
             m_angleLength = lineNum-1;
 
+            distanceReader.close();
+            angleReader.close();
+
         } catch (FileNotFoundException e) {
             e.printStackTrace();
         } catch (IOException e) {
@@ -123,6 +128,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)) {