diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 92c1caa..dfe4926 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -32,31 +32,38 @@ public final class Constants {
         public static final int PIGEON_ID = 6;
 
         /* Drive Inversions */
-        public static final boolean isRightMotorInverted = false;
+        public static final boolean isRightMotorInverted = true;
         public static final boolean isLeftMotorInverted = false;
         public static final boolean isRightArcadeInverted = false;
         public static final boolean isAuxPIDInverted = false;
 
         /* Drive Configuration */
-        public static final double OPEN_LOOP_RAMP_RATE = 0.1;
+        public static final int DRIVE_TIMEOUT_MS = 30; // Use for all motor config
+        public static final double OPEN_LOOP_RAMP_RATE = 0.1; // Seconds from 0 to full power on motor
         public static final double NEUTRAL_DEADBAND = 0.04;
         public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG =
-            new SupplyCurrentLimitConfiguration(false, 40, 35, 0.01);
-        public static final int CLOSED_LOOP_TIME_MS = 1;
+            new SupplyCurrentLimitConfiguration(false, 60, 40, 2);
+        public static final int CLOSED_LOOP_TIME_MS = 1; // Higher numbers mean slower control loops
+
+        /* Drive Train Characteristics */
+        public static final double MOTOR_ROT_PER_WHEEL_ROT_HIGH = 5.13;
+        public static final double MOTOR_ROT_PER_WHEEL_ROT_LOW = 15;
+        public static final double WHEEL_DIAMETER_INCHES = 6;
+        public static final double TICKS_PER_GYRO_REV = 8192;
+        public static final double TICKS_PER_MOTOR_REV = 2048;
 
         /* PID Constants Drive*/
-        public static final int DRIVE_TIMEOUT_MS = 30;
         public static final Gains DRIVE_DISTANCE_GAINS_LOW = new Gains(0.1, 0.0, 1.0, 0.0, 0, 0.5);
-        public static final Gains DRIVE_VELOCITY_GAINS_LOW = new Gains(0.1, 0.0, 0.2, 0.025, 0, 1.0);
-        public static final Gains DRIVE_TURNING_GAINS_LOW = new Gains(0.5, 0.0, 0.05, 0.0, 0, 0.55);
-        public static final Gains DRIVE_MOTION_MAGIC_GAINS_LOW = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0);
-        public static final int DRIVE_CRUISE_VELOCITY = 20000;
-        public static final int DRIVE_ACCELERATION = 7000;
+        public static final Gains DRIVE_VELOCITY_GAINS_LOW = new Gains(0.05, 0.0, 1.0, 0.025, 0, 1.0);
+        public static final Gains DRIVE_TURNING_GAINS_LOW = new Gains(0.5, 0.0, 2.0, 0.0, 0, 0.55);
+        public static final Gains DRIVE_MOTION_MAGIC_GAINS_LOW = new Gains(0.1, 0.0, 0, 0.025, 0, 1.0);
+        public static final int DRIVE_CRUISE_VELOCITY = 30000;
+        public static final int DRIVE_ACCELERATION = 23000;
 
-        public static final Gains DRIVE_DISTANCE_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 0.5);
-        public static final Gains DRIVE_VELOCITY_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0);
-        public static final Gains DRIVE_TURNING_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 0.55);
-        public static final Gains DRIVE_MOTION_MAGIC_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0);
+        public static final Gains DRIVE_DISTANCE_GAINS_HIGH = new Gains(0.1, 0.0, 0.0, 0.0, 0, 0.5);
+        public static final Gains DRIVE_VELOCITY_GAINS_HIGH = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0);
+        public static final Gains DRIVE_TURNING_GAINS_HIGH = new Gains(0.2, 0.0, 0.0, 0.0, 0, 0.55);
+        public static final Gains DRIVE_MOTION_MAGIC_GAINS_HIGH = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0);
         public static final int DRIVE_CRUISE_VELOCITY_HIGH = 20000;
         public static final int DRIVE_ACCELERATION_HIGH = 7000;
 
@@ -80,13 +87,6 @@ public final class Constants {
 	    public final static int SLOT_TURNING = 2;
 	    public final static int SLOT_MOTION_MAGIC = 3;
         
-        /* Drive Train Characteristics */
-        public static final double TICKS_PER_MOTOR_REV = 2048;
-        public static final double MOTOR_ROT_PER_WHEEL_ROT_HIGH = 5.13;
-        public static final double MOTOR_ROT_PER_WHEEL_ROT_LOW = 15;
-        public static final double WHEEL_DIAMETER_INCHES = 6;
-        public static final double TICKS_PER_GYRO_REV = 8192;
-        
         /* Ratio Calculation */
         public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI;
         public static final double TICK_TIME_TO_SECONDS = 0.1;
@@ -105,39 +105,60 @@ 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 int INTAKE_SPARK_ID = -9;
-        public static final int EXTENDER_SPARK_ID = -10;
+    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 */
-        public static final int SHOOTER_FALCON_ID = -1;
-        public static final int SHOOTER_ANGLE_ADJUST_ID = -1;
-        public static final int SHOOTER_ROTATE_ID = 10;
+        public static final int SHOOTER_FALCON_ID = 8;
+        public static final int SHOOTER_ANGLE_ADJUST_ID = 10;
+        public static final int SHOOTER_ROTATE_ID = 9;
 
         /* PID Constants Shooter */
         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 {
-        public static final int CLIMBER_SPARK_ID = 10;
+        public static final int CLIMBER_SPARK_ID = 14;
     }
   
     public static final class LevelerConstants {
-        public static final int LEVELER_CAN_ID = -1;
+        public static final int LEVELER_CAN_ID = 15;
     }
   
     public static final class StorageConstants {
-        public static final int STORAGE_CAN_ID = -1;
+        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;
@@ -154,9 +175,18 @@ 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 {
+        public static final int PCM_MODULE_ID = 7;
+
+        public static final int SPEED_SHIFT_FORWARD_ID = 0;
+        public static final int SPEED_SHIFT_REVERSE_ID = 1;
+
+        public static final int COOL_FALCON_FORWARD_ID = 3;
+        public static final int COOL_FALCON_REVERSE_ID = 2;
     }
   
     public static final class LEDConstants {
@@ -166,8 +196,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/Robot.java b/src/main/java/frc4388/robot/Robot.java
index 1029754..9e827dd 100644
--- a/src/main/java/frc4388/robot/Robot.java
+++ b/src/main/java/frc4388/robot/Robot.java
@@ -77,7 +77,7 @@ public class Robot extends TimedRobot {
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
 
     m_robotContainer.setDriveNeutralMode(NeutralMode.Brake);
-    //m_robotContainer.setDriveGearState(true);
+    m_robotContainer.setDriveGearState(false);
     m_robotContainer.resetOdometry();
     //m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor);
 
@@ -105,7 +105,7 @@ public class Robot extends TimedRobot {
   @Override
   public void teleopInit() {
     m_robotContainer.setDriveNeutralMode(NeutralMode.Brake);
-    m_robotContainer.setDriveGearState(true);
+    m_robotContainer.setDriveGearState(false);
     //m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor);
     
     // This makes sure that the autonomous stops running when
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 0a152ba..ea39121 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,24 +29,61 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 
 import frc4388.robot.Constants.*;
+import frc4388.robot.commands.AutoPath1FromCenter;
+import frc4388.robot.commands.AutoPath2FromRight;
+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.DriveWithJoystickUsingDeadAssistPID;
+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.TurnDegrees;
 import frc4388.robot.commands.Wait;
 import frc4388.robot.commands.storageOutake;
+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.subsystems.Camera;
+import frc4388.robot.subsystems.Leveler;
+import frc4388.robot.subsystems.Pneumatics;
 import frc4388.robot.subsystems.Storage;
 import frc4388.utility.controller.IHandController;
 import frc4388.utility.controller.XboxController;
@@ -60,91 +98,103 @@ import frc4388.utility.controller.XboxController;
 public class RobotContainer {
     /* Subsystems */
     private final Drive m_robotDrive = new Drive();
+    private final Pneumatics m_robotPneumatics = new Pneumatics();
     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 */
+        m_robotPneumatics.passRequiredSubsystem(m_robotDrive);
+        m_robotDrive.passRequiredSubsystem(m_robotPneumatics);
+
         configureButtonBindings();
 
         /* Default Commands */
         // drives the robot with a two-axis input from the driver controller
-        m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, getDriverController()));
+        
+        m_robotDrive.setDefaultCommand(new DriveWithJoystickDriveStraight(m_robotDrive, getDriverController()));
+        //m_robotDrive.setDefaultCommand(new DriveWithJoystickUsingDeadAssistPID(m_robotDrive, m_robotPneumatics, 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
         new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
-            .whenPressed(new InstantCommand());
+            .whenPressed(new DriveStraightToPositionMM(m_robotDrive, m_robotPneumatics, 24.0));
         
         // B driver test button
         new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
-            .whenPressed(new InstantCommand());
+            .whenPressed(new TurnDegrees(m_robotDrive, 90));
 
         // Y driver test button
         new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
-            .whenPressed(new InstantCommand());
+            .whenPressed(new Wait(m_robotDrive, 0, 0));
 
         // X driver test button
         new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
             .whenPressed(new InstantCommand());
-     
+      
         /* Driver Buttons */
         // sets solenoids into high gear
         new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
-            .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive));
+            .whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(true), m_robotDrive));
 
         // sets solenoids into low gear
         new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON)
-            .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive));
+            .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));
@@ -154,17 +204,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));
+
     }
 
     /**
@@ -179,7 +250,9 @@ public class RobotContainer {
         RamseteCommand ramseteCommand = getRamseteCommand(trajectory);
         // Run path following command, then stop at the end.
         //return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
-
+        //return new AutoPath1FromCenter(m_robotDrive, m_robotPneumatics);
+        //return new AutoPath2FromRight(m_robotDrive, m_robotPneumatics);
+      
         if (Constants.SELECTED_AUTO == 1) {
           return new SequentialCommandGroup(new Wait(5, m_robotDrive),
                                             new TurnDegrees(45, m_robotDrive),
@@ -191,7 +264,6 @@ public class RobotContainer {
         return new InstantCommand();
         // return new DrivePositionMPAux(m_robotDrive, 500.0, 12.0, 2, 60.0, 0.0);
     }
-
     TrajectoryConfig getTrajectoryConfig() {
         return new TrajectoryConfig(
             DriveConstants.MAX_SPEED_METERS_PER_SECOND,
@@ -243,7 +315,7 @@ public class RobotContainer {
      * @param state the gearing of the gearbox (true is high, false is low)
      */
     public void setDriveGearState(boolean state) {
-        m_robotDrive.setShiftState(state);
+        m_robotPneumatics.setShiftState(state);
     }
 
     /**
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/AutoPath1FromCenter.java b/src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java
new file mode 100644
index 0000000..4ec080a
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java
@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------------*/
+/* 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.SequentialCommandGroup;
+import frc4388.robot.subsystems.Drive;
+import frc4388.robot.subsystems.Pneumatics;
+
+// 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 AutoPath1FromCenter extends SequentialCommandGroup {
+  Drive m_drive;
+  Pneumatics m_pneumatics;
+
+  /**
+   * Creates a new AutoPath1FromCenter.
+   */
+  public AutoPath1FromCenter(Drive subsystem, Pneumatics subsystem2) {
+    // Add your commands in the super() call, e.g.
+    // super(new FooCommand(), new BarCommand());
+    m_drive = subsystem;
+    m_pneumatics = subsystem2;
+
+    addCommands(  new Wait(m_drive, 0, 1), 
+                  //shoot pre-loaded 3 balls
+                  new GotoCoordinates(m_drive, m_pneumatics, 75, 44, -90),
+                  //Start Intake Ball 1
+                  new GotoCoordinates(m_drive, m_pneumatics, 0, 12),
+                  new GotoCoordinates(m_drive, m_pneumatics, 0, 28),
+                  //Start Intake Ball 2
+                  new GotoCoordinates(m_drive, m_pneumatics, 0, 8),
+                  new GotoCoordinates(m_drive, m_pneumatics, 0, 28),
+                  //Start Intake Ball 3
+                  new GotoCoordinates(m_drive, m_pneumatics, 0, 8),
+                  new Wait(m_drive, 0, 2)
+                  //Shoot 3 Balls
+                  );
+  }
+}
diff --git a/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java b/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java
new file mode 100644
index 0000000..43657c3
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* 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.SequentialCommandGroup;
+import frc4388.robot.subsystems.Drive;
+import frc4388.robot.subsystems.Pneumatics;
+
+// 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 AutoPath2FromRight extends SequentialCommandGroup {
+  Drive m_drive;
+  Pneumatics m_pneumatics;
+
+  /**
+   * Creates a new AutoPath2FromRight.
+   */
+  public AutoPath2FromRight(Drive subsystem, Pneumatics subsystem2) {
+    // Add your commands in the super() call, e.g.
+    // super(new FooCommand(), new BarCommand());
+    m_drive = subsystem;
+    m_pneumatics = subsystem2;
+    
+    addCommands(  new Wait(m_drive, 0, 1),
+                  new GotoCoordinates(m_drive, m_pneumatics, 0, 77),
+                  //Start Intake Ball 1
+                  new GotoCoordinates(m_drive, m_pneumatics, 0, 8),
+                  new GotoCoordinates(m_drive, m_pneumatics, 0, 28),
+                  //Start Intake Ball 2
+                  new GotoCoordinates(m_drive, m_pneumatics, 0, 8),
+                  //Shoot 5 Balls
+                  new GotoCoordinates(m_drive, m_pneumatics, 0, 28),
+                  //Start Intake Ball 1 (second round)
+                  new GotoCoordinates(m_drive, m_pneumatics, 0, 8),
+                  //Start Moving to 4th Ball
+                  new GotoCoordinates(m_drive, m_pneumatics, 60, -50),
+                  new Wait(m_drive, 0, 2)
+                  );
+  }
+}
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/DrivePositionMPAux.java b/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java
index 68d9538..68d8390 100644
--- a/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java
+++ b/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java
@@ -23,17 +23,18 @@ public class DrivePositionMPAux extends CommandBase {
   double m_rampAcc;
   long m_startTime;
   long m_rampRate;
+  int m_counter;
 
   /**
    * Creates a new DrivePositionMPAux.
    * 
    * @param subsystem The drive subsystem
-   * @param cruiseVel The target velocity for the motors in units
+   * @param cruiseVel The target velocity for the motors in in/s
    * @param rampDist  The distance before cruise velocity is reached in inches
    * @param rampRate  The time to reach the cruise velocity in seconds
    * @param targetPos The target position
    */
-  public DrivePositionMPAux(Drive subsystem, double cruiseVel, double rampDist, float rampRate, double targetPos, double targetGyro) {
+  public DrivePositionMPAux(Drive subsystem, double cruiseVel, double rampDist, float rampRate, double targetPos) {
     // Use addRequirements() here to declare subsystem dependencies.
     m_drive = subsystem;
     m_cruiseVel = cruiseVel * DriveConstants.TICKS_PER_INCH_LOW / 10;
@@ -54,6 +55,7 @@ public class DrivePositionMPAux extends CommandBase {
     m_targetVel = m_currentVel;
     m_startTime = System.currentTimeMillis();
     m_rampAcc = (m_cruiseVel - m_currentVel) / m_rampRate;
+    m_counter = 0;
   }
 
   // Called every m_isRamptime the scheduler runs while the command is scheduled.
@@ -64,14 +66,15 @@ public class DrivePositionMPAux extends CommandBase {
     if (System.currentTimeMillis() - m_startTime < m_rampRate) {
       // Ramping
       m_targetVel += m_rampAcc * m_drive.m_deltaTimeMs;
-      m_drive.runDriveVelocityPID(-m_targetVel, m_targetGyro);
+      m_drive.runDriveVelocityPID(m_targetVel, m_targetGyro);
     } else if (m_targetPos - m_currentPos > m_rampDist) {
       // Cruising
-      m_drive.runDriveVelocityPID(-m_cruiseVel, m_targetGyro);
+      m_drive.runDriveVelocityPID(m_cruiseVel, m_targetGyro);
     } else {
       // Deramp PID
-      m_drive.runDrivePositionPID(-m_targetPos, m_targetGyro);
+      m_drive.runDrivePositionPID(m_targetPos, m_targetGyro);
     }
+    m_counter ++;
   }
 
   // Called once the command ends or is interrupted.
@@ -82,8 +85,8 @@ public class DrivePositionMPAux extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (m_currentPos - m_targetPos <= 0.5f * DriveConstants.TICKS_PER_INCH_LOW) {
-      return true;
+    if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && (m_counter > 5)) {
+      //return true;
     }
     return false;
   }
diff --git a/src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java b/src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java
index 14cc97e..c79ccbc 100644
--- a/src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java
+++ b/src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java
@@ -37,7 +37,7 @@ public class DriveStraightAtVelocityPID extends CommandBase {
   @Override
   public void execute() {
     //System.err.println(m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
-    m_drive.runDriveVelocityPID(-m_targetVel, m_targetGyro);
+    m_drive.runDriveVelocityPID(m_targetVel, m_targetGyro);
   }
 
   // Called once the command ends or is interrupted.
diff --git a/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java b/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java
index c56a36b..c9aa927 100644
--- a/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java
+++ b/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java
@@ -11,9 +11,11 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.robot.Constants.DriveConstants;
 import frc4388.robot.subsystems.Drive;
+import frc4388.robot.subsystems.Pneumatics;
 
 public class DriveStraightToPositionMM extends CommandBase {
   Drive m_drive;
+  Pneumatics m_pneumatics;
   double m_targetPosIn;
   double m_targetPosOut;
   double m_targetGyro;
@@ -25,10 +27,19 @@ public class DriveStraightToPositionMM extends CommandBase {
    * @param subsystem drive subsystem
    * @param targetPos distance to travel in inches
    */
-  public DriveStraightToPositionMM(Drive subsystem, double targetPos) {
+  public DriveStraightToPositionMM(Drive subsystem, Pneumatics subsystem2, double targetPos) {
     // Use addRequirements() here to declare subsystem dependencies.
     m_drive = subsystem;
-    m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW;
+    m_pneumatics = subsystem2;
+    try {
+      if (m_pneumatics.m_isSpeedShiftHigh) {
+        m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_HIGH * 2;
+      } else {
+        m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW * 2;
+      }
+    } catch (Exception e) {
+      System.err.println("Error In Motion Magic Switch Gains.");
+    }
     addRequirements(m_drive);
     //SmartDashboard.putNumber("Distance Target Inches", targetPos);
   }
diff --git a/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java b/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java
index 949a4bf..a457e26 100644
--- a/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java
+++ b/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java
@@ -11,9 +11,11 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.robot.Constants.DriveConstants;
 import frc4388.robot.subsystems.Drive;
+import frc4388.robot.subsystems.Pneumatics;
 
 public class DriveStraightToPositionPID extends CommandBase {
   Drive m_drive;
+  Pneumatics m_pneumatics;
   double m_targetPosIn;
   double m_targetPosOut;
   double m_targetGyro;
@@ -24,10 +26,19 @@ public class DriveStraightToPositionPID extends CommandBase {
    * @param subsystem drive subsystem
    * @param targetPos distance to travel in inches
    */
-  public DriveStraightToPositionPID(Drive subsystem, double targetPos) {
+  public DriveStraightToPositionPID(Drive subsystem, Pneumatics subsystem2, double targetPos) {
     // Use addRequirements() here to declare subsystem dependencies.
     m_drive = subsystem;
-    m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW;
+    m_pneumatics = subsystem2;
+    try {
+      if (m_pneumatics.m_isSpeedShiftHigh) {
+        m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_HIGH * 2;
+      } else {
+        m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW * 2;
+      }
+    } catch (Exception e) {
+      System.err.println("Error In Motion Magic Switch Gains.");
+    }
     addRequirements(m_drive);
     //SmartDashboard.putNumber("Distance Target Inches", targetPos);
   }
@@ -58,7 +69,7 @@ public class DriveStraightToPositionPID extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && i > 5){
+    if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && i > 10){
       return true;
     } else {
       i++;
diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
index f51621a..3c6e88f 100644
--- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
+++ b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
@@ -7,13 +7,16 @@
 
 package frc4388.robot.commands;
 
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.robot.subsystems.Drive;
+import frc4388.robot.subsystems.Pneumatics;
 import frc4388.utility.controller.IHandController;
 
 public class DriveWithJoystick extends CommandBase {
   private Drive m_drive;
   private IHandController m_controller;
+  private Pneumatics m_pneumatics;
 
   /**
    * Creates a new DriveWithJoystick to control the drivetrain with an Xbox controller.
@@ -23,9 +26,10 @@ public class DriveWithJoystick extends CommandBase {
    * {@link frc4388.robot.RobotContainer#getDriverJoystick() getDriverJoystick()} method in
    * {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
    */
-  public DriveWithJoystick(Drive subsystem, IHandController controller) {
+  public DriveWithJoystick(Drive subsystem, Pneumatics subsystem2, IHandController controller) {
     // Use addRequirements() here to declare subsystem dependencies.
     m_drive = subsystem;
+    m_pneumatics = subsystem2;
     m_controller = controller;
     addRequirements(m_drive);
   }
@@ -48,8 +52,15 @@ public class DriveWithJoystick extends CommandBase {
       moveOutput = Math.cos(1.571*moveInput)-1;
     }
 
-    double cosMultiplier = .55;
+    double cosMultiplier = 1.0;
     double deadzone = .1;
+
+    if (m_pneumatics.m_isSpeedShiftHigh) {
+      cosMultiplier = 0.8;
+    } else {
+      cosMultiplier = 1.0;
+    }
+
     if (steerInput > 0){
       steerOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*steerInput) + cosMultiplier;
     } else if (steerInput < 0) {
@@ -58,6 +69,31 @@ public class DriveWithJoystick extends CommandBase {
       steerOutput = 0;
     }
 
+    /*
+    double outputLimit = 0.8;
+
+    boolean isMoveOutputLimited = false;
+    boolean isSteerOutputLimited = false;
+    
+    if (m_pneumatics.m_isSpeedShiftHigh) {
+      if (isMoveOutputLimited) {
+        if (moveOutput > outputLimit) {
+          moveOutput = outputLimit;
+        } else if(moveOutput < -outputLimit) {
+          moveOutput = -outputLimit;
+        }
+      }
+
+      if (isSteerOutputLimited) {
+        if (steerOutput > outputLimit) {
+          steerOutput = outputLimit;
+        } else if(steerOutput < -outputLimit) {
+          steerOutput = -outputLimit;
+        }
+      }
+    } 
+  */
+    
     m_drive.driveWithInput(moveOutput, steerOutput);
   }
 
diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java b/src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java
index a298f9c..ef9c1b9 100644
--- a/src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java
+++ b/src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java
@@ -21,6 +21,7 @@ public class DriveWithJoystickDriveStraight extends CommandBase {
   long m_deadTimeSteer, m_deadTimeMove;
   long m_deadTimeout = 100;
   IHandController m_controller;
+  boolean m_isInterrupted;
 
   /**
    * Creates a new DriveWithJoystickDriveStraight to control the drivetrain with an Xbox controller.
@@ -54,6 +55,11 @@ public class DriveWithJoystickDriveStraight extends CommandBase {
     m_deltaTime = System.currentTimeMillis() - m_currTime;
     m_currTime = System.currentTimeMillis();
 
+    if (m_isInterrupted) {
+      resetGyroTarget();
+      m_isInterrupted = false;
+    }
+
     /* If steer stick is being used */
     if (steerInput != 0) {
       m_deadTimeSteer = m_currTime;
@@ -78,14 +84,14 @@ public class DriveWithJoystickDriveStraight extends CommandBase {
   }
 
   private void runDriveWithInput(double move, double steer) {
-    double cosMultiplier = .45;
+    double cosMultiplier = .7;
     double steerOutput = 0;
-    double deadzone = .2;
+    double deadzone = .1;
     /* Curves the steer output to be similarily gradual */
-    if (steer > 0){
-      steerOutput = -cosMultiplier*Math.cos(1.571*steer)+(cosMultiplier+deadzone);
-    } else {
-      steerOutput = cosMultiplier*Math.cos(1.571*steer)-(cosMultiplier+deadzone);
+    if (steer > 0) {
+      steerOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*steer) + cosMultiplier;
+    } else if (steer < 0) {
+      steerOutput = (cosMultiplier - deadzone) * Math.cos(1.571*steer) - cosMultiplier;
     }
     m_drive.driveWithInput(move, steerOutput);
     System.out.println("Driving With Input");
@@ -108,6 +114,7 @@ public class DriveWithJoystickDriveStraight extends CommandBase {
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
+    m_isInterrupted = interrupted;
   }
 
   // Returns true when the command should end.
diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java
index 12d605c..0b154e7 100644
--- a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java
+++ b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java
@@ -12,10 +12,12 @@ import edu.wpi.first.wpilibj2.command.CommandBase;
 import edu.wpi.first.wpiutil.math.MathUtil;
 import frc4388.robot.Constants.DriveConstants;
 import frc4388.robot.subsystems.Drive;
+import frc4388.robot.subsystems.Pneumatics;
 import frc4388.utility.controller.IHandController;
 
 public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
   Drive m_drive;
+  Pneumatics m_pneumatics;
   double m_targetGyro, m_currentGyro;
   double m_stopPos;
   long m_currTime, m_deltaTime;
@@ -44,14 +46,15 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
    * Creates a new DriveWithJoystickUsingDeadAssistPID to control the drivetrain with an Xbox controller.
    * Applies a curved ramp to the input from the controllers to make the robot less "touchy".
    * Also uses PIDs to keep the robot on course when given a "dead" or 0 input.
-   * @param subsystem pass the Drive subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
+   * @param subsystemDrive pass the Drive subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
    * @param controller pass the Driver {@link frc4388.utility.controller.IHandController#getClass() IHandController} using the
    * {@link frc4388.robot.RobotContainer#getDriverJoystick() getDriverJoystick()} method in
    * {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
    */
-  public DriveWithJoystickUsingDeadAssistPID(Drive subsystem, IHandController controller) {
+  public DriveWithJoystickUsingDeadAssistPID(Drive subsystemDrive, Pneumatics subsystemPneumatics, IHandController controller) {
     // Use addRequirements() here to declare subsystem dependencies.
-    m_drive = subsystem;
+    m_drive = subsystemDrive;
+    m_pneumatics = subsystemPneumatics;
     m_controller = controller;
     addRequirements(m_drive);
   }
@@ -96,7 +99,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
       moveOutput = Math.cos(1.571*moveInput)-1;
     }
 
-    if (m_drive.m_isSpeedShiftHigh) {
+    if (m_pneumatics.m_isSpeedShiftHigh) {
       runDriveWithInput(moveOutput, steerInput);
       resetGyroTarget();
     }
@@ -119,14 +122,14 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
   }
 
   private void runDriveWithInput(double move, double steerInput) {
-    double cosMultiplier = .55;
+    double cosMultiplier = .70;
     double steerOutput = 0;
-    double deadzone = .2;
+    double deadzone = .1;
     /* Curves the steer output to be similarily gradual */
     if (steerInput > 0){
-      steerOutput = -cosMultiplier*Math.cos(1.571*steerInput)+(cosMultiplier+deadzone);
+      steerOutput = -(cosMultiplier - deadzone)*Math.cos(1.571*steerInput)+(cosMultiplier);
     } else if (steerInput < 0) {
-      steerOutput = cosMultiplier*Math.cos(1.571*steerInput)-(cosMultiplier+deadzone);
+      steerOutput = (cosMultiplier - deadzone)*Math.cos(1.571*steerInput)-(cosMultiplier);
     }
 
     m_drive.driveWithInput(move, steerOutput);
diff --git a/src/main/java/frc4388/robot/commands/GotoCoordinates.java b/src/main/java/frc4388/robot/commands/GotoCoordinates.java
new file mode 100644
index 0000000..6ca3deb
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/GotoCoordinates.java
@@ -0,0 +1,87 @@
+/*----------------------------------------------------------------------------*/
+/* 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.SequentialCommandGroup;
+import frc4388.robot.subsystems.Drive;
+import frc4388.robot.subsystems.Pneumatics;
+
+// 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 GotoCoordinates extends SequentialCommandGroup {
+  Drive m_drive;
+  Pneumatics m_pneumatics;
+
+  double m_xTarget;
+  double m_yTarget;
+  double m_currentAngle;
+  double m_hypotDist;
+  double m_endAngle;
+
+  /**
+   * Creates a new GotoPosition.
+   */
+  public GotoCoordinates(Drive subsystem, Pneumatics subsystem2, double xTarget, double yTarget, double endAngle) {
+    // Add your commands in the super() call, e.g.
+    // super(new FooCommand(), new BarCommand());
+    m_drive = subsystem;
+    m_pneumatics = subsystem2;
+
+    m_xTarget = xTarget;
+    m_yTarget = yTarget;
+
+    m_hypotDist = Math.sqrt((m_xTarget*m_xTarget) + (m_yTarget*m_yTarget));
+
+    m_currentAngle = calcAngle();
+
+    m_endAngle = endAngle;
+
+    addCommands(  new TurnDegrees(m_drive, m_currentAngle),
+                  new Wait(m_drive, 0, 0),
+                  new DriveStraightToPositionPID(m_drive, m_pneumatics, m_hypotDist), 
+                  new TurnDegrees(m_drive, m_endAngle - m_currentAngle));
+  }
+
+  public GotoCoordinates(Drive subsystem, Pneumatics subsystem2, double xTarget, double yTarget) {
+    m_drive = subsystem;
+    m_pneumatics = subsystem2;
+
+    m_xTarget = xTarget;
+    m_yTarget = yTarget;
+
+    m_hypotDist = Math.sqrt((m_xTarget*m_xTarget) + (m_yTarget*m_yTarget));
+
+    m_currentAngle = calcAngle();
+
+    m_endAngle = m_currentAngle;
+
+    addCommands(  new TurnDegrees(m_drive, m_currentAngle),
+                  new Wait(m_drive, 0, 0),
+                  new DriveStraightToPositionPID(m_drive, m_pneumatics, m_hypotDist), 
+                  new TurnDegrees(m_drive, m_endAngle - m_currentAngle));
+  }
+
+  public boolean isQuadrantThree() {
+    if ((m_xTarget < 0) && (m_yTarget < 0)) {
+      return true;
+    } else {
+      return false;
+    }
+  }
+
+  public double calcAngle() {
+    if (isQuadrantThree()) {
+      return 360 + (Math.atan2(m_yTarget, m_xTarget) * (180 / Math.PI)) - 90;
+    } else {
+      return (Math.atan2(m_yTarget, m_xTarget) * (180 / Math.PI)) - 90;
+    }
+  }
+
+}
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/RunIntakeWithTriggers.java b/src/main/java/frc4388/robot/commands/RunIntakeWithTriggers.java
index bb7cef2..5f9dc4a 100644
--- a/src/main/java/frc4388/robot/commands/RunIntakeWithTriggers.java
+++ b/src/main/java/frc4388/robot/commands/RunIntakeWithTriggers.java
@@ -41,15 +41,15 @@ public class RunIntakeWithTriggers extends CommandBase {
     double rightTrigger = m_controller.getRightTriggerAxis();
     double leftTrigger = m_controller.getLeftTriggerAxis();
     double output = 0;
-    if (rightTrigger < .5) {
-      if(rightTrigger > leftTrigger) {
-        output = rightTrigger;
+    if (leftTrigger < .5) {
+      if(leftTrigger > rightTrigger) {
+        output = leftTrigger;
       }
-      if (leftTrigger > rightTrigger) {
+      if (rightTrigger > leftTrigger) {
         output = -leftTrigger;
       }
     } else {
-      output = rightTrigger;
+      output = leftTrigger;
     }
     m_intake.runIntake(output);
   }
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/commands/TurnDegrees.java b/src/main/java/frc4388/robot/commands/TurnDegrees.java
index 2b47050..83630dd 100644
--- a/src/main/java/frc4388/robot/commands/TurnDegrees.java
+++ b/src/main/java/frc4388/robot/commands/TurnDegrees.java
@@ -24,7 +24,7 @@ public class TurnDegrees extends CommandBase {
   /**
    * Creates a new TurnDeg.
    */
-  public TurnDegrees(double targetAngle, Drive subsystem) {
+  public TurnDegrees(Drive subsystem, double targetAngle) {
     // Use addRequirements() here to declare subsystem dependencies.
 
     m_targetAngle = targetAngle;
@@ -64,7 +64,7 @@ public class TurnDegrees extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if ((Math.abs(m_drive.getTurnRate()) < 1) && (i > 5)) {
+    if ((Math.abs(m_drive.getTurnRate()) < 1) && (Math.abs(m_currentYawInTicks - m_targetAngleTicksOut) < 70)) {
       return true;
     }
     return false;
diff --git a/src/main/java/frc4388/robot/commands/Wait.java b/src/main/java/frc4388/robot/commands/Wait.java
index 934b508..580d279 100644
--- a/src/main/java/frc4388/robot/commands/Wait.java
+++ b/src/main/java/frc4388/robot/commands/Wait.java
@@ -17,15 +17,19 @@ public class Wait extends CommandBase {
   long m_waitTime;
   long m_currentTime;
   SubsystemBase m_subsystem;
+  int m_waitNum;
+
+  int counter  = 0;
 
   /**
    * Creates a new WaitCommand.
    */
-  public Wait(float seconds, SubsystemBase subsystem) {
+  public Wait(SubsystemBase subsystem, double seconds, int waitNum) {
     // Use addRequirements() here to declare subsystem dependencies.
 
     m_waitTime = (long) (seconds * 1000);
     m_subsystem = subsystem;
+    m_waitNum = waitNum;
 
     addRequirements(m_subsystem);
   }
@@ -40,8 +44,15 @@ public class Wait extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
+
+    if (counter == 0) {
+      SmartDashboard.putNumber("Wait Coordinates" + m_waitNum, m_currentTime);
+    }
+
     m_currentTime = System.currentTimeMillis();
     SmartDashboard.putNumber("Time Difference for Wait", (m_currentTime - m_startTime));
+
+    counter ++;
   }
 
   // Called once the command ends or is interrupted.
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/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java
index f6c9339..e9adf86 100644
--- a/src/main/java/frc4388/robot/subsystems/Climber.java
+++ b/src/main/java/frc4388/robot/subsystems/Climber.java
@@ -31,10 +31,10 @@ public class Climber extends SubsystemBase {
     m_climberMotor.setIdleMode(IdleMode.kBrake);
     m_climberMotor.setInverted(false);
     
-    m_climberForwardLimit = m_climberMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
-    m_climberReverseLimit = m_climberMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
-    m_climberForwardLimit.enableLimitSwitch(false);
-    m_climberReverseLimit.enableLimitSwitch(false);
+    m_climberForwardLimit = m_climberMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
+    m_climberReverseLimit = m_climberMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
+    m_climberForwardLimit.enableLimitSwitch(true);
+    m_climberReverseLimit.enableLimitSwitch(true);
   }
 
   @Override
diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java
index fca1e7b..059d3c6 100644
--- a/src/main/java/frc4388/robot/subsystems/Drive.java
+++ b/src/main/java/frc4388/robot/subsystems/Drive.java
@@ -36,23 +36,25 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 import frc4388.robot.Constants.DriveConstants;
+import frc4388.robot.Constants.PneumaticsConstants;
 import frc4388.robot.Gains;
 
 public class Drive extends SubsystemBase {
-  /* Create Motors, Gyros, Solenoids, etc */
+  /* Create Motors, Gyros, etc */
   public WPI_TalonFX m_leftFrontMotor = new WPI_TalonFX(DriveConstants.DRIVE_LEFT_FRONT_CAN_ID);
   public WPI_TalonFX m_rightFrontMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_FRONT_CAN_ID);
   public WPI_TalonFX m_leftBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_LEFT_BACK_CAN_ID);
   public WPI_TalonFX m_rightBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_BACK_CAN_ID);
   public static PigeonIMU m_pigeon = new PigeonIMU(DriveConstants.PIGEON_ID);
-  public DoubleSolenoid m_speedShift = new DoubleSolenoid(7,0,1);
-  public DoubleSolenoid m_coolFalcon = new DoubleSolenoid(7,3,2);
   
   /* Drive objects to manage Drive Train */
   public DifferentialDrive m_driveTrain;
   public final DifferentialDriveOdometry m_odometry;
   public Orchestra m_orchestra;
 
+  /* Pneumatics Subsystem */
+  Pneumatics m_pneumaticsSubsystem;
+
   /* Low Gear Gains */
   public static Gains m_gainsDistanceLow = DriveConstants.DRIVE_DISTANCE_GAINS_LOW;
   public static Gains m_gainsVelocityLow = DriveConstants.DRIVE_VELOCITY_GAINS_LOW;
@@ -86,11 +88,11 @@ public class Drive extends SubsystemBase {
   public double m_lastAngleYaw = 0;
   public double m_currentAngleYaw = 0;
 
+  public double m_lastAngleGotoCoordinates;
   /* Smart Dashboard Objects */
   SendableChooser<String> m_songChooser = new SendableChooser<String>();
   
   /* Misc */
-  public boolean m_isSpeedShiftHigh;
   String m_currentSong = "";
 
   /**
@@ -105,12 +107,6 @@ public class Drive extends SubsystemBase {
     m_pigeon.configFactoryDefault();
     resetGyroYaw();
 
-    /* flip input so forward becomes back, etc */
-    m_leftFrontMotor.setInverted(DriveConstants.isLeftMotorInverted);
-    m_rightFrontMotor.setInverted(DriveConstants.isRightMotorInverted);
-    m_leftBackMotor.setInverted(DriveConstants.isLeftMotorInverted);
-    m_rightBackMotor.setInverted(DriveConstants.isRightMotorInverted);
-    m_driveTrain.setRightSideInverted(DriveConstants.isRightArcadeInverted);
 
     /* Config Open Loop Ramp so we don't make sudden output changes */
     m_rightFrontMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
@@ -118,11 +114,16 @@ public class Drive extends SubsystemBase {
     m_leftFrontMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
     m_leftBackMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
 
+    m_rightFrontMotor.configClosedloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightBackMotor.configClosedloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_leftFrontMotor.configClosedloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_leftBackMotor.configClosedloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
+
     /* Config Supply Current Limit (Use only for debugging) */
-    m_rightFrontMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
-    m_leftFrontMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
-    m_rightBackMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
-    m_leftBackMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
+    //m_rightFrontMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
+    //m_leftFrontMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
+    //m_rightBackMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
+    //m_leftBackMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
 
     /* Config deadbands so that  */
     m_leftBackMotor.configNeutralDeadband(DriveConstants.NEUTRAL_DEADBAND, DriveConstants.DRIVE_TIMEOUT_MS);
@@ -131,7 +132,15 @@ public class Drive extends SubsystemBase {
     m_rightFrontMotor.configNeutralDeadband(DriveConstants.NEUTRAL_DEADBAND, DriveConstants.DRIVE_TIMEOUT_MS);
 
     /* PID for Front Motor Control in Teleop */
-    setRightMotorGains(false);
+    try {
+      if (m_pneumaticsSubsystem.m_isSpeedShiftHigh) {
+        setRightMotorGains(true);
+      } else {
+        setRightMotorGains(false);
+      }
+    } catch (Exception e) {
+      System.err.println("Error while trying to switch gains.");
+    }
 
     /* PID for Back Motor Control in Tank Drive Vel */
     m_rightBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
@@ -251,6 +260,13 @@ public class Drive extends SubsystemBase {
 
     /* Set up Orchestra */
     m_orchestra = new Orchestra();
+
+    /* flip input so forward becomes back, etc */
+    m_leftFrontMotor.setInverted(DriveConstants.isLeftMotorInverted);
+    m_rightFrontMotor.setInverted(DriveConstants.isRightMotorInverted);
+    m_leftBackMotor.setInverted(DriveConstants.isLeftMotorInverted);
+    m_rightBackMotor.setInverted(DriveConstants.isRightMotorInverted);
+    //m_driveTrain.setRightSideInverted(DriveConstants.isRightArcadeInverted);
     
     /* Set up music for drive train */
     m_orchestra.addInstrument(m_leftBackMotor);
@@ -273,14 +289,33 @@ public class Drive extends SubsystemBase {
 
   @Override
   public void periodic() {
+    updateTime();
+    updateAngles();
+    updatePosition();
+    updateSmartDashboard();
+  }
+
+  /**
+   * Passes subsystem needed.
+   * @param subsystem Subsystem needed.
+   */
+  public void passRequiredSubsystem(Pneumatics subsystem) {
+    m_pneumaticsSubsystem = subsystem;
+  }
+
+  public void updateTime() {
     m_lastTimeMs = m_currentTimeMs;
     m_currentTimeMs = System.currentTimeMillis();
     m_currentTimeSec = m_currentTimeMs / 1000;
     m_deltaTimeMs = m_currentTimeMs - m_lastTimeMs;
+  }
 
+  public void updateAngles() {
     m_lastAngleYaw = m_currentAngleYaw;
     m_currentAngleYaw = getGyroYaw();
+  }
 
+  public void updatePosition() {
     m_rightFrontMotorPos = m_rightFrontMotor.getSelectedSensorPosition();
     m_rightFrontMotorVel = m_rightFrontMotor.getSelectedSensorVelocity();
 
@@ -295,9 +330,6 @@ public class Drive extends SubsystemBase {
     m_odometry.update(Rotation2d.fromDegrees( getHeading()),
                                               getDistanceInches(m_leftFrontMotor),
                                               -getDistanceInches(m_rightFrontMotor));
-
-    runFalconCooling();
-    updateSmartDashboard();
   }
 
   /**
@@ -431,47 +463,6 @@ public class Drive extends SubsystemBase {
     m_driveTrain.feedWatchdog();
   }
 
-  /**
-   * Set to high or low gear based on boolean state, true = high, false = low
-   * @param state Chooses between high or low gear
-   */
-  public void setShiftState(boolean state) {
-    if (state == true) {
-			m_speedShift.set(DoubleSolenoid.Value.kReverse);
-    }
-		if (state == false) {
-			m_speedShift.set(DoubleSolenoid.Value.kForward);
-    }
-    setRightMotorGains(state);
-    m_isSpeedShiftHigh = state;
-  }
-
-  /**
-   * Set to open or close solenoid that cools the falcon, true = open, false = close
-   * @param state Chooses between open and close
-   */
-  public void coolFalcon(boolean state) {
-    if (state == true) {
-      m_coolFalcon.set(DoubleSolenoid.Value.kForward);
-    }
-    if (state == false) {
-      m_coolFalcon.set(DoubleSolenoid.Value.kReverse);
-    }
-  }
-
-  /**
-   * 
-   */
-  public void runFalconCooling() {
-    if (m_currentTimeSec % 30 == 0) {
-      coolFalcon(true);
-      SmartDashboard.putBoolean("Solenoid", true);
-    } else if ((m_currentTimeSec - 1) % 30 == 0) {
-      coolFalcon(false);
-      SmartDashboard.putBoolean("Solenoid", false);
-    }
-  }
-
   /**
    * Selects a song to play!
    * @param song The name of the song to be played
@@ -624,7 +615,7 @@ public class Drive extends SubsystemBase {
    * @return The converted value in inches
    */
   public double  ticksToInches(double ticks) {
-    if (m_isSpeedShiftHigh) {
+    if (m_pneumaticsSubsystem.m_isSpeedShiftHigh) {
       return ticks * DriveConstants.INCHES_PER_TICK_HIGH;
     } else {
       return ticks * DriveConstants.INCHES_PER_TICK_LOW;
@@ -637,7 +628,7 @@ public class Drive extends SubsystemBase {
    * @return The converted value in ticks
    */
   public double inchesToTicks(double inches) {
-    if (m_isSpeedShiftHigh) {
+    if (m_pneumaticsSubsystem.m_isSpeedShiftHigh) {
       return inches * DriveConstants.TICKS_PER_INCH_HIGH;
     } else {
       return inches * DriveConstants.TICKS_PER_INCH_LOW;
@@ -747,8 +738,14 @@ public class Drive extends SubsystemBase {
       //SmartDashboard.putNumber("Pigeon Roll", getGyroRoll());
       SmartDashboard.putNumber("Left Front Output", m_leftFrontMotor.get());
       SmartDashboard.putNumber("Right Front Output", m_rightFrontMotor.get());
-      SmartDashboard.putNumber("Left Back Output", m_leftBackMotor.get());
-      SmartDashboard.putNumber("Right Back Output", m_rightBackMotor.get());
+      //SmartDashboard.putNumber("Left Back Output", m_leftBackMotor.get());
+      //SmartDashboard.putNumber("Right Back Output", m_rightBackMotor.get());
+
+      double leftRPM = ticksToInches(m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
+      double rightRPM = ticksToInches(m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
+
+      SmartDashboard.putNumber("Left Motor RPM", leftRPM);
+      SmartDashboard.putNumber("Right Motor RPM", rightRPM);
 
       //SmartDashboard.putNumber("Left Back Motor Velocity Raw", m_leftBackMotor.getSelectedSensorVelocity());
       //SmartDashboard.putNumber("Right Back Motor Velocity Raw", m_rightBackMotor.getSelectedSensorVelocity());
@@ -780,9 +777,9 @@ public class Drive extends SubsystemBase {
       SmartDashboard.putNumber("Left Back Motor Current Stator", m_leftBackMotor.getStatorCurrent());
 
       //SmartDashboard.putNumber("PID 0 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_PRIMARY));
-      //SmartDashboard.putNumber("PID 1 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
+      SmartDashboard.putNumber("PID 1 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
       //SmartDashboard.putNumber("PID 0 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_PRIMARY));
-      //SmartDashboard.putNumber("PID 1 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN));
+      SmartDashboard.putNumber("PID 1 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN));
       //SmartDashboard.putNumber("PID 0 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY));
       //SmartDashboard.putNumber("PID 1 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN));
 
@@ -790,7 +787,7 @@ public class Drive extends SubsystemBase {
       //SmartDashboard.putNumber("Odometry Heading", getHeading());
 
       //SmartDashboard.putNumber("Time Seconds", m_currentTimeSec);
-      //SmartDashboard.putNumber("Delta Time", m_deltaTime);
+      SmartDashboard.putNumber("Delta Time", m_deltaTimeMs);
 
       if (m_currentSong != m_songChooser.getSelected()){
         m_currentSong = m_songChooser.getSelected();
diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java
index 6b716f4..f074b33 100644
--- a/src/main/java/frc4388/robot/subsystems/Intake.java
+++ b/src/main/java/frc4388/robot/subsystems/Intake.java
@@ -41,9 +41,9 @@ public class Intake extends SubsystemBase {
     m_extenderMotor.setInverted(false);
     
     m_extenderForwardLimit = m_extenderMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
-    m_extenderReverseLimit = m_extenderMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
-    m_extenderForwardLimit.enableLimitSwitch(false);
-    m_extenderReverseLimit.enableLimitSwitch(false);
+    m_extenderReverseLimit = m_extenderMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
+    m_extenderForwardLimit.enableLimitSwitch(true);
+    m_extenderReverseLimit.enableLimitSwitch(true);
   }
 
   @Override
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/Pneumatics.java b/src/main/java/frc4388/robot/subsystems/Pneumatics.java
new file mode 100644
index 0000000..1c373ed
--- /dev/null
+++ b/src/main/java/frc4388/robot/subsystems/Pneumatics.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 edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.RobotContainer;
+import frc4388.robot.Constants.PneumaticsConstants;
+
+public class Pneumatics extends SubsystemBase {
+  /* Create Solenoids */
+  public DoubleSolenoid m_speedShift = new DoubleSolenoid(  PneumaticsConstants.PCM_MODULE_ID, 
+                                                            PneumaticsConstants.SPEED_SHIFT_FORWARD_ID,
+                                                            PneumaticsConstants.SPEED_SHIFT_REVERSE_ID );
+
+  public DoubleSolenoid m_coolFalcon = new DoubleSolenoid(  PneumaticsConstants.PCM_MODULE_ID, 
+                                                            PneumaticsConstants.COOL_FALCON_FORWARD_ID, 
+                                                            PneumaticsConstants.COOL_FALCON_REVERSE_ID );
+
+  /* Get Drive Subsystem */
+  Drive m_driveSubsystem;
+
+  public boolean m_isSpeedShiftHigh;
+
+  /**
+   * Creates a new Pneumatics.
+   */
+  public Pneumatics() {
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+    runFalconCooling();
+  }
+  
+  /**
+   * Passes subsystem needed.
+   * @param subsystem Subsystem needed.
+   */
+  public void passRequiredSubsystem(Drive subsystem) {
+    m_driveSubsystem = subsystem;
+  }
+
+  /**
+   * Set to high or low gear based on boolean state, true = high, false = low
+   * @param state Chooses between high or low gear
+   */
+  public void setShiftState(boolean state) {
+    if (state == true) {
+			m_speedShift.set(DoubleSolenoid.Value.kReverse);
+    }
+		if (state == false) {
+			m_speedShift.set(DoubleSolenoid.Value.kForward);
+    }
+    m_driveSubsystem.setRightMotorGains(state);
+    m_isSpeedShiftHigh = state;
+  }
+
+  /**
+   * Set to open or close solenoid that cools the falcon, true = open, false = close
+   * @param state Chooses between open and close
+   */
+  public void coolFalcon(boolean state) {
+    if (state == true) {
+      m_coolFalcon.set(DoubleSolenoid.Value.kForward);
+    }
+    if (state == false) {
+      m_coolFalcon.set(DoubleSolenoid.Value.kReverse);
+    }
+  }
+
+  /**
+   * Runs coolFalcon every 30 seconds for 1 second.
+   */
+  public void runFalconCooling() {
+    if (m_driveSubsystem.m_currentTimeSec % 30 == 0) {
+      coolFalcon(true);
+    } else if ((m_driveSubsystem.m_currentTimeSec - 1) % 30 == 0) {
+      coolFalcon(false);
+    }
+  }
+}
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 3d460fe..7bcc411 100644
--- a/src/main/java/frc4388/robot/subsystems/Storage.java
+++ b/src/main/java/frc4388/robot/subsystems/Storage.java
@@ -20,12 +20,13 @@ 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();
@@ -69,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
@@ -79,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 ++;