diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index f0782cb..e09b86a 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -103,7 +103,8 @@ public final class Constants {
         public static final double INCHES_PER_TICK_LOW = 1/TICKS_PER_INCH_LOW;
     }
     
-    public static final class IntakeConstants {
+    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;
     }
@@ -118,12 +119,28 @@ 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 SHOOTER_TURRET_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0);
+        //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.2, 0.0, 0, 0.0453, 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, 1.0);
         public static final double SHOOTER_TURRET_MIN = -1.0;
         public static final double ENCODER_TICKS_PER_REV = 2048;
         public static final double NEO_UNITS_PER_REV = 42;
         public static final double DEGREES_PER_ROT = 360;
+
+        public static final int TURRET_RIGHT_SOFT_LIMIT = -2;
+        public static final int TURRET_LEFT_SOFT_LIMIT = -55;
+        public static final double TURRET_SPEED_MULTIPLIER = 0.3;
+        public static final double TURRET_CALIBRATE_SPEED = 0.075;
+
+        public static final int HOOD_UP_SOFT_LIMIT = 33;
+        public static final int HOOD_DOWN_SOFT_LIMIT = 3;
+        public static final double HOOD_CONVERT_SLOPE = 0.47;
+        public static final double HOOD_CONVERT_B = 40.5;
+        public static final double HOOD_CALIBRATE_SPEED = 0.1;
+
+        public static final double DRUM_RAMP_LIMIT = 1000;
+        public static final double DRUM_VELOCITY_BOUND = 300;
     }
     
     public static final class ClimberConstants {
@@ -136,6 +153,10 @@ public final class Constants {
   
     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_TIMEOUT = 2000;
 
         /* Ball Indexes */
         public static final int BEAM_SENSOR_DIO_0 = 0;
@@ -152,9 +173,8 @@ public final class Constants {
         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.2, 0.0, 0.0, 0.0, 0, 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 {
@@ -174,8 +194,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 = 82.75;
-        public static final double LIME_ANGLE = 18.7366;
+        public static final double TARGET_HEIGHT = 64;
+        public static final double LIME_ANGLE = 25;
         public static final double TURN_P_VALUE = 0.65;
         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/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 2fda40a..718abcb 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -16,6 +16,7 @@ import edu.wpi.first.wpilibj.controller.RamseteController;
 import edu.wpi.first.wpilibj.geometry.Pose2d;
 import edu.wpi.first.wpilibj.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj.trajectory.Trajectory;
 import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
 import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
@@ -28,26 +29,52 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 
 import frc4388.robot.Constants.*;
+import frc4388.robot.commands.CalibrateShooter;
 import frc4388.robot.commands.DrivePositionMPAux;
 import frc4388.robot.commands.DriveStraightAtVelocityPID;
 import frc4388.robot.commands.DriveStraightToPositionMM;
 import frc4388.robot.commands.DriveStraightToPositionPID;
 import frc4388.robot.commands.DriveWithJoystick;
+import frc4388.robot.commands.DriveStraightToPositionMM;
+import frc4388.robot.commands.DriveStraightToPositionPID;
+import frc4388.robot.commands.DriveWithJoystickUsingDeadAssistPID;
+import frc4388.robot.commands.HoldTarget;
+import frc4388.robot.commands.HoodPositionPID;
+import frc4388.robot.commands.DriveWithJoystickDriveStraight;
+import frc4388.robot.commands.RunClimberWithTriggers;
+import frc4388.robot.commands.RunExtenderOutIn;
+import frc4388.robot.commands.RunIntakeWithTriggers;
+import frc4388.robot.commands.ShooterVelocityControlPID;
+import frc4388.robot.commands.StorageIntake;
 import frc4388.robot.commands.GotoCoordinates;
 import frc4388.robot.commands.RunClimberWithTriggers;
 import frc4388.robot.commands.RunExtenderOutIn;
 import frc4388.robot.commands.RunIntakeWithTriggers;
-import frc4388.robot.commands.StorageIntakeGroup;
 import frc4388.robot.subsystems.Drive;
 import frc4388.robot.subsystems.Intake;
 import frc4388.robot.subsystems.LED;
 import frc4388.robot.subsystems.Shooter;
+import frc4388.robot.subsystems.ShooterAim;
 import frc4388.robot.subsystems.Climber;
 import frc4388.robot.commands.RunLevelerWithJoystick;
+import frc4388.robot.commands.ShootFireGroup;
+import frc4388.robot.commands.ShootFullGroup;
+import frc4388.robot.commands.ShootPrepGroup;
+import frc4388.robot.subsystems.Drive;
+import frc4388.robot.subsystems.Intake;
+import frc4388.robot.subsystems.LED;
 import frc4388.robot.commands.TrackTarget;
+import frc4388.robot.commands.TrimShooter;
+import frc4388.robot.commands.StorageOutake;
+import frc4388.robot.commands.StoragePrepAim;
+import frc4388.robot.commands.StoragePrepIntake;
+import frc4388.robot.commands.StorageRun;
+import frc4388.robot.subsystems.Camera;
+import frc4388.robot.subsystems.Leveler;
+import frc4388.robot.subsystems.LimeLight;
 import frc4388.robot.commands.TurnDegrees;
 import frc4388.robot.commands.Wait;
-import frc4388.robot.commands.storageOutake;
+import frc4388.robot.commands.StorageOutake;
 import frc4388.robot.subsystems.Camera;
 import frc4388.robot.subsystems.Leveler;
 import frc4388.robot.subsystems.Pneumatics;
@@ -69,20 +96,23 @@ public class RobotContainer {
     private final LED m_robotLED = new LED();
     private final Intake m_robotIntake = new Intake();
     private final Shooter m_robotShooter = new Shooter();
+    private final ShooterAim m_robotShooterAim = new ShooterAim();
     private final Climber m_robotClimber = new Climber();
     private final Leveler m_robotLeveler = new Leveler();
     private final Storage m_robotStorage = new Storage();
 
     /* Cameras */
-    private final Camera m_robotCameraFront = new Camera("front",0,160,120,40);
-    private final Camera m_robotCameraBack = new Camera("back",1,160,120,40);
+    private final Camera m_robotCameraFront = new Camera("front", 0, 160, 120, 40);
+    private final Camera m_robotCameraBack = new Camera("back", 1, 160, 120, 40);
+    private final LimeLight m_robotLime = new LimeLight();
 
     /* Controllers */
     private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
     private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
 
+
     /**
-     * The container for the robot.  Contains subsystems, OI devices, and commands.
+     * The container for the robot. Contains subsystems, OI devices, and commands.
      */
     public RobotContainer() {
         /* Passing Drive and Pneumatics Subsystems */
@@ -96,23 +126,26 @@ public class RobotContainer {
         m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, getDriverController()));
         // drives intake with input from triggers on the opperator controller
         m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController()));
-        // runs the drum shooter in idle mode
-        m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runShooterWithInput(m_operatorXbox.getLeftXAxis()), m_robotShooter));
+        // 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));
         // 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
         m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController()));
         // continually sends updates to the Blinkin LED controller to keep the lights on
         m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
+        //m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0), m_robotStorage));
+        //m_robotLime.setDefaultCommand(new RunCommand(() -> m_robotLime.limeOff(), m_robotLime));
     }
-      
 
     /**
-    * Use this method to define your button->command mappings.  Buttons can be created by
-    * instantiating a {@link GenericHID} or one of its subclasses ({@link
-    * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
-    * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
-    */
+     * Use this method to define your button->command mappings. Buttons can be
+     * created by instantiating a {@link GenericHID} or one of its subclasses
+     * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then
+     * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+     */
     private void configureButtonBindings() {
         /* Test Buttons */
         // A driver test button
@@ -129,8 +162,8 @@ public class RobotContainer {
 
         // X driver test button
         new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
-            .whenPressed(new DriveStraightToPositionPID(m_robotDrive, 36));
-     
+            .whenPressed(new InstantCommand());
+      
         /* Driver Buttons */
         // sets solenoids into high gear
         new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
@@ -140,21 +173,19 @@ public class RobotContainer {
         new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON)
             .whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(false), m_robotDrive));
 
+
         /* Operator Buttons */
-        //TODO: Shooter Buttons
+
         // shoots until released
-        //new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
-        //    .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 5));
+        new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
+            .whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false)
+            .whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
 
         // shoots one ball
-        //new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON)
-        //    .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 1));
-            
-        // aims the turret
-        new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
-            .whileHeld(new TrackTarget(m_robotShooter));
-            //.whenPressed(new RunCommand(() -> m_robotStorage.storeAim()));
-        
+        new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON)
+            .whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false)
+            .whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
+
         // extends or retracts the extender
         new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
             .whenPressed(new RunExtenderOutIn(m_robotIntake));
@@ -164,17 +195,38 @@ public class RobotContainer {
             .whenPressed(new InstantCommand(() -> m_robotClimber.setSafetyPressed(), m_robotClimber))
             .whenReleased(new InstantCommand(() -> m_robotClimber.setSafetyNotPressed(), m_robotClimber));
 
-        /* Storage Neo PID Test */
+        // starts tracking target
         new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
-            .whileHeld(new TrackTarget(m_robotShooter));
+            .whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage))
+            .whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage));
+            
 
         //Prepares storage for intaking
         new JoystickButton(getOperatorJoystick(), XboxController.LEFT_TRIGGER_AXIS)
-            .whileHeld(new StorageIntakeGroup(m_robotIntake, m_robotStorage));
+            .whileHeld(new StorageIntake(m_robotIntake, m_robotStorage));
             
         //Runs storage to outtake
         new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_TRIGGER_AXIS)
-            .whileHeld(new storageOutake(m_robotStorage));
+            .whileHeld(new StorageOutake(m_robotStorage));
+
+        //TEST FOR HOOD
+        new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON)
+            .whileHeld(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0.3)))
+            .whenReleased(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0)));
+
+        //TEST FOR HOOD
+        new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON)
+            .whileHeld(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(-0.3)))
+            .whenReleased(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0)));
+
+        //Trims shooter
+        new JoystickButton(getOperatorJoystick(), XboxController.TOP_BOTTOM_DPAD_AXIS)
+            .whenPressed(new TrimShooter(m_robotShooter));
+
+        //Calibrates turret and hood
+        new JoystickButton(getOperatorJoystick(), XboxController.START_BUTTON)
+            .whileHeld(new CalibrateShooter(m_robotShooter, m_robotShooterAim));
+
     }
 
     /**
@@ -313,4 +365,4 @@ public class RobotContainer {
     {
         return m_driverXbox.getJoyStick();
     }
-}
+}
\ No newline at end of file
diff --git a/src/main/java/frc4388/robot/Trims.java b/src/main/java/frc4388/robot/Trims.java
new file mode 100644
index 0000000..859794f
--- /dev/null
+++ b/src/main/java/frc4388/robot/Trims.java
@@ -0,0 +1,18 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-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;
+
+public class Trims{
+    public double m_turretTrim;
+    public double m_hoodTrim;
+
+    public Trims(double turretTrim, double hoodTrim){
+        m_turretTrim = turretTrim;
+        m_hoodTrim = hoodTrim;
+    }
+}
\ No newline at end of file
diff --git a/src/main/java/frc4388/robot/commands/CalibrateShooter.java b/src/main/java/frc4388/robot/commands/CalibrateShooter.java
new file mode 100644
index 0000000..1b766d7
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/CalibrateShooter.java
@@ -0,0 +1,66 @@
+/*----------------------------------------------------------------------------*/
+/* 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 com.revrobotics.CANSparkMax.SoftLimitDirection;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.Constants.ShooterConstants;
+import frc4388.robot.subsystems.Shooter;
+import frc4388.robot.subsystems.ShooterAim;
+
+public class CalibrateShooter extends CommandBase {
+  Shooter m_shooter;
+  ShooterAim m_shooterAim;
+  /**
+   * Calibrates the turret by moving the hood all the way down and moving the turret all the way right, then reseting the encoders
+   * @param shootSub The Shooter subsystem
+   * @param aimSub The ShooterAim subsystem
+   */
+  public CalibrateShooter(Shooter shootSub, ShooterAim aimSub) {
+    m_shooter = shootSub;
+    m_shooterAim = aimSub;
+    addRequirements(m_shooter, m_shooterAim);
+  }
+
+  // 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_shooter.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, false);
+    m_shooter.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, false);
+    m_shooter.m_angleEncoder.setPosition(0);
+    m_shooter.m_angleAdjustMotor.set(-ShooterConstants.HOOD_CALIBRATE_SPEED);
+
+    m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, false);
+    m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, false);
+    m_shooterAim.m_shooterRotateEncoder.setPosition(0);
+    m_shooterAim.m_shooterRotateMotor.set(ShooterConstants.TURRET_CALIBRATE_SPEED);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    m_shooter.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, true);
+    m_shooter.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
+
+    m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, true);
+    m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
diff --git a/src/main/java/frc4388/robot/commands/HoldTarget.java b/src/main/java/frc4388/robot/commands/HoldTarget.java
new file mode 100644
index 0000000..cbdee25
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/HoldTarget.java
@@ -0,0 +1,117 @@
+/*----------------------------------------------------------------------------*/
+/* 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 frc4388.robot.Constants.ShooterConstants;
+import frc4388.robot.Constants.VisionConstants;
+import frc4388.robot.subsystems.Drive;
+import frc4388.robot.subsystems.LimeLight;
+import frc4388.robot.subsystems.Shooter;
+import frc4388.robot.subsystems.ShooterAim;
+import frc4388.utility.controller.IHandController;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class HoldTarget extends CommandBase {
+    //Setup
+  NetworkTableEntry xEntry;
+  ShooterAim m_shooterAim;
+  Shooter m_shooter;
+  IHandController m_driverController;
+    //Aiming
+  double turnAmount = 0;
+  double xAngle = 0;
+  double yAngle = 0;
+  double target = 0;
+  public double distance;
+  public double fireVel;
+  public double fireAngle;
+
+  public double m_hoodTrim;
+  public double m_turretTrim;
+
+  /**
+   * Uses the Limelight to track the target
+   * @param shooterSubsystem The Shooter subsystem
+   * @param aimSubsystem The ShooterAim subsystem
+   */
+  public HoldTarget(Shooter shooterSubsystem, ShooterAim aimSubsystem) {
+    m_shooterAim = aimSubsystem;
+    m_shooter = shooterSubsystem;
+    addRequirements(m_shooterAim);
+    NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+      //Vision Processing Mode
+      NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
+      NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3);
+  }
+
+
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    target = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0);
+    xAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0);
+    yAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0);
+    
+    if (target == 1.0){ //If target in view
+        //Aiming Left/Right
+      turnAmount = (xAngle/VisionConstants.FOV)*VisionConstants.TURN_P_VALUE;
+      if (Math.abs(xAngle) < VisionConstants.X_ANGLE_ERROR){turnAmount = 0;} //Angle Error Zone 
+        //Deadzones
+      else if(turnAmount > 0 && turnAmount < VisionConstants.MOTOR_DEAD_ZONE){turnAmount = VisionConstants.MOTOR_DEAD_ZONE;} 
+      else if(turnAmount < 0 && turnAmount > -VisionConstants.MOTOR_DEAD_ZONE){turnAmount = -VisionConstants.MOTOR_DEAD_ZONE;}
+      m_shooterAim.runShooterWithInput(-turnAmount - m_shooter.shooterTrims.m_turretTrim);
+
+        //Finding Distance
+      distance = VisionConstants.TARGET_HEIGHT/Math.tan((VisionConstants.LIME_ANGLE + yAngle)*(Math.PI/180));
+      SmartDashboard.putNumber("Distance to Target", distance);
+
+      double yVel = Math.sqrt(2*VisionConstants.GRAV*VisionConstants.TARGET_HEIGHT);
+      double xVel = (distance*VisionConstants.GRAV)/(yVel);
+
+      fireVel = Math.sqrt((Math.pow(xVel, 2))+(Math.pow(yVel,2)));
+      fireAngle = Math.atan(yVel/xVel) * (180/Math.PI);
+      m_shooter.m_fireVel = fireVel;
+      m_shooter.m_fireAngle = fireAngle + m_shooter.shooterTrims.m_hoodTrim;
+
+    }/*
+    else{
+      System.err.println("Shooter Pos: " + m_shooterAim.getShooterRotatePosition());
+      double curveInput = -Math.abs(-Math.cos(Math.PI * ((2*m_shooterAim.getShooterRotatePosition())/55))+1) * 0.1;
+      if (m_shooterAim.getShooterRotatePosition() >= -3 || m_shooterAim.getShooterRotatePosition() <= -54){
+        curveInput = -curveInput;
+      }
+      System.err.println("Curve Input: " + curveInput);
+      
+      m_shooterAim.runShooterWithInput(curveInput);
+    }
+    */
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
+    NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
diff --git a/src/main/java/frc4388/robot/commands/HoodPositionPID.java b/src/main/java/frc4388/robot/commands/HoodPositionPID.java
new file mode 100644
index 0000000..efecb58
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/HoodPositionPID.java
@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* 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.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.Constants.ShooterConstants;
+import frc4388.robot.subsystems.Shooter;
+
+public class HoodPositionPID extends CommandBase {
+  Shooter m_shooter;
+  double firingAngle;
+  /**
+   * Creates a new HoodPositionPID.
+   */
+  public HoodPositionPID(Shooter subSystem) {
+    m_shooter = subSystem;
+    //addRequirements(m_shooter);
+  }
+
+  // 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() {
+    double slope = ShooterConstants.HOOD_CONVERT_SLOPE;
+    double b = ShooterConstants.HOOD_CONVERT_B;
+    firingAngle = (-slope*m_shooter.addFireAngle())+b;
+    SmartDashboard.putNumber("Shoot Angle From Equation", m_shooter.addFireAngle());
+    SmartDashboard.putNumber("Fire Angle", firingAngle);
+    m_shooter.runAngleAdjustPID(firingAngle);
+  }
+
+  // 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 encoderPos = m_shooter.m_angleAdjustMotor.getEncoder().getPosition();
+    if(encoderPos < firingAngle + 1 || encoderPos < firingAngle - 1){
+      return true;
+    }
+    return false;
+  }
+}
diff --git a/src/main/java/frc4388/robot/commands/ShootFireGroup.java b/src/main/java/frc4388/robot/commands/ShootFireGroup.java
new file mode 100644
index 0000000..7d23c65
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/ShootFireGroup.java
@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* 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.ParallelRaceGroup;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import frc4388.robot.subsystems.Shooter;
+import frc4388.robot.subsystems.ShooterAim;
+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, Storage m_storage) {
+    addCommands(
+      new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel(), m_shooter.m_shooterFalcon.getSelectedSensorVelocity())),
+      new RunCommand(() -> m_shooter.runAngleAdjustPID(m_shooter.addFireAngle())),
+      new HoldTarget(m_shooter, m_shooterAim),
+      new StorageRun(m_storage)
+    );
+  }
+}
\ No newline at end of file
diff --git a/src/main/java/frc4388/robot/commands/StorageIntakeGroup.java b/src/main/java/frc4388/robot/commands/ShootFullGroup.java
similarity index 61%
rename from src/main/java/frc4388/robot/commands/StorageIntakeGroup.java
rename to src/main/java/frc4388/robot/commands/ShootFullGroup.java
index 767ed69..55557b8 100644
--- a/src/main/java/frc4388/robot/commands/StorageIntakeGroup.java
+++ b/src/main/java/frc4388/robot/commands/ShootFullGroup.java
@@ -8,20 +8,24 @@
 package frc4388.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import frc4388.robot.subsystems.Intake;
+import frc4388.robot.subsystems.Shooter;
+import frc4388.robot.subsystems.ShooterAim;
 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 StorageIntakeGroup extends SequentialCommandGroup {
+public class ShootFullGroup extends SequentialCommandGroup {
   /**
-   * Creates a new StorageIntakeGroup.
+   * Preps and Fires the Shooter
+   * @param m_shooter The Shooter subsytem
+   * @param m_shooterAim The ShooterAim subsystem
+   * @param m_storage The Storage subsytem
    */
-  public StorageIntakeGroup(Intake m_intake, Storage m_storage) {
+  public ShootFullGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) {
     addCommands(
-      new storagePrepIntake(m_intake, m_storage), 
-      new storageIntake(m_intake, m_storage), 
-      new StorageIntakeFinal(m_storage));
+      new ShootPrepGroup(m_shooter, m_shooterAim, m_storage), 
+      new ShootFireGroup(m_shooter, m_shooterAim, m_storage)
+    );
   }
 }
diff --git a/src/main/java/frc4388/robot/commands/ShootPrepGroup.java b/src/main/java/frc4388/robot/commands/ShootPrepGroup.java
new file mode 100644
index 0000000..5c20683
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/ShootPrepGroup.java
@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* 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.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import frc4388.robot.subsystems.Shooter;
+import frc4388.robot.subsystems.ShooterAim;
+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 ShootPrepGroup extends ParallelCommandGroup {
+  /**
+   * Prepares the Shooter to be fired
+   * @param m_shooter The Shooter subsytem
+   * @param m_shooterAim The ShooterAim subsystem
+   * @param m_storage The Storage subsytem
+   */
+  public ShootPrepGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) {
+    addCommands(
+      new TrackTarget(m_shooter, m_shooterAim),
+      new ShooterVelocityControlPID(m_shooter),
+      new StoragePrepAim(m_storage),
+      new HoodPositionPID(m_shooter)
+    );
+  }
+}
diff --git a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java
index 67b5cb1..52dea7f 100644
--- a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java
+++ b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java
@@ -7,32 +7,37 @@
 
 package frc4388.robot.commands;
 
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.Constants.ShooterConstants;
 import frc4388.robot.subsystems.Shooter;
 
 public class ShooterVelocityControlPID extends CommandBase {
   Shooter m_shooter;
   double m_targetVel;
+  double m_actualVel;
   /**
-   * Creates a new ShooterVelocityControlPID.
+   * Runs the drum at a velocity
+   * @param subsystem The Shooter subsytem
    */
-  public ShooterVelocityControlPID(Shooter subsystem, double targetVel) {
-    // Use addRequirements() here to declare subsystem dependencies.
+  public ShooterVelocityControlPID(Shooter subsystem) {
     m_shooter = subsystem;
-    m_targetVel = targetVel;
     addRequirements(m_shooter);
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
+    
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    System.err.println(m_shooter.m_shooterFalcon.getSelectedSensorVelocity());
-    m_shooter.runDrumShooterVelocityPID(m_targetVel, m_shooter.m_shooterFalcon.getSelectedSensorVelocity());
+    m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel(), m_shooter.m_shooterFalcon.getSelectedSensorVelocity());
+    m_shooter.runAngleAdjustPID(m_shooter.addFireAngle());
+    SmartDashboard.putNumber("Fire Velocity", m_shooter.addFireVel());
+    SmartDashboard.putNumber("Fire Angle", m_shooter.addFireAngle());
   }
 
   // Called once the command ends or is interrupted.
@@ -43,6 +48,16 @@ public class ShooterVelocityControlPID extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return false;
+    //Tells whether the target velocity has been reached
+    double upperBound = m_targetVel + ShooterConstants.DRUM_VELOCITY_BOUND;
+    double lowerBound = m_targetVel - ShooterConstants.DRUM_VELOCITY_BOUND;
+    if (m_actualVel < upperBound && m_actualVel > lowerBound){
+      SmartDashboard.putBoolean("ShooterVelocityPID Finished", true);
+      return true;
+    }
+    else{
+      SmartDashboard.putBoolean("ShooterVelocityPID Finished", false);
+      return false;
+    }
   }
 }
diff --git a/src/main/java/frc4388/robot/commands/StorageIntake.java b/src/main/java/frc4388/robot/commands/StorageIntake.java
new file mode 100644
index 0000000..baf1630
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/StorageIntake.java
@@ -0,0 +1,64 @@
+/*----------------------------------------------------------------------------*/
+/* 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 frc4388.robot.Constants.IntakeConstants;
+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(0)){
+      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(0) && intook){
+      return true;
+    }
+    return false;
+  }
+}
diff --git a/src/main/java/frc4388/robot/commands/storageOutake.java b/src/main/java/frc4388/robot/commands/StorageOutake.java
similarity index 80%
rename from src/main/java/frc4388/robot/commands/storageOutake.java
rename to src/main/java/frc4388/robot/commands/StorageOutake.java
index 4820dc0..3231600 100644
--- a/src/main/java/frc4388/robot/commands/storageOutake.java
+++ b/src/main/java/frc4388/robot/commands/StorageOutake.java
@@ -8,14 +8,16 @@
 package frc4388.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.Constants.StorageConstants;
 import frc4388.robot.subsystems.Storage;
 
-public class storageOutake extends CommandBase {
+public class StorageOutake extends CommandBase {
   Storage m_storage;
   /**
-   * Creates a new storageOutake.
+   * Runs the Storage out for outaking
+   * @param storeSub The Storage subsystem
    */
-  public storageOutake(Storage storeSub) {
+  public StorageOutake(Storage storeSub) {
     m_storage = storeSub;
     addRequirements(m_storage);
   }
@@ -28,7 +30,7 @@ public class storageOutake extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-      m_storage.runStorage(-0.5);
+      m_storage.runStorage(-StorageConstants.STORAGE_SPEED);
   }
 
   // Called once the command ends or is interrupted.
diff --git a/src/main/java/frc4388/robot/commands/storagePrepIntake.java b/src/main/java/frc4388/robot/commands/StoragePositionPID.java
similarity index 71%
rename from src/main/java/frc4388/robot/commands/storagePrepIntake.java
rename to src/main/java/frc4388/robot/commands/StoragePositionPID.java
index 7fab016..bc607e5 100644
--- a/src/main/java/frc4388/robot/commands/storagePrepIntake.java
+++ b/src/main/java/frc4388/robot/commands/StoragePositionPID.java
@@ -8,36 +8,31 @@
 package frc4388.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc4388.robot.subsystems.Intake;
+import frc4388.robot.Constants.StorageConstants;
 import frc4388.robot.subsystems.Storage;
 
-public class storagePrepIntake extends CommandBase {
-  public Intake m_intake;
+public class StoragePositionPID extends CommandBase {
   public Storage m_storage;
+  double startPos;
   /**
-   * Creates a new storagePrepIntake.
+   * Moves the storage a number of rotations
+   * @param subsystem The Storage subsystem
    */
-  public storagePrepIntake(Intake inSub, Storage storeSub) {
-    m_intake = inSub;
-    m_storage = storeSub;
-    addRequirements(m_intake);
+  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() {
-    if (m_storage.getBeam(1) == false){
-      m_storage.runStorage(-0.5);
-    }
-    else{
-      m_storage.runStorage(0);
-    }
+    m_storage.runStoragePositionPID(StorageConstants.STORAGE_FULL_BALL);
   }
 
   // Called once the command ends or is interrupted.
@@ -48,9 +43,10 @@ public class storagePrepIntake extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (m_storage.getBeam(1)){
+    /*if (startPos + StorageConstants.STORAGE_FULL_BALL == m_storage.getEncoderPos())
+    {
       return true;
-    }
+    }*/
     return false;
   }
 }
diff --git a/src/main/java/frc4388/robot/commands/storagePrepAim.java b/src/main/java/frc4388/robot/commands/StoragePrepAim.java
similarity index 64%
rename from src/main/java/frc4388/robot/commands/storagePrepAim.java
rename to src/main/java/frc4388/robot/commands/StoragePrepAim.java
index c8b3bad..f0e52c1 100644
--- a/src/main/java/frc4388/robot/commands/storagePrepAim.java
+++ b/src/main/java/frc4388/robot/commands/StoragePrepAim.java
@@ -7,15 +7,19 @@
 
 package frc4388.robot.commands;
 
+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 StoragePrepAim extends CommandBase {
   Storage m_storage;
+  double startTime;
   /**
-   * Creates a new storagePrepAim.
+   * Prepares the Storage for aiming
+   * @param storeSub The Storage subsystem
    */
-  public storagePrepAim(Storage storeSub) {
+  public StoragePrepAim(Storage storeSub) {
     m_storage = storeSub;
     addRequirements(m_storage);
   }
@@ -23,13 +27,14 @@ public class storagePrepAim extends CommandBase {
   // 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(2) == false){
-      m_storage.runStorage(0.5);
+    if (m_storage.getBeam(1)){
+      m_storage.runStorage(StorageConstants.STORAGE_SPEED);
     }
     else{
       m_storage.runStorage(0);
@@ -44,9 +49,11 @@ public class storagePrepAim extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (m_storage.getBeam(2)){
+    if (!m_storage.getBeam(1) || startTime + StorageConstants.STORAGE_TIMEOUT <= System.currentTimeMillis()){
+      SmartDashboard.putBoolean("StoragePrepAim Finished", true);
       return true;
     }
+    SmartDashboard.putBoolean("StoragePrepAim Finished", false);
     return false;
   }
 }
diff --git a/src/main/java/frc4388/robot/commands/storageIntake.java b/src/main/java/frc4388/robot/commands/StoragePrepIntake.java
similarity index 70%
rename from src/main/java/frc4388/robot/commands/storageIntake.java
rename to src/main/java/frc4388/robot/commands/StoragePrepIntake.java
index ee30708..44a2d2e 100644
--- a/src/main/java/frc4388/robot/commands/storageIntake.java
+++ b/src/main/java/frc4388/robot/commands/StoragePrepIntake.java
@@ -8,16 +8,21 @@
 package frc4388.robot.commands;
 
 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 class StoragePrepIntake extends CommandBase {
   public Intake m_intake;
   public Storage m_storage;
+  public double startTime;
+
   /**
-   * Creates a new storageIntake.
+   * Prepares the Storage for intaking
+   * @param inSub The Intake subsystem
+   * @param storeSub the Storage subsystem
    */
-  public storageIntake(Intake inSub, Storage storeSub) {
+  public StoragePrepIntake(Intake inSub, Storage storeSub) {
     m_intake = inSub;
     m_storage = storeSub;
     addRequirements(m_intake);
@@ -27,18 +32,17 @@ public class storageIntake extends CommandBase {
   // 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(0)){
-      m_storage.setStoragePID(m_storage.getEncoderPos() + 2);
-      m_intake.runExtender(-0.3); 
+      m_storage.runStorage(-StorageConstants.STORAGE_SPEED);
     }
     else{
-      m_intake.runExtender(0.3); 
+      m_storage.runStorage(0);
     }
   }
 
@@ -50,7 +54,7 @@ public class storageIntake extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (m_storage.getBeam(0)){
+    if (!m_storage.getBeam(0) || startTime + StorageConstants.STORAGE_TIMEOUT <= System.currentTimeMillis()){
       return true;
     }
     return false;
diff --git a/src/main/java/frc4388/robot/commands/StorageIntakeFinal.java b/src/main/java/frc4388/robot/commands/StorageRun.java
similarity index 76%
rename from src/main/java/frc4388/robot/commands/StorageIntakeFinal.java
rename to src/main/java/frc4388/robot/commands/StorageRun.java
index 9576f35..91632f5 100644
--- a/src/main/java/frc4388/robot/commands/StorageIntakeFinal.java
+++ b/src/main/java/frc4388/robot/commands/StorageRun.java
@@ -8,16 +8,17 @@
 package frc4388.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.Constants.StorageConstants;
 import frc4388.robot.subsystems.Storage;
 
-public class StorageIntakeFinal extends CommandBase {
+public class StorageRun extends CommandBase {
   Storage m_storage;
   /**
-   * Creates a new StorageIntakeFinal.
+   * Runs the Storage at a speed
+   * @param storageSub The Storage subsytem
    */
-  public StorageIntakeFinal(Storage subsystem) {
-    m_storage = subsystem;
-    addRequirements(m_storage);
+  public StorageRun(Storage storageSub) {
+    m_storage = storageSub;
   }
 
   // Called when the command is initially scheduled.
@@ -28,14 +29,13 @@ public class StorageIntakeFinal extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if (m_storage.getBeam(1)){
-      m_storage.setStoragePID(m_storage.getEncoderPos() + 5);
-    }
+    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.
diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java
index 8102f13..8bdae59 100644
--- a/src/main/java/frc4388/robot/commands/TrackTarget.java
+++ b/src/main/java/frc4388/robot/commands/TrackTarget.java
@@ -9,21 +9,29 @@ package frc4388.robot.commands;
 
 import frc4388.robot.Constants.ShooterConstants;
 import frc4388.robot.Constants.VisionConstants;
+import frc4388.robot.subsystems.Camera;
 import frc4388.robot.subsystems.Drive;
+import frc4388.robot.subsystems.LimeLight;
 import frc4388.robot.subsystems.Shooter;
+import frc4388.robot.subsystems.ShooterAim;
+import frc4388.robot.commands.TrimShooter;
+import frc4388.utility.ShooterTables;
 import frc4388.utility.controller.IHandController;
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 public class TrackTarget extends CommandBase {
-    //Setup
+  // Setup
   NetworkTableEntry xEntry;
+  ShooterAim m_shooterAim;
   Shooter m_shooter;
   IHandController m_driverController;
-    //Aiming
+  // Aiming
   double turnAmount = 0;
   double xAngle = 0;
   double yAngle = 0;
@@ -32,66 +40,91 @@ public class TrackTarget extends CommandBase {
   public static double fireVel;
   public static double fireAngle;
 
+  public double m_hoodTrim;
+  public double m_turretTrim;
+
+  ShooterTables m_shooterTable;
+
   /**
    * Uses the Limelight to track the target
+   * @param shooterSubsystem The Shooter subsystem
+   * @param aimSubsystem The ShooterAim subsystem
    */
-  public TrackTarget(Shooter shooterSubsystem) {
+  public TrackTarget(Shooter shooterSubsystem, ShooterAim aimSubsystem) {
+    m_shooterAim = aimSubsystem;
     m_shooter = shooterSubsystem;
-    addRequirements(m_shooter);
+    addRequirements(m_shooterAim);
     NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-      //Vision Processing Mode
+    // Vision Processing Mode
     NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
     NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3);
+    m_shooterTable = new ShooterTables();
   }
 
-
-
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
     target = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0);
     xAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0);
     yAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0);
-    
-    if (target == 1.0){ //If target in view
-        //Aiming Left/Right
-      turnAmount = (xAngle/VisionConstants.FOV)*VisionConstants.TURN_P_VALUE;
-      if (Math.abs(xAngle) < VisionConstants.X_ANGLE_ERROR){turnAmount = 0;} //Angle Error Zone 
-        //Deadzones
-      else if(turnAmount > 0 && turnAmount < VisionConstants.MOTOR_DEAD_ZONE){turnAmount = VisionConstants.MOTOR_DEAD_ZONE;} 
-      else if(turnAmount < 0 && turnAmount > -VisionConstants.MOTOR_DEAD_ZONE){turnAmount = -VisionConstants.MOTOR_DEAD_ZONE;}
-      m_shooter.runShooterWithInput(turnAmount/5);
 
-        //Finding Distance
-      distance = VisionConstants.TARGET_HEIGHT/Math.tan((VisionConstants.LIME_ANGLE + yAngle)*(Math.PI/180));
+    if (target == 1.0) { // If target in view
+      // Aiming Left/Right
+      turnAmount = (xAngle / VisionConstants.FOV) * VisionConstants.TURN_P_VALUE;
+      if (Math.abs(xAngle) < VisionConstants.X_ANGLE_ERROR) {
+        turnAmount = 0;
+      } // Angle Error Zone
+      // Deadzones
+      else if (turnAmount > 0 && turnAmount < VisionConstants.MOTOR_DEAD_ZONE) {
+        turnAmount = VisionConstants.MOTOR_DEAD_ZONE;
+      } else if (turnAmount < 0 && turnAmount > -VisionConstants.MOTOR_DEAD_ZONE) {
+        turnAmount = -VisionConstants.MOTOR_DEAD_ZONE;
+      }
+      m_shooterAim.runShooterWithInput(-turnAmount - m_shooter.shooterTrims.m_turretTrim);
+
+      // Finding Distance
+      distance = VisionConstants.TARGET_HEIGHT / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180));
       SmartDashboard.putNumber("Distance to Target", distance);
 
-      double yVel = Math.sqrt(2*VisionConstants.GRAV*VisionConstants.TARGET_HEIGHT);
-      double xVel = (distance*VisionConstants.GRAV)/(yVel);
+        //START Equation Code
+      double yVel = Math.sqrt(2 * VisionConstants.GRAV * VisionConstants.TARGET_HEIGHT);
+      double xVel = (distance * VisionConstants.GRAV) / (yVel);
 
       fireVel = Math.sqrt((Math.pow(xVel, 2))+(Math.pow(yVel,2)));
-      fireAngle = Math.atan(yVel/xVel);
+      fireAngle = Math.atan(yVel/xVel) * (180/Math.PI);
+        //END Equation Code
+
+        /*//START CSV Code
+      fireVel = m_shooterTable.getVelocity(distance);
+      fireAngle = m_shooterTable.getHood(distance); //Note: Ensure to follow because units are different
+        //END CSV Code*/
+
+      
       m_shooter.m_fireVel = fireVel;
-      m_shooter.m_fireAngle = fireAngle;
+      m_shooter.m_fireAngle = fireAngle + m_shooter.shooterTrims.m_hoodTrim;
     }
   }
 
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-      //Drive Camera Mode
-    NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
-    NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0);
+
   }
 
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
+    if (xAngle < 1 && xAngle > -1 && target == 1)
+    {
+      SmartDashboard.putBoolean("TrackTarget Finished", true);
+      return true;
+    }
+    SmartDashboard.putBoolean("TrackTarget Finished", false);
     return false;
   }
 }
diff --git a/src/main/java/frc4388/robot/commands/TrimShooter.java b/src/main/java/frc4388/robot/commands/TrimShooter.java
new file mode 100644
index 0000000..b77a4ba
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/TrimShooter.java
@@ -0,0 +1,69 @@
+/*----------------------------------------------------------------------------*/
+/* 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.wpilibj.Joystick;
+import frc4388.utility.controller.XboxController;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.RobotContainer;
+import frc4388.robot.Trims;
+import frc4388.robot.Constants.OIConstants;
+import frc4388.robot.subsystems.Shooter;
+import frc4388.robot.subsystems.ShooterAim;
+
+public class TrimShooter extends CommandBase {
+  private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
+  public double turretTrim = 0;
+  public double hoodTrim = 0;
+
+  public Shooter m_shooter;
+  /**
+   * Trims the shooter based on the D-Pad inputs
+   * @param shootSub The Shooter subsytems
+   */
+  public TrimShooter(Shooter shootSub) {
+    m_shooter = shootSub;
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+
+    if(m_operatorXbox.getDPadTop()){
+      hoodTrim += 1;
+    }
+    else if(m_operatorXbox.getDPadBottom()){
+      hoodTrim -= 1;
+    }
+    else if(m_operatorXbox.getDPadRight()){
+      turretTrim += 1;
+    }
+    else if(m_operatorXbox.getDPadLeft()){
+      turretTrim -= 1;
+    }
+
+    m_shooter.shooterTrims.m_turretTrim = turretTrim;
+    m_shooter.shooterTrims.m_hoodTrim = hoodTrim;
+  }
+
+  // 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/subsystems/Camera.java b/src/main/java/frc4388/robot/subsystems/Camera.java
index e483009..332bb46 100644
--- a/src/main/java/frc4388/robot/subsystems/Camera.java
+++ b/src/main/java/frc4388/robot/subsystems/Camera.java
@@ -11,6 +11,7 @@ import edu.wpi.cscore.MjpegServer;
 import edu.wpi.cscore.UsbCamera;
 import edu.wpi.cscore.VideoSource;
 import edu.wpi.first.cameraserver.CameraServer;
+import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 
@@ -38,8 +39,7 @@ public class Camera extends SubsystemBase {
     catch(Exception e){
       System.err.println("Camera broken, pls nerf");
     }
-
-  }
+  }  
 
   @Override
   public void periodic() {
diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java
index 8c2e2bd..096feff 100644
--- a/src/main/java/frc4388/robot/subsystems/Drive.java
+++ b/src/main/java/frc4388/robot/subsystems/Drive.java
@@ -107,6 +107,7 @@ public class Drive extends SubsystemBase {
     m_pigeon.configFactoryDefault();
     resetGyroYaw();
 
+
     /* Config Open Loop Ramp so we don't make sudden output changes */
     m_rightFrontMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
     m_rightBackMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
diff --git a/src/main/java/frc4388/robot/subsystems/LimeLight.java b/src/main/java/frc4388/robot/subsystems/LimeLight.java
new file mode 100644
index 0000000..d8e39a4
--- /dev/null
+++ b/src/main/java/frc4388/robot/subsystems/LimeLight.java
@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* 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.subsystems;
+
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class LimeLight extends SubsystemBase {
+  /**
+   * Creates a new LimeLight.
+   */
+  public LimeLight() {
+
+  }
+
+  public void limeOff(){
+    NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
+    NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0);
+  }
+  
+  public void limeOn(){
+    NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
+    NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3);
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}
diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java
index 9bca6c9..cd0dda3 100644
--- a/src/main/java/frc4388/robot/subsystems/Shooter.java
+++ b/src/main/java/frc4388/robot/subsystems/Shooter.java
@@ -13,9 +13,12 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkMax.IdleMode;
+import com.revrobotics.CANSparkMax.SoftLimitDirection;
+import com.revrobotics.CANDigitalInput;
 import com.revrobotics.CANEncoder;
 import com.revrobotics.CANPIDController;
 import com.revrobotics.ControlType;
+import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 
 import edu.wpi.first.wpilibj.Joystick;
@@ -23,6 +26,7 @@ import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Gains;
+import frc4388.robot.Trims;
 import frc4388.robot.Constants.ShooterConstants;
 import frc4388.utility.ShooterTables;
 import frc4388.utility.controller.IHandController;
@@ -31,22 +35,15 @@ public class Shooter extends SubsystemBase {
 
   public WPI_TalonFX m_shooterFalcon = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_ID);
   public CANSparkMax m_angleAdjustMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless);
-  public CANSparkMax m_shooterRotateMotor = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, MotorType.kBrushless);
 
+  CANPIDController m_angleAdjustPIDController = m_angleAdjustMotor.getPIDController();
+  public CANEncoder m_angleEncoder = m_angleAdjustMotor.getEncoder();
 
-  public static Gains m_shooterTurretGains = ShooterConstants.SHOOTER_TURRET_GAINS;
   public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS;
+  public static Gains m_angleAdjustGains = ShooterConstants.SHOOTER_ANGLE_GAINS;
   public static Shooter m_shooter;
   public static IHandController m_controller;
-
-
-  // Configure PID Controllers
-  CANPIDController m_angleAdjustPIDController = m_angleAdjustMotor.getPIDController();
-  CANPIDController m_shooterRotatePIDController = m_shooterRotateMotor.getPIDController();
-
-  CANEncoder m_angleEncoder = m_angleAdjustMotor.getEncoder();
-  CANEncoder m_shooterRotateEncoder = m_shooterRotateMotor.getEncoder();
-
+  
   double velP;
   double input;
 
@@ -55,20 +52,23 @@ public class Shooter extends SubsystemBase {
   public boolean velReached;
   public double m_fireVel;
   public double m_fireAngle;
+  CANDigitalInput m_hoodUpLimit, m_hoodDownLimit;
+
+  public Trims shooterTrims;
   
   /*
-   * Creates a new Shooter subsystem.
+   * Creates a new Shooter subsystem, with the drum shooter and the angle adjsuter.
    */
   public Shooter() {
     //Testing purposes reseting gyros
-    resetGyroAngleAdj();
-    resetGyroShooterRotate();
+    //resetGyroAngleAdj();
+    shooterTrims = new Trims(0, 0);
 
     m_shooterFalcon.configFactoryDefault();
-    m_shooterRotateMotor.setIdleMode(IdleMode.kBrake);
     m_shooterFalcon.setNeutralMode(NeutralMode.Coast);
-    m_shooterFalcon.setInverted(false);
-    
+    m_shooterFalcon.setInverted(true);
+    m_angleAdjustMotor.setIdleMode(IdleMode.kBrake);
+    m_shooterFalcon.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
     setShooterGains();
 
     m_shooterFalcon.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS);
@@ -90,6 +90,19 @@ public class Shooter extends SubsystemBase {
     SmartDashboard.putNumber("CSV A 10", m_shooterTable.getAngleDisplacement(10));
     SmartDashboard.putNumber("CSV A 5", m_shooterTable.getAngleDisplacement(5));
     SmartDashboard.putNumber("CSV A 30", m_shooterTable.getAngleDisplacement(30));
+
+
+
+    m_hoodUpLimit = m_angleAdjustMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
+    m_hoodDownLimit = m_angleAdjustMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
+    m_hoodUpLimit.enableLimitSwitch(true);
+    m_hoodDownLimit.enableLimitSwitch(true);
+
+    m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, true);
+    m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
+    m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.HOOD_UP_SOFT_LIMIT);
+    m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.HOOD_DOWN_SOFT_LIMIT);
+    
   }
 
   @Override
@@ -105,6 +118,7 @@ public class Shooter extends SubsystemBase {
     return m_fireAngle;
   }
 
+
   /**
    * Runs drum shooter motor.
    * @param speed Speed to set the motor at
@@ -118,89 +132,47 @@ public class Shooter extends SubsystemBase {
    */
   public void setShooterGains() {
     m_shooterFalcon.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX);
-    m_shooterFalcon.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterTurretGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS);
-    m_shooterFalcon.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterTurretGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS);
-    m_shooterFalcon.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterTurretGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS);
-    m_shooterFalcon.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterTurretGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS);
-  }
-  /**
-   * Runs drum shooter velocity PID.
-   * @param falcon Motor to use
-   * @param targetVel Target velocity to run motor at
-   */
-  public void runDrumShooterVelocityPID(double targetVel, double actualVel) {
-    velP = actualVel/targetVel; //Percent of target
-    double runSpeed = actualVel + (12000*velP); //Ramp up equation
-    SmartDashboard.putNumber("shoot", actualVel);
-    runSpeed = runSpeed/targetVel; //Convert to percent
-
-    if (actualVel < targetVel - 1000){ //PID Based on ramp up amount
-      m_shooterFalcon.set(TalonFXControlMode.PercentOutput, runSpeed);
-    }
-    else{ //PID Based on targetVel
-      m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID
-    }
-
-      //Tells wether the target velocity has been reached
-    double upperBound = targetVel + 300;
-    double lowerBound = targetVel - 300;
-    if (actualVel < upperBound && actualVel > lowerBound)
-    {
-      velReached = true;
-    }
-    else{
-      velReached = false;
-    }
-  }
-
-
-  public void runShooterWithInput(double input) {
-    m_shooterRotateMotor.set(input);
+    m_shooterFalcon.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    m_shooterFalcon.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    m_shooterFalcon.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    m_shooterFalcon.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS);
   }
 
   /* Angle Adjustment PID Control */
   public void runAngleAdjustPID(double targetAngle)
   {
     // Set PID Coefficients
-    m_angleAdjustPIDController.setP(m_shooterTurretGains.m_kP);
-    m_angleAdjustPIDController.setI(m_shooterTurretGains.m_kI);
-    m_angleAdjustPIDController.setD(m_shooterTurretGains.m_kD);
-    m_angleAdjustPIDController.setIZone(m_shooterTurretGains.m_kIzone);
-    m_angleAdjustPIDController.setFF(m_shooterTurretGains.m_kF);
-    m_angleAdjustPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTurretGains.m_kPeakOutput); 
-
-    // Convert input angle in degrees to rotations of the motor
-    targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT;
+    m_angleAdjustPIDController.setP(m_angleAdjustGains.m_kP);
+    m_angleAdjustPIDController.setI(m_angleAdjustGains.m_kI);
+    m_angleAdjustPIDController.setD(m_angleAdjustGains.m_kD);
+    m_angleAdjustPIDController.setIZone(m_angleAdjustGains.m_kIzone);
+    m_angleAdjustPIDController.setFF(m_angleAdjustGains.m_kF);
+    m_angleAdjustPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_angleAdjustGains.m_kPeakOutput); 
 
     m_angleAdjustPIDController.setReference(targetAngle, ControlType.kPosition);
   }
 
-  /* Rotate Shooter PID Control */
-  public void runshooterRotatePID(double targetAngle)
-  {
-    // Set PID Coefficients
-    m_shooterRotatePIDController.setP(m_shooterTurretGains.m_kP);
-    m_shooterRotatePIDController.setI(m_shooterTurretGains.m_kI);
-    m_shooterRotatePIDController.setD(m_shooterTurretGains.m_kD);
-    m_shooterRotatePIDController.setFF(m_shooterTurretGains.m_kF);
-    m_shooterRotatePIDController.setIZone(m_shooterTurretGains.m_kIzone);
-    m_shooterRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTurretGains.m_kPeakOutput); 
-
-    // Convert input angle in degrees to rotations of the motor
-    targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT;
-
-    m_shooterRotatePIDController.setReference(targetAngle, ControlType.kPosition);
+  /**
+   * Runs drum shooter velocity PID.
+   * @param falcon Motor to use
+   * @param targetVel Target velocity to run motor at
+   */
+  public void runDrumShooterVelocityPID(double targetVel, double actualVel) {
+    SmartDashboard.putNumber("shoot", actualVel);
+    if (actualVel < targetVel - ShooterConstants.DRUM_RAMP_LIMIT){
+      m_shooterFalcon.set(TalonFXControlMode.PercentOutput, 1);//Ramp up
+    }
+    else{ //PID Based on targetVel
+      m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID
+    }
+    m_shooterFalcon.feed();
   }
 
-  /* For Testing Purposes, reseting gyro for angle adjuster */
-  public void resetGyroAngleAdj()
-  {
-    m_angleEncoder.setPosition(0);
+  public void resetGyroAngleAdj(){
+      m_angleEncoder.setPosition(0);
   }
 
-  /* For Testing Purposes, reseting gyro for shooter rotation */
-  public void resetGyroShooterRotate()
-  {
-    m_shooterRotateEncoder.setPosition(0);
+  public double getAnglePosition(){
+    return m_angleAdjustMotor.getEncoder().getPosition();
   }
 }
\ No newline at end of file
diff --git a/src/main/java/frc4388/robot/subsystems/ShooterAim.java b/src/main/java/frc4388/robot/subsystems/ShooterAim.java
new file mode 100644
index 0000000..1111749
--- /dev/null
+++ b/src/main/java/frc4388/robot/subsystems/ShooterAim.java
@@ -0,0 +1,89 @@
+/*----------------------------------------------------------------------------*/
+/* 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.subsystems;
+
+import com.revrobotics.CANDigitalInput;
+import com.revrobotics.CANEncoder;
+import com.revrobotics.CANPIDController;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.ControlType;
+import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
+import com.revrobotics.CANSparkMax.IdleMode;
+import com.revrobotics.CANSparkMax.SoftLimitDirection;
+import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.Gains;
+import frc4388.robot.Constants.ShooterConstants;
+
+public class ShooterAim extends SubsystemBase {
+  public CANSparkMax m_shooterRotateMotor = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, MotorType.kBrushless);
+  public static Gains m_shooterTurretGains = ShooterConstants.SHOOTER_TURRET_GAINS;
+  CANDigitalInput m_shooterRightLimit, m_shooterLeftLimit;
+
+    // Configure PID Controllers
+    CANPIDController m_shooterRotatePIDController = m_shooterRotateMotor.getPIDController();
+    public CANEncoder m_shooterRotateEncoder = m_shooterRotateMotor.getEncoder();
+
+
+  /**
+   * Creates a subsytem for the turret aiming
+   */
+  public ShooterAim() {
+    //resetGyroShooterRotate();
+    m_shooterRotateMotor.setIdleMode(IdleMode.kBrake);
+
+    m_shooterLeftLimit = m_shooterRotateMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
+    m_shooterRightLimit = m_shooterRotateMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
+    m_shooterRightLimit.enableLimitSwitch(true);
+    m_shooterLeftLimit.enableLimitSwitch(true);
+
+    m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, false);
+    m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
+    m_shooterRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_RIGHT_SOFT_LIMIT);
+    m_shooterRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.TURRET_LEFT_SOFT_LIMIT);
+  }
+
+  public void runShooterWithInput(double input) {
+    m_shooterRotateMotor.set(input*ShooterConstants.TURRET_SPEED_MULTIPLIER);
+  }
+
+
+  /* Rotate Shooter PID Control */
+  public void runshooterRotatePID(double targetAngle)
+  {
+    // Set PID Coefficients
+    m_shooterRotatePIDController.setP(m_shooterTurretGains.m_kP);
+    m_shooterRotatePIDController.setI(m_shooterTurretGains.m_kI);
+    m_shooterRotatePIDController.setD(m_shooterTurretGains.m_kD);
+    m_shooterRotatePIDController.setFF(m_shooterTurretGains.m_kF);
+    m_shooterRotatePIDController.setIZone(m_shooterTurretGains.m_kIzone);
+    m_shooterRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTurretGains.m_kPeakOutput); 
+
+    // Convert input angle in degrees to rotations of the motor
+    targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT;
+
+    m_shooterRotatePIDController.setReference(targetAngle, ControlType.kPosition);
+  }
+
+    public void resetGyroShooterRotate()
+    {
+        m_shooterRotateEncoder.setPosition(0);
+    }
+
+    public double getShooterRotatePosition(){
+      return m_shooterRotateMotor.getEncoder().getPosition();
+    }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}
diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java
index b6eb85e..7bcc411 100644
--- a/src/main/java/frc4388/robot/subsystems/Storage.java
+++ b/src/main/java/frc4388/robot/subsystems/Storage.java
@@ -19,12 +19,14 @@ import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.command.WaitUntilCommand;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Gains;
 import frc4388.robot.Constants.StorageConstants;
 
 public class Storage extends SubsystemBase {
-  private CANSparkMax m_storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless);
+  public CANSparkMax m_storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless);
   private DigitalInput[] m_beamSensors = new DigitalInput[6];
 
   CANPIDController m_storagePIDController = m_storageMotor.getPIDController();
@@ -68,6 +70,11 @@ 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 */
   public void runStoragePositionPID(double targetPos){
     // Set PID Coefficients
@@ -78,6 +85,8 @@ public class Storage extends SubsystemBase {
     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());
     m_storagePIDController.setReference(targetPos, ControlType.kPosition);
   }
 
diff --git a/src/main/java/frc4388/utility/ShooterTables.java b/src/main/java/frc4388/utility/ShooterTables.java
index 638f140..dc7fa83 100644
--- a/src/main/java/frc4388/utility/ShooterTables.java
+++ b/src/main/java/frc4388/utility/ShooterTables.java
@@ -93,7 +93,7 @@ public class ShooterTables {
         SmartDashboard.putNumber("Distance last row 2", m_distance[m_distanceLength-1][2]);
     }
 
-    public double getHood(double distance) {
+    public double getHood(double distance) { //Rotations of motor
         int i = 0;
         while ((i < m_distanceLength) && (m_distance[i][m_columnDistance] < distance)) {
             i ++;
@@ -108,7 +108,7 @@ public class ShooterTables {
         }
     }
 
-    public double getVelocity(double distance) {
+    public double getVelocity(double distance) { //Units per 100ms
         int i = 0;
         while ((i < m_distanceLength) && (m_distance[i][m_columnDistance] < distance)) {
             i ++;