diff --git a/src/main/deploy/Robot Data - Angles.csv b/src/main/deploy/Robot Data - Angles.csv
new file mode 100644
index 0000000..273d4ba
--- /dev/null
+++ b/src/main/deploy/Robot Data - Angles.csv	
@@ -0,0 +1,6 @@
+Angle (deg),Displacement (deg)
+-20,-5
+-10,-2
+0,0
+10,2
+20,5
\ No newline at end of file
diff --git a/src/main/deploy/Robot Data - Distances.csv b/src/main/deploy/Robot Data - Distances.csv
new file mode 100644
index 0000000..e058b92
--- /dev/null
+++ b/src/main/deploy/Robot Data - Distances.csv	
@@ -0,0 +1,6 @@
+Distance (in),Hood Ext. (u),Drum Velocity (u/ds)
+21,10,10000
+100,23,11000
+200,30,14000
+300,56,17000
+480,100,20000
\ No newline at end of file
diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 6d194b5..2a22799 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -7,6 +7,8 @@
 
 package frc4388.robot;
 
+import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
+
 import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
 import frc4388.utility.LEDPatterns;
 
@@ -27,14 +29,34 @@ public final class Constants {
         public static final int DRIVE_RIGHT_BACK_CAN_ID = 5;
         public static final int PIGEON_ID = 6;
 
+        /* Drive Inversions */
+        public static final boolean isRightMotorInverted = false;
+        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 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;
+
         /* PID Constants Drive*/
         public static final int DRIVE_TIMEOUT_MS = 30;
-        public static final Gains DRIVE_DISTANCE_GAINS = new Gains(0.1, 0.0, 1.0, 0.0, 0, 0.3);
-        public static final Gains DRIVE_VELOCITY_GAINS = new Gains(0.1, 0.0, 0.2, 0.025, 0, 0.05);
-        public static final Gains DRIVE_TURNING_GAINS = new Gains(0.5, 0.0, 0.05, 0.0, 0, 0.5);
-        //public static final Gains DRIVE_MOTION_MAGIC_GAINS = 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_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_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 int DRIVE_CRUISE_VELOCITY_HIGH = 20000;
+        public static final int DRIVE_ACCELERATION_HIGH = 7000;
 
         /* Trajectory Constants */
         public static final double MAX_SPEED_METERS_PER_SECOND = 3;
@@ -58,20 +80,27 @@ public final class Constants {
         
         /* Drive Train Characteristics */
         public static final double TICKS_PER_MOTOR_REV = 2048;
-        public static final double MOTOR_ROT_PER_WHEEL_ROT = 5.13;
+        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;
         public static final double SECONDS_TO_TICK_TIME = 1/TICK_TIME_TO_SECONDS;
-        public static final double WHEEL_ROT_PER_MOTOR_ROT = 1/MOTOR_ROT_PER_WHEEL_ROT;
-        public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_ROT_PER_WHEEL_ROT;
-        public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI;
-        public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV/INCHES_PER_WHEEL_REV;
-        public static final double INCHES_PER_TICK = 1/TICKS_PER_INCH;
         public static final double INCHES_PER_METER = 39.370;
         public static final double METERS_PER_INCH = 1/INCHES_PER_METER;
+        
+        public static final double WHEEL_ROT_PER_MOTOR_ROT_HIGH = 1/MOTOR_ROT_PER_WHEEL_ROT_HIGH;
+        public static final double TICKS_PER_WHEEL_REV_HIGH = TICKS_PER_MOTOR_REV * MOTOR_ROT_PER_WHEEL_ROT_HIGH;
+        public static final double TICKS_PER_INCH_HIGH = TICKS_PER_WHEEL_REV_HIGH/INCHES_PER_WHEEL_REV;
+        public static final double INCHES_PER_TICK_HIGH = 1/TICKS_PER_INCH_HIGH;
+
+        public static final double WHEEL_ROT_PER_MOTOR_ROT_LOW = 1/MOTOR_ROT_PER_WHEEL_ROT_LOW;
+        public static final double TICKS_PER_WHEEL_REV_LOW = TICKS_PER_MOTOR_REV * MOTOR_ROT_PER_WHEEL_ROT_LOW;
+        public static final double TICKS_PER_INCH_LOW = TICKS_PER_WHEEL_REV_LOW/INCHES_PER_WHEEL_REV;
+        public static final double INCHES_PER_TICK_LOW = 1/TICKS_PER_INCH_LOW;
     }
     
     public static final class IntakeConstants {
diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java
index 5ada949..1029754 100644
--- a/src/main/java/frc4388/robot/Robot.java
+++ b/src/main/java/frc4388/robot/Robot.java
@@ -7,7 +7,6 @@
 
 package frc4388.robot;
 
-import com.ctre.phoenix.motorcontrol.FeedbackDevice;
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 
 import edu.wpi.first.wpilibj.TimedRobot;
@@ -64,7 +63,6 @@ public class Robot extends TimedRobot {
   @Override
   public void disabledInit() {
     m_robotContainer.setDriveNeutralMode(NeutralMode.Coast);
-    //m_robotContainer.setDriveGearState(true);
   }
 
   @Override
@@ -79,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(true);
     m_robotContainer.resetOdometry();
     //m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor);
 
@@ -106,7 +104,7 @@ public class Robot extends TimedRobot {
 
   @Override
   public void teleopInit() {
-    m_robotContainer.setDriveNeutralMode(NeutralMode.Coast);
+    m_robotContainer.setDriveNeutralMode(NeutralMode.Brake);
     m_robotContainer.setDriveGearState(true);
     //m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor);
     
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 006f0e0..7759b3f 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -9,7 +9,6 @@ package frc4388.robot;
 
 import java.util.List;
 
-import com.ctre.phoenix.motorcontrol.FeedbackDevice;
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 
 import edu.wpi.first.wpilibj.Joystick;
@@ -17,7 +16,6 @@ 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.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.trajectory.Trajectory;
 import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
 import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
@@ -28,17 +26,11 @@ import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 
 import frc4388.robot.Constants.*;
-import frc4388.robot.commands.DriveStraightAtVelocityPID;
+import frc4388.robot.commands.DrivePositionMPAux;
 import frc4388.robot.commands.DriveWithJoystick;
-import frc4388.robot.commands.DriveStraightToPositionMM;
-import frc4388.robot.commands.DriveStraightToPositionPID;
-import frc4388.robot.commands.DriveWithJoystick;
-import frc4388.robot.commands.DriveWithJoystickUsingDeadAssistPID;
-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.StorageIntakeGroup;
 import frc4388.robot.subsystems.Drive;
 import frc4388.robot.subsystems.Intake;
@@ -46,15 +38,11 @@ import frc4388.robot.subsystems.LED;
 import frc4388.robot.subsystems.Shooter;
 import frc4388.robot.subsystems.Climber;
 import frc4388.robot.commands.RunLevelerWithJoystick;
-import frc4388.robot.subsystems.Drive;
-import frc4388.robot.subsystems.Intake;
-import frc4388.robot.subsystems.LED;
 import frc4388.robot.commands.TrackTarget;
 import frc4388.robot.commands.storageOutake;
 import frc4388.robot.subsystems.Camera;
 import frc4388.robot.subsystems.Leveler;
 import frc4388.robot.subsystems.Storage;
-import frc4388.utility.LEDPatterns;
 import frc4388.utility.controller.IHandController;
 import frc4388.utility.controller.XboxController;
 
@@ -94,15 +82,14 @@ public class RobotContainer {
         m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, getDriverController()));
         // drives intake with input from triggers on the opperator controller
         m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController()));
-        // drives climber with input from triggers on the opperator controller
-        m_robotClimber.setDefaultCommand(new RunClimberWithTriggers(m_robotClimber, getDriverController()));
-        // continually sends updates to the Blinkin LED controller to keep the lights on
-        m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
         // runs the drum shooter in idle mode
         m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runShooterWithInput(m_operatorXbox.getLeftXAxis()), 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));
     }
       
 
@@ -113,20 +100,34 @@ public class RobotContainer {
     * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
     */
     private void configureButtonBindings() {
-        /* Driver Buttons */
+        /* Test Buttons */
+        // A driver test button
+        new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
+            .whenPressed(new InstantCommand());
         
+        // B driver test button
+        new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
+            .whenPressed(new InstantCommand());
+
+        // Y driver test button
+        new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
+            .whenPressed(new InstantCommand());
+
+        // 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(true), m_robotDrive));
+            .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive));
 
         // sets solenoids into low gear
         new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON)
-            .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive));
-
+            .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive));
 
         /* Operator Buttons */
-      
-            //TODO: Shooter Buttons
+        //TODO: Shooter Buttons
         // shoots until released
         //new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
         //    .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 5));
@@ -161,7 +162,62 @@ public class RobotContainer {
         new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_TRIGGER_AXIS)
             .whileHeld(new storageOutake(m_robotStorage));
     }
-       
+
+    /**
+     * Use this to pass the autonomous command to the main {@link Robot} class.
+     *
+     * @return the command to run in autonomous
+     */
+    public Command getAutonomousCommand() {
+        // Create config for trajectory
+        TrajectoryConfig config = getTrajectoryConfig();
+        Trajectory trajectory = getTrajectory(config);
+        RamseteCommand ramseteCommand = getRamseteCommand(trajectory);
+        // Run path following command, then stop at the end.
+        //return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
+
+        // 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,
+            DriveConstants.MAX_ACCELERATION_METERS_PER_SECOND_SQUARED)
+            // Add kinematics to ensure max speed is actually obeyed
+            .setKinematics(DriveConstants.kDriveKinematics);
+    }
+
+    Trajectory getTrajectory(TrajectoryConfig config) {
+        Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
+            // Start at the origin facing the +X direction
+            new Pose2d(0, 0, new Rotation2d(0)),
+            // Pass through these two interior waypoints, making an 's' curve path
+            List.of(
+                new Translation2d(10, 0)
+            ),
+            // End 3 meters straight ahead of where we started, facing forward
+            new Pose2d(20, 20, new Rotation2d(0)),
+            // Pass config
+            config);
+            // 10 = 20, 20 = 35, 30 = 53.5
+            // (0,10) = (8,22)
+        
+        return exampleTrajectory;
+    }
+
+    RamseteCommand getRamseteCommand(Trajectory trajectory) {
+        RamseteCommand ramseteCommand = new RamseteCommand(
+            trajectory,
+            m_robotDrive::getPose,
+            new RamseteController(), 
+            DriveConstants.kDriveKinematics,
+            m_robotDrive::tankDriveVelocity,
+            m_robotDrive);
+        
+        return ramseteCommand;
+    }
+
     /**
      * Sets Motors to a NeutralMode.
      * @param mode NeutralMode to set motors to
@@ -178,54 +234,14 @@ public class RobotContainer {
         m_robotDrive.setShiftState(state);
     }
 
-    public void configDriveTrainSensors(FeedbackDevice type) {
-        m_robotDrive.configMotorSensor(type);
-    } 
-
+    /**
+     * 
+     */
     public void resetOdometry() {
         m_robotDrive.resetGyroAngles();
         m_robotDrive.setOdometry(new Pose2d());
     }
 
-    /**
-     * Use this to pass the autonomous command to the main {@link Robot} class.
-     *
-     * @return the command to run in autonomous
-     */
-    public Command getAutonomousCommand() {
-
-        // Create config for trajectory
-        /*TrajectoryConfig config = new TrajectoryConfig( DriveConstants.MAX_SPEED_METERS_PER_SECOND,
-                                                        DriveConstants.MAX_ACCELERATION_METERS_PER_SECOND_SQUARED)
-                                                        // Add kinematics to ensure max speed is actually obeyed
-                                                        .setKinematics(DriveConstants.kDriveKinematics);
-        
-        Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
-            // Start at the origin facing the +X direction
-            new Pose2d(0, 0, new Rotation2d(0)),
-            // Pass through these two interior waypoints, making an 's' curve path
-            List.of(
-                new Translation2d(10, 0)
-            ),
-            // End 3 meters straight ahead of where we started, facing forward
-            new Pose2d(20, 20, new Rotation2d(0)),
-            // Pass config
-            config);
-            // 10 = 20, 20 = 35, 30 = 53.5
-            // (0,10) = (8,22)
-        RamseteCommand ramseteCommand = new RamseteCommand(
-            exampleTrajectory,
-            m_robotDrive::getPose,
-            new RamseteController(), 
-            DriveConstants.kDriveKinematics,
-            m_robotDrive::tankDriveVelocity,
-            m_robotDrive);
-
-        // Run path following command, then stop at the end.
-        return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));*/
-        return new InstantCommand();
-    }
-
     /**
      * Used for analog inputs like triggers and axises.
      * @return IHandController interface for the Driver Controller.
diff --git a/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java b/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java
new file mode 100644
index 0000000..68d9538
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java
@@ -0,0 +1,90 @@
+/*----------------------------------------------------------------------------*/
+/* 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.DriveConstants;
+import frc4388.robot.subsystems.Drive;
+
+public class DrivePositionMPAux extends CommandBase {
+  Drive m_drive;
+  double m_cruiseVel;
+  double m_rampDist;
+  double m_targetPos;
+  double m_currentVel;
+  double m_currentPos;
+  double m_targetGyro;
+  double m_targetVel;
+  double m_rampAcc;
+  long m_startTime;
+  long m_rampRate;
+
+  /**
+   * Creates a new DrivePositionMPAux.
+   * 
+   * @param subsystem The drive subsystem
+   * @param cruiseVel The target velocity for the motors in units
+   * @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) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    m_drive = subsystem;
+    m_cruiseVel = cruiseVel * DriveConstants.TICKS_PER_INCH_LOW / 10;
+    m_rampDist = rampDist * DriveConstants.TICKS_PER_INCH_LOW;
+    m_rampRate = (long) rampRate * 1000;
+    m_targetPos = targetPos * DriveConstants.TICKS_PER_INCH_LOW;
+    //m_targetGyro = targetGyro * DriveConstants.TICKS_PER_GYRO_REV / 360;
+    m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
+    addRequirements(m_drive);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    m_currentVel = m_drive.m_rightFrontMotorVel;
+    m_currentPos = m_drive.m_rightFrontMotorPos;
+    m_targetPos = m_targetPos + m_currentPos;
+    m_targetVel = m_currentVel;
+    m_startTime = System.currentTimeMillis();
+    m_rampAcc = (m_cruiseVel - m_currentVel) / m_rampRate;
+  }
+
+  // Called every m_isRamptime the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_currentVel = m_drive.m_rightFrontMotorVel;
+    m_currentPos = m_drive.m_rightFrontMotorPos;
+    if (System.currentTimeMillis() - m_startTime < m_rampRate) {
+      // Ramping
+      m_targetVel += m_rampAcc * m_drive.m_deltaTimeMs;
+      m_drive.runDriveVelocityPID(-m_targetVel, m_targetGyro);
+    } else if (m_targetPos - m_currentPos > m_rampDist) {
+      // Cruising
+      m_drive.runDriveVelocityPID(-m_cruiseVel, m_targetGyro);
+    } else {
+      // Deramp PID
+      m_drive.runDrivePositionPID(-m_targetPos, m_targetGyro);
+    }
+  }
+
+  // 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_currentPos - m_targetPos <= 0.5f * DriveConstants.TICKS_PER_INCH_LOW) {
+      return true;
+    }
+    return false;
+  }
+}
diff --git a/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java b/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java
index 259c571..c56a36b 100644
--- a/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java
+++ b/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java
@@ -18,6 +18,7 @@ public class DriveStraightToPositionMM extends CommandBase {
   double m_targetPosOut;
   double m_targetGyro;
   boolean isGoneFast;
+  int i;
   
   /**
    * Creates a new DriveToDistancePID.
@@ -27,7 +28,7 @@ public class DriveStraightToPositionMM extends CommandBase {
   public DriveStraightToPositionMM(Drive subsystem, double targetPos) {
     // Use addRequirements() here to declare subsystem dependencies.
     m_drive = subsystem;
-    m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH;
+    m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW;
     addRequirements(m_drive);
     //SmartDashboard.putNumber("Distance Target Inches", targetPos);
   }
@@ -39,6 +40,7 @@ public class DriveStraightToPositionMM extends CommandBase {
     m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
     m_targetPosOut = m_targetPosIn + m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY);
     isGoneFast = false;
+    i = 0;
   }
 
   // Called every time the scheduler runs while the command is scheduled.
@@ -48,6 +50,8 @@ public class DriveStraightToPositionMM extends CommandBase {
     //System.err.println("Sensor Error \n" + m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
     //System.err.println("Sensor Target \n" + m_drive.m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN));
     m_drive.runMotionMagicPID(m_targetPosOut, m_targetGyro);
+    SmartDashboard.putBoolean("MM Run", true);
+    i++;
   }
 
   // Called once the command ends or is interrupted.
@@ -59,9 +63,10 @@ public class DriveStraightToPositionMM extends CommandBase {
   @Override
   public boolean isFinished() {
     if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && isGoneFast){
+      SmartDashboard.putBoolean("MM Run", false);
       return true;
     } else {
-      if (m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY) > 100) {
+      if ((m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY) > 100)) {
         isGoneFast = true;
       }
       return false;
diff --git a/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java b/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java
index 3b74edf..949a4bf 100644
--- a/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java
+++ b/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java
@@ -27,7 +27,7 @@ public class DriveStraightToPositionPID extends CommandBase {
   public DriveStraightToPositionPID(Drive subsystem, double targetPos) {
     // Use addRequirements() here to declare subsystem dependencies.
     m_drive = subsystem;
-    m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH;
+    m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW;
     addRequirements(m_drive);
     //SmartDashboard.putNumber("Distance Target Inches", targetPos);
   }
diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
index 5387e8d..f51621a 100644
--- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
+++ b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
@@ -48,12 +48,14 @@ public class DriveWithJoystick extends CommandBase {
       moveOutput = Math.cos(1.571*moveInput)-1;
     }
 
-    double cosMultiplier = .45;
-    double deadzone = .2;
+    double cosMultiplier = .55;
+    double deadzone = .1;
     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 - deadzone) * Math.cos(1.571*steerInput) - cosMultiplier;
     } else {
-      steerOutput = cosMultiplier*Math.cos(1.571*steerInput)-(cosMultiplier+deadzone);
+      steerOutput = 0;
     }
 
     m_drive.driveWithInput(moveOutput, steerOutput);
diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java
index 7756772..12d605c 100644
--- a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java
+++ b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java
@@ -7,6 +7,7 @@
 
 package frc4388.robot.commands;
 
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import edu.wpi.first.wpiutil.math.MathUtil;
 import frc4388.robot.Constants.DriveConstants;
@@ -21,6 +22,23 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
   long m_deadTimeSteer, m_deadTimeMove;
   long m_deadTimeout = 100;
   IHandController m_controller;
+  boolean m_isInterrupted;
+
+  /* Deadassist Constants */
+  final float stopPosVelCoefLow = 1;
+  final float stopPosVelCoefHigh = 3;
+  final float cosMultiplierLow = 0.55f;
+  final float cosMultiplierHigh = 0.35f;
+  final float targetAngleCoefLow = 5;
+  final float targetAngleCoefHigh = 5;
+  final float gyroVelCoefLow = 1;
+  final float gyroVelCoefHigh = 3;
+
+  /* Deadassist Coeficients */
+  final float stopPosVelCoef = 1;
+  final float cosMultiplier = 0.55f;
+  final float targetAngleCoef = 5;
+  final float gyroVelCoef = 1;
 
   /**
    * Creates a new DriveWithJoystickUsingDeadAssistPID to control the drivetrain with an Xbox controller.
@@ -42,6 +60,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
   @Override
   public void initialize() {
     m_currTime = System.currentTimeMillis();
+    resetGyroTarget();
   }
 
   // Called every time the scheduler runs while the command is scheduled.
@@ -54,6 +73,11 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
     m_deltaTime = System.currentTimeMillis() - m_currTime;
     m_currTime = System.currentTimeMillis();
 
+    if (m_isInterrupted) {
+      resetGyroTarget();
+      m_isInterrupted = false;
+    }
+
     /* If move stick is being used */
     if (moveInput != 0) {
       m_deadTimeMove = m_currTime;
@@ -65,15 +89,19 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
       m_deadTimeSteer = m_currTime;
     }
 
-    /* If move stick has been pressed within 1 sec */
-    if (m_currTime - m_deadTimeMove < m_deadTimeout) {
-      /* Curves the moveInput to be slightly more gradual at first */
-      if (moveInput >= 0) {
-        moveOutput = -Math.cos(1.571*moveInput)+1;
-      } else {
-        moveOutput = Math.cos(1.571*moveInput)-1;
-      }
+    /* Curves the moveInput to be slightly more gradual at first */
+    if (moveInput >= 0) {
+      moveOutput = -Math.cos(1.571*moveInput)+1;
+    } else {
+      moveOutput = Math.cos(1.571*moveInput)-1;
+    }
 
+    if (m_drive.m_isSpeedShiftHigh) {
+      runDriveWithInput(moveOutput, steerInput);
+      resetGyroTarget();
+    }
+    /* If move stick has been pressed within 1 sec */
+    else if (m_currTime - m_deadTimeMove < m_deadTimeout) {
       /* If steer stick has not been used for less than 1 sec */
       if (m_currTime - m_deadTimeSteer < m_deadTimeout) {
         runDriveWithInput(moveOutput, steerInput);
@@ -90,16 +118,17 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
     }
   }
 
-  private void runDriveWithInput(double move, double steer) {
-    double cosMultiplier = .45;
+  private void runDriveWithInput(double move, double steerInput) {
+    double cosMultiplier = .55;
     double steerOutput = 0;
     double deadzone = .2;
     /* 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 (steerInput > 0){
+      steerOutput = -cosMultiplier*Math.cos(1.571*steerInput)+(cosMultiplier+deadzone);
+    } else if (steerInput < 0) {
+      steerOutput = cosMultiplier*Math.cos(1.571*steerInput)-(cosMultiplier+deadzone);
     }
+
     m_drive.driveWithInput(move, steerOutput);
     System.out.println("Driving With Input");
   }
@@ -110,8 +139,23 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
   }
 
   private void runStoppedTurn(double steer) {
-    updateGyroTarget(steer);
-    m_drive.runDrivePositionPID(m_stopPos, m_targetGyro);
+    double cosMultiplier = 0.55;
+    double steerOutput = 0;
+    double deadzone = .2;
+    /* Curves the steer output to be similarily gradual */
+    if (steer > 0) {
+      steerOutput = -cosMultiplier*Math.cos(1.571*steer)+(cosMultiplier+deadzone);
+    } else if (steer < 0) {
+      steerOutput = cosMultiplier*Math.cos(1.571*steer)-(cosMultiplier+deadzone);
+    }
+
+    updateGyroTarget(steerOutput);
+    double currentPos = m_drive.m_rightFrontMotorPos;
+    if (Math.abs(currentPos - m_stopPos) > 200) {
+      m_drive.runDrivePositionPID(m_stopPos, m_targetGyro);
+    } else {
+      m_drive.driveWithInputAux(0, m_targetGyro);
+    }
     System.out.println("Turning with Target: " + m_targetGyro);
   }
 
@@ -121,15 +165,15 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
   private void updateGyroTarget(double steerInput) {
     m_targetGyro -= 5 * steerInput * m_deltaTime;
     m_targetGyro = MathUtil.clamp(  m_targetGyro,
-                                    m_currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/8),
-                                    m_currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/8));
+                                    m_currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/3),
+                                    m_currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/3));
   }
 
   /**
    * set target angle to current angle (prevents buildup of gyro error).
    */
   private void resetGyroTarget() {
-    m_targetGyro = m_currentGyro;
+    //m_targetGyro = m_currentGyro;
     m_targetGyro =  m_currentGyro
                     + m_drive.getTurnRate();
   }
@@ -137,6 +181,7 @@ public class DriveWithJoystickUsingDeadAssistPID 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/TurnDegrees.java b/src/main/java/frc4388/robot/commands/TurnDegrees.java
new file mode 100644
index 0000000..2b47050
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/TurnDegrees.java
@@ -0,0 +1,73 @@
+/*----------------------------------------------------------------------------*/
+/* 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.DriveConstants;
+import frc4388.robot.subsystems.Drive;
+
+public class TurnDegrees extends CommandBase {
+
+  double m_targetAngle;
+  Drive m_drive;
+  double m_currentYawInTicks;
+  double m_targetAngleTicksIn;
+  double m_targetAngleTicksOut;
+  int i;
+
+  /**
+   * Creates a new TurnDeg.
+   */
+  public TurnDegrees(double targetAngle, Drive subsystem) {
+    // Use addRequirements() here to declare subsystem dependencies.
+
+    m_targetAngle = targetAngle;
+    m_drive = subsystem;
+
+    addRequirements(m_drive);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    m_targetAngleTicksIn = (m_targetAngle / 360) * DriveConstants.TICKS_PER_GYRO_REV;
+    m_currentYawInTicks = (m_drive.getGyroYaw() / 360) * DriveConstants.TICKS_PER_GYRO_REV;
+    m_targetAngleTicksOut = m_targetAngleTicksIn + m_currentYawInTicks;
+
+    i = 0;
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_currentYawInTicks = (m_drive.getGyroYaw() / 360) * DriveConstants.TICKS_PER_GYRO_REV;
+
+    m_drive.runTurningPID(m_targetAngleTicksOut);
+
+    SmartDashboard.putNumber("Turning Error", Math.abs(m_currentYawInTicks - m_targetAngleTicksOut));
+    SmartDashboard.putNumber("Turning Target", m_targetAngleTicksOut);
+
+    i++;
+  }
+
+  // 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 ((Math.abs(m_drive.getTurnRate()) < 1) && (i > 5)) {
+      return true;
+    }
+    return false;
+  }
+}
+
diff --git a/src/main/java/frc4388/robot/commands/Wait.java b/src/main/java/frc4388/robot/commands/Wait.java
new file mode 100644
index 0000000..934b508
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/Wait.java
@@ -0,0 +1,61 @@
+/*----------------------------------------------------------------------------*/
+/* 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 edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Wait extends CommandBase {
+
+  long m_startTime;
+  long m_waitTime;
+  long m_currentTime;
+  SubsystemBase m_subsystem;
+
+  /**
+   * Creates a new WaitCommand.
+   */
+  public Wait(float seconds, SubsystemBase subsystem) {
+    // Use addRequirements() here to declare subsystem dependencies.
+
+    m_waitTime = (long) (seconds * 1000);
+    m_subsystem = subsystem;
+
+    addRequirements(m_subsystem);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    m_currentTime = System.currentTimeMillis();
+    m_startTime = m_currentTime;
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_currentTime = System.currentTimeMillis();
+    SmartDashboard.putNumber("Time Difference for Wait", (m_currentTime - m_startTime));
+  }
+
+  // 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_currentTime - m_startTime) >= m_waitTime) {
+      return true;
+    } else {
+      return false;
+    }
+  }
+}
diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java
index 1902e0d..fca1e7b 100644
--- a/src/main/java/frc4388/robot/subsystems/Drive.java
+++ b/src/main/java/frc4388/robot/subsystems/Drive.java
@@ -8,20 +8,11 @@
 package frc4388.robot.subsystems;
 
 import java.io.File;
-import java.io.FilenameFilter;
-import java.io.IOException;
-import java.nio.file.Files;
-import java.nio.file.Path;
-import java.nio.file.Paths;
-import java.util.ArrayList;
-import java.util.List;
-import java.util.stream.Collectors;
-import java.util.stream.Stream;
+
 import com.ctre.phoenix.motorcontrol.ControlMode;
 import com.ctre.phoenix.motorcontrol.DemandType;
 import com.ctre.phoenix.motorcontrol.FeedbackDevice;
 import com.ctre.phoenix.motorcontrol.FollowerType;
-import com.ctre.phoenix.motorcontrol.InvertType;
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
 import com.ctre.phoenix.motorcontrol.SensorTerm;
@@ -34,59 +25,73 @@ import com.ctre.phoenix.sensors.PigeonIMU_StatusFrame;
 
 import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.DoubleSolenoid;
-
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj.geometry.Pose2d;
 import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry;
 import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
-import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import edu.wpi.first.wpiutil.math.MathUtil;
+
 import frc4388.robot.Constants.DriveConstants;
 import frc4388.robot.Gains;
 
-/**
- * Add your docs here.
- */
 public class Drive extends SubsystemBase {
-  // Put methods for controlling this subsystem
-  // here. Call these from Commands.
-
+  /* Create Motors, Gyros, Solenoids, 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 Orchestra m_orchestra = new Orchestra();
-
-  public double m_rightFrontMotorPos;
-
-  public double m_rightFrontMotorVel;
-
-  public DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor);
-
-  SendableChooser<Gains> m_chooser = new SendableChooser<Gains>();
-  public static Gains m_gainsDistance = DriveConstants.DRIVE_DISTANCE_GAINS;
-  public static Gains m_gainsVelocity = DriveConstants.DRIVE_VELOCITY_GAINS;
-  public static Gains m_gainsTurning = DriveConstants.DRIVE_TURNING_GAINS;
-  //public static Gains m_gainsMotionMagic = DriveConstants.DRIVE_MOTION_MAGIC_GAINS;
+  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 DoubleSolenoid m_speedShift;
-  public DoubleSolenoid m_coolFalcon;
+  public Orchestra m_orchestra;
 
+  /* Low Gear Gains */
+  public static Gains m_gainsDistanceLow = DriveConstants.DRIVE_DISTANCE_GAINS_LOW;
+  public static Gains m_gainsVelocityLow = DriveConstants.DRIVE_VELOCITY_GAINS_LOW;
+  public static Gains m_gainsTurningLow = DriveConstants.DRIVE_TURNING_GAINS_LOW;
+  public static Gains m_gainsMotionMagicLow = DriveConstants.DRIVE_MOTION_MAGIC_GAINS_LOW;
+
+  /* High Gear Gains */
+  public static Gains m_gainsDistanceHigh = DriveConstants.DRIVE_DISTANCE_GAINS_HIGH;
+  public static Gains m_gainsVelocityHigh = DriveConstants.DRIVE_VELOCITY_GAINS_HIGH;
+  public static Gains m_gainsTurningHigh = DriveConstants.DRIVE_TURNING_GAINS_HIGH;
+  public static Gains m_gainsMotionMagicHigh = DriveConstants.DRIVE_MOTION_MAGIC_GAINS_HIGH;
+
+  /* Timey Whimey */
+  public long m_currentTimeMs = System.currentTimeMillis();
+  public long m_lastTimeMs = m_currentTimeMs;
+  public long m_deltaTimeMs = 0;
+  public long m_currentTimeSec = m_currentTimeMs / 1000;
+
+  /* Position Tracking */
+  public double m_rightFrontMotorPos = 0;
+  public double m_rightFrontMotorVel = 0;
+
+  public double m_totalLeftDistanceInches = 0;
+  public double m_totalRightDistanceInches = 0;
+
+  public double m_currentLeftPosTicks = 0;
+  public double m_currentRightPosTicks = 0;
+  public double m_lastLeftPosTicks = 0;
+  public double m_lastRightPosTicks = 0;
+
+  public double m_lastAngleYaw = 0;
+  public double m_currentAngleYaw = 0;
+
+  /* Smart Dashboard Objects */
   SendableChooser<String> m_songChooser = new SendableChooser<String>();
-
-  public int m_currentTimeSec = (int)(System.currentTimeMillis() / 1000);
-  public long m_lastTime, m_deltaTime; //in milliseconds
-
-  public double m_lastAngleYaw, m_currentAngleYaw, m_kinematicsTargetAngle;
+  
+  /* Misc */
+  public boolean m_isSpeedShiftHigh;
+  String m_currentSong = "";
 
   /**
    * Add your docs here.
@@ -100,81 +105,50 @@ public class Drive extends SubsystemBase {
     m_pigeon.configFactoryDefault();
     resetGyroYaw();
 
-    m_odometry = new DifferentialDriveOdometry( Rotation2d.fromDegrees(getHeading()), 
-                                                new Pose2d(0, 0, new Rotation2d()) );
-
-    m_speedShift = new DoubleSolenoid(7,0,1);
-    m_coolFalcon = new DoubleSolenoid(7,3,2);
-
-    coolFalcon(false);
-    
-    /* set back motors as followers */
-    m_leftBackMotor.follow(m_leftFrontMotor);
-    m_rightBackMotor.follow(m_rightFrontMotor);
-
     /* flip input so forward becomes back, etc */
-    m_leftFrontMotor.setInverted(false);
-    m_rightFrontMotor.setInverted(true);
-    //m_driveTrain.setRightSideInverted(false);
-    m_leftBackMotor.setInverted(InvertType.FollowMaster);
-    m_rightBackMotor.setInverted(InvertType.FollowMaster);
+    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);
 
-    setDriveTrainNeutralMode(NeutralMode.Coast);
+    /* Config Open Loop Ramp so we don't make sudden output changes */
+    m_rightFrontMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightBackMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_leftFrontMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_leftBackMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
 
-    /* deadbands */
-    m_leftBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // DO NOT CHANGE
-    m_rightBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // Ensures motors run at the same speed
-    //m_leftFrontMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // DO NOT CHANGE
-    //m_rightFrontMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // Ensures motors run at the same speed
+    /* 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);
+
+    /* Config deadbands so that  */
+    m_leftBackMotor.configNeutralDeadband(DriveConstants.NEUTRAL_DEADBAND, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightBackMotor.configNeutralDeadband(DriveConstants.NEUTRAL_DEADBAND, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_leftFrontMotor.configNeutralDeadband(DriveConstants.NEUTRAL_DEADBAND, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightFrontMotor.configNeutralDeadband(DriveConstants.NEUTRAL_DEADBAND, DriveConstants.DRIVE_TIMEOUT_MS);
 
     /* PID for Front Motor Control in Teleop */
-    //m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
-    //m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
-    //m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
-    //m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
-    //m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
-    //m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
+    setRightMotorGains(false);
 
-    m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
-    m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
-
-    m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY);
-    m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
-
-    //m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY);
-    //m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
-    //m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
-    //m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
-    //m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
-    //m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
-    //m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY, DriveConstants.DRIVE_TIMEOUT_MS);
-    //m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION, DriveConstants.DRIVE_TIMEOUT_MS);
-    //m_rightFrontMotor.configMotionSCurveStrength(0, DriveConstants.DRIVE_TIMEOUT_MS);
-
-    /* PID for Back Motor control in Auto */
+    /* PID for Back Motor Control in Tank Drive Vel */
     m_rightBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
-    m_rightBackMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightBackMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightBackMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightBackMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightBackMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightBackMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightBackMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightBackMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
 
     m_leftBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
-    m_leftBackMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_leftBackMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_leftBackMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_leftBackMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_leftBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_leftBackMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_leftBackMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_leftBackMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_leftBackMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_leftBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
 
-    /* Setup Sensors for WPI_TalonFXs */
+    /* Reset Sensors for WPI_TalonFXs */
     resetEncoders();
 
     /* Configure the left Talon's selected sensor as local QuadEncoder */
@@ -194,50 +168,43 @@ public class Drive extends SubsystemBase {
 
     /* Configure the Remote Talon's selected sensor as a remote sensor for the right Talon */
     m_rightFrontMotor.configRemoteFeedbackFilter( m_leftFrontMotor.getDeviceID(), // Device ID of Source
-                                                  RemoteSensorSource.TalonSRX_SelectedSensor, DriveConstants.REMOTE_0, // Source number [0, 1]
+                                                  RemoteSensorSource.TalonSRX_SelectedSensor,
+                                                  DriveConstants.REMOTE_0, // Source number [0, 1]
                                                   DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout
 
+    /* Diff Signal signal to be used for Distance*/
+    m_rightFrontMotor.configSensorTerm(SensorTerm.Diff1, FeedbackDevice.RemoteSensor0, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightFrontMotor.configSensorTerm(SensorTerm.Diff0, FeedbackDevice.IntegratedSensor, DriveConstants.DRIVE_TIMEOUT_MS);
+
+    /* Configure Diff [Sum of both QuadEncoders] to be used for Primary PID Index */
+    m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.SensorDifference,
+                                                    DriveConstants.PID_PRIMARY,
+                                                    DriveConstants.DRIVE_TIMEOUT_MS);
+
     /* Configure the Pigeon IMU to the other Remote Slot available on the right Talon */
     m_rightFrontMotor.configRemoteFeedbackFilter( m_pigeon.getDeviceID(), RemoteSensorSource.Pigeon_Yaw,
                                                   DriveConstants.REMOTE_1, DriveConstants.DRIVE_TIMEOUT_MS);
 
-    /* Setup Sum signal to be used for Distance */
-    m_rightFrontMotor.configSensorTerm(SensorTerm.Sum0, FeedbackDevice.RemoteSensor0, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.configSensorTerm(SensorTerm.Sum1, FeedbackDevice.IntegratedSensor, DriveConstants.DRIVE_TIMEOUT_MS);
-
-    /* Diff Signal */
-    m_rightFrontMotor.configSensorTerm(SensorTerm.Diff1, FeedbackDevice.RemoteSensor0, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.configSensorTerm(SensorTerm.Diff0, FeedbackDevice.IntegratedSensor, DriveConstants.DRIVE_TIMEOUT_MS);
-
-    /* Configure Sum [Sum of both QuadEncoders] to be used for Primary PID Index */
-    configMotorSensor(FeedbackDevice.SensorDifference);
-
-    /* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) DOESN'T WORK */
-    /*
-    m_rightFrontMotor.configSelectedFeedbackCoefficient(  1, // Coefficient
-                                                          DriveConstants.PID_PRIMARY, // PID Slot of Source
-                                                          DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout
-    */
-
+    /* Config Remote1 to be used for Aux PID Index */
     m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.RemoteSensor1,
                                                     DriveConstants.PID_TURN,
                                                     DriveConstants.DRIVE_TIMEOUT_MS);
 
-    /* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) DOESN'T WORK */
-    //m_rightFrontMotor.configSelectedFeedbackCoefficient(1, DriveConstants.PID_TURN, DriveConstants.DRIVE_TIMEOUT_MS);
-
-    /* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) DOESN'T WORK */
-    //m_leftFrontMotor.configSelectedFeedbackCoefficient(1, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS);
+    /**
+     * configAuxPIDPolarity(boolean invert, int timeoutMs) false means talon's local
+     * output is PID0 + PID1, and other side Talon is PID0 - PID1 true means talon's
+     * local output is PID0 - PID1, and other side Talon is PID0 + PID1
+     */
+    m_rightFrontMotor.configAuxPIDPolarity(DriveConstants.isAuxPIDInverted, DriveConstants.DRIVE_TIMEOUT_MS);
 
     /* Set status frame periods to ensure we don't have stale data */
     m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_12_Feedback1, 20, DriveConstants.DRIVE_TIMEOUT_MS);
     m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_13_Base_PIDF0, 20, DriveConstants.DRIVE_TIMEOUT_MS);
     m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_14_Turn_PIDF1, 20, DriveConstants.DRIVE_TIMEOUT_MS);
     m_leftFrontMotor.setStatusFramePeriod(StatusFrame.Status_2_Feedback0, 5, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR, 5, DriveConstants.DRIVE_TIMEOUT_MS);
-
     m_leftBackMotor.setStatusFramePeriod(StatusFrame.Status_13_Base_PIDF0, 20, DriveConstants.DRIVE_TIMEOUT_MS);
     m_rightBackMotor.setStatusFramePeriod(StatusFrame.Status_13_Base_PIDF0, 20, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR, 5, DriveConstants.DRIVE_TIMEOUT_MS);
 
     /**
      * Max out the peak output (for all modes). However you can limit the output of
@@ -259,37 +226,39 @@ public class Drive extends SubsystemBase {
      * derivative error never gets large enough to be useful. - sensor movement is
      * very slow causing the derivative error to be near zero.
      */
-    int closedLoopTimeMs = 1;
     m_rightFrontMotor.configClosedLoopPeriod( DriveConstants.PID_PRIMARY, 
-                                              closedLoopTimeMs, 
+                                              DriveConstants.CLOSED_LOOP_TIME_MS, 
                                               DriveConstants.DRIVE_TIMEOUT_MS);
 
     m_rightFrontMotor.configClosedLoopPeriod( DriveConstants.PID_TURN, 
-                                              closedLoopTimeMs,
+                                              DriveConstants.CLOSED_LOOP_TIME_MS,
                                               DriveConstants.DRIVE_TIMEOUT_MS);
 
     m_leftBackMotor.configClosedLoopPeriod( DriveConstants.PID_PRIMARY, 
-                                              closedLoopTimeMs,
+                                              DriveConstants.CLOSED_LOOP_TIME_MS,
                                               DriveConstants.DRIVE_TIMEOUT_MS);
 
-    m_leftBackMotor.configClosedLoopPeriod( DriveConstants.PID_PRIMARY, 
-                                              closedLoopTimeMs,
+    m_rightBackMotor.configClosedLoopPeriod( DriveConstants.PID_PRIMARY, 
+                                              DriveConstants.CLOSED_LOOP_TIME_MS,
                                               DriveConstants.DRIVE_TIMEOUT_MS);
-    
-    /**
-     * configAuxPIDPolarity(boolean invert, int timeoutMs) false means talon's local
-     * output is PID0 + PID1, and other side Talon is PID0 - PID1 true means talon's
-     * local output is PID0 - PID1, and other side Talon is PID0 + PID1
-     */
-    m_rightFrontMotor.configAuxPIDPolarity(false, DriveConstants.DRIVE_TIMEOUT_MS);
+                                             
+    /* Set up Differential Drive */
+    m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor);
 
-    m_lastTime = System.currentTimeMillis();
+    /* Set up Differential Drive Odometry. */
+    m_odometry = new DifferentialDriveOdometry( Rotation2d.fromDegrees(getHeading()), 
+                                                new Pose2d(0, 0, new Rotation2d()));
+
+    /* Set up Orchestra */
+    m_orchestra = new Orchestra();
     
+    /* Set up music for drive train */
     m_orchestra.addInstrument(m_leftBackMotor);
     m_orchestra.addInstrument(m_rightFrontMotor);
     m_orchestra.addInstrument(m_rightBackMotor);
     m_orchestra.addInstrument(m_leftFrontMotor);
 
+    /* Create chooser to choose song to play */
     File songsDir = new File(Filesystem.getDeployDirectory().getAbsolutePath() + "/songs");
     System.err.println(songsDir.getPath());
     String[] songsStrings = songsDir.list();
@@ -297,90 +266,38 @@ public class Drive extends SubsystemBase {
       m_songChooser.addOption(songString, songsDir.getAbsolutePath() + "/" + songString);
     }
     Shuffleboard.getTab("Songs").add(m_songChooser);
-  } 
 
-  String currentSong = "";
+    /* Start counting time */
+    m_lastTimeMs = System.currentTimeMillis();
+  }
+
   @Override
   public void periodic() {
-    m_currentTimeSec = (int)(System.currentTimeMillis() / 1000);
-    SmartDashboard.putNumber("Time Seconds", System.currentTimeMillis());
+    m_lastTimeMs = m_currentTimeMs;
+    m_currentTimeMs = System.currentTimeMillis();
+    m_currentTimeSec = m_currentTimeMs / 1000;
+    m_deltaTimeMs = m_currentTimeMs - m_lastTimeMs;
 
-    if (m_currentTimeSec % 30 == 0) {
-      coolFalcon(true);
-      SmartDashboard.putBoolean("Solenoid", true);
-    } else if ((m_currentTimeSec - 1) % 30 == 0) {
-      coolFalcon(false);
-      SmartDashboard.putBoolean("Solenoid", false);
-    }
-
-    m_deltaTime = System.currentTimeMillis() - m_lastTime;
-    m_lastTime = System.currentTimeMillis();
     m_lastAngleYaw = m_currentAngleYaw;
     m_currentAngleYaw = getGyroYaw();
 
     m_rightFrontMotorPos = m_rightFrontMotor.getSelectedSensorPosition();
     m_rightFrontMotorVel = m_rightFrontMotor.getSelectedSensorVelocity();
-    
-    try {
-      SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw());
-      //SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch());
-      //SmartDashboard.putNumber("Pigeon Roll", getGyroRoll());
-      SmartDashboard.putNumber("Left Back Output", m_leftBackMotor.get());
-      SmartDashboard.putNumber("Right Back Output", m_rightBackMotor.get());
 
-      SmartDashboard.putNumber("Left Back Motor Velocity Raw", m_leftBackMotor.getSelectedSensorVelocity());
-      SmartDashboard.putNumber("Right Back Motor Velocity Raw", m_rightBackMotor.getSelectedSensorVelocity());
-      //SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition());
-      //SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0));
-      SmartDashboard.putNumber("Right Motor Velocity Int Sensor", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
-      SmartDashboard.putNumber("Left Motor Velocity Int Sensor", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
+    m_lastRightPosTicks = m_currentRightPosTicks;
+    m_lastLeftPosTicks = m_currentLeftPosTicks;
+    m_currentRightPosTicks = m_rightFrontMotor.getSensorCollection().getIntegratedSensorPosition();
+    m_currentLeftPosTicks = m_leftFrontMotor.getSensorCollection().getIntegratedSensorPosition();
 
-      SmartDashboard.putNumber("Right Motor Temp", m_rightFrontMotor.getTemperature());
-      SmartDashboard.putNumber("Left Motor Temp", m_leftFrontMotor.getTemperature());
-
-      //SmartDashboard.putNumber("Right Front Motor Current Supply", m_rightFrontMotor.getSupplyCurrent());
-      //SmartDashboard.putNumber("Left Front Motor Current Supply", m_leftFrontMotor.getSupplyCurrent());
-      //SmartDashboard.putNumber("Right Front Motor Current Stator ", m_rightFrontMotor.getStatorCurrent());
-      //SmartDashboard.putNumber("Left Front Motor Current Stator", m_leftFrontMotor.getSupplyCurrent());
-
-      //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 0 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_PRIMARY));
-      //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));
-
-      SmartDashboard.putString("Odometry Values Meters", getPose().toString());
-      SmartDashboard.putNumber("Odometry Heading", getHeading());
-
-      SmartDashboard.putNumber("Time Seconds", m_currentTimeSec);
-      //SmartDashboard.putNumber("Delta Time", m_deltaTime);
-
-      if (currentSong != m_songChooser.getSelected()){
-        currentSong = m_songChooser.getSelected();
-        selectSong(currentSong);
-        System.err.println(currentSong);
-      }
-    } catch (Exception e) {
-      System.err.println("Error in the Drive Subsystem");
-      // e.printStackTrace(System.err);
-    }
+    m_totalRightDistanceInches += ticksToInches(m_currentRightPosTicks - m_lastRightPosTicks);
+    m_totalLeftDistanceInches += ticksToInches(m_currentLeftPosTicks - m_lastLeftPosTicks);
 
     m_odometry.update(Rotation2d.fromDegrees( getHeading()),
-                                              inchesToMeters(getDistanceInches(m_leftBackMotor)),
-                                              -inchesToMeters(getDistanceInches(m_rightBackMotor)));
-  }
+                                              getDistanceInches(m_leftFrontMotor),
+                                              -getDistanceInches(m_rightFrontMotor));
 
-  /**
-   * Sets Motors to a NeutralMode.
-   * 
-   * @param mode NeutralMode to set motors to
-   */
-  public void setDriveTrainNeutralMode(NeutralMode mode) {
-    m_leftFrontMotor.setNeutralMode(mode);
-    m_rightFrontMotor.setNeutralMode(mode);
-    m_leftBackMotor.setNeutralMode(mode);
-    m_rightBackMotor.setNeutralMode(mode);
+    runFalconCooling();
+    updateSmartDashboard();
   }
 
   /**
@@ -388,7 +305,7 @@ public class Drive extends SubsystemBase {
    * using the Differential Drive class to manage the two inputs
    */
   public void driveWithInput(double move, double steer) {
-    m_driveTrain.arcadeDrive(move, steer);
+    m_driveTrain.arcadeDrive(steer, move);
     m_leftBackMotor.follow(m_leftFrontMotor);
     m_rightBackMotor.follow(m_rightFrontMotor);
   }
@@ -424,7 +341,7 @@ public class Drive extends SubsystemBase {
     m_leftBackMotor.follow(m_leftFrontMotor);
     m_rightBackMotor.follow(m_rightFrontMotor);
 
-    //m_driveTrain.feedWatchdog();
+    m_driveTrain.feedWatchdog();
   }
 
   /**
@@ -442,7 +359,7 @@ public class Drive extends SubsystemBase {
     m_leftBackMotor.follow(m_leftFrontMotor);
     m_rightBackMotor.follow(m_rightFrontMotor);
 
-    //m_driveTrain.feedWatchdog();
+    m_driveTrain.feedWatchdog();
   }
 
   /**
@@ -456,6 +373,7 @@ public class Drive extends SubsystemBase {
 
     m_rightFrontMotor.set(ControlMode.MotionMagic, targetPos, DemandType.AuxPID, targetGyro);
     m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
+
     m_leftBackMotor.follow(m_leftFrontMotor);
     m_rightBackMotor.follow(m_rightFrontMotor);
 
@@ -469,9 +387,9 @@ public class Drive extends SubsystemBase {
    * @param targetAngle target angle in degrees
    */
   public void runTurningPID(double targetAngle) {
-    double targetGyro = (targetAngle / 360) * DriveConstants.TICKS_PER_GYRO_REV;
+    //double targetGyro = (targetAngle / 360) * DriveConstants.TICKS_PER_GYRO_REV;
 
-    runDriveVelocityPID(0, targetGyro);
+    runDriveVelocityPID(0, targetAngle);
   }
 
   /**
@@ -514,12 +432,100 @@ public class Drive extends SubsystemBase {
   }
 
   /**
-   * Selects the feedback device for the motors.
-   * @param feedbackDevice The feedback device to set it to, usually SensorDifference or
+   * Set to high or low gear based on boolean state, true = high, false = low
+   * @param state Chooses between high or low gear
    */
-  public void configMotorSensor(FeedbackDevice type) {
-    m_rightFrontMotor.configSelectedFeedbackSensor( type, DriveConstants.PID_PRIMARY,
-                                                    DriveConstants.DRIVE_TIMEOUT_MS);
+  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
+   */
+  public void selectSong(String song) {
+    SmartDashboard.putString("Selected Song", song);
+    m_orchestra.loadMusic(song);
+  }
+  
+  /*
+   * Plays Music!
+   */
+  public void playSong() {
+    m_orchestra.play();
+  }
+
+  /**
+   * Resets the encoders for both motors.
+   */
+  public void resetEncoders() {
+    m_leftFrontMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightFrontMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_leftBackMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightBackMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS);
+
+    m_totalLeftDistanceInches = 0;
+    m_totalRightDistanceInches = 0;
+  }
+
+  /**
+   * Resets the odometry to the specified pose.
+   * 
+   * @param pose The pose to which to set the odometry.
+   */
+  public void setOdometry(Pose2d pose) {
+    resetEncoders();
+    m_odometry.resetPosition(pose, Rotation2d.fromDegrees(getHeading()));
+  }
+
+  /**
+   * Resets the yaw of the pigeon
+   */
+  public void resetGyroYaw() {
+    m_pigeon.setYaw(0);
+    m_pigeon.setAccumZAngle(0);
+    resetGyroAngles();
+  }
+
+  /**
+   * Add docs here
+   */
+  public void resetGyroAngles() {
+    m_lastAngleYaw = 0;
+    m_currentAngleYaw = 0;
   }
 
   /**
@@ -553,29 +559,13 @@ public class Drive extends SubsystemBase {
     return ypr[2];
   }
 
-  /**
-   * Resets the yaw of the pigeon
-   */
-  public void resetGyroYaw() {
-    m_pigeon.setYaw(0);
-    m_pigeon.setAccumZAngle(0);
-    resetGyroAngles();
-  }
-
-  /**
-   * Add docs here
-   */
-  public void resetGyroAngles() {
-    m_lastAngleYaw = 0;
-    m_currentAngleYaw = 0;
-    m_kinematicsTargetAngle = 0;
-  }
 //lol
 //sko
 //ridge
-  /**
 //brayden=bad coder
-  * Returns the heading of the robot
+
+  /**
+   * Returns the heading of the robot
    * @return The robot's heading in degrees, from -180 to 180
    */
   public double getHeading() {
@@ -589,7 +579,7 @@ public class Drive extends SubsystemBase {
    */
   public double getTurnRate() {
     double deltaYaw = m_currentAngleYaw - m_lastAngleYaw;
-    double turnRate = 1000 * deltaYaw / m_deltaTime;
+    double turnRate = 1000 * deltaYaw / m_deltaTimeMs;
     return turnRate;
   }
 
@@ -606,28 +596,8 @@ public class Drive extends SubsystemBase {
    * @return The current wheel speeds.
    */
   public DifferentialDriveWheelSpeeds getWheelSpeeds() {
-    return new DifferentialDriveWheelSpeeds(  inchesToMeters(getVelocityInchesPerSecond(m_leftBackMotor)), 
-                                              -inchesToMeters(getVelocityInchesPerSecond(m_rightBackMotor)));
-  }
-
-  /**
-   * Resets the encoders for both motors.
-   */
-  public void resetEncoders() {
-    m_leftFrontMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_leftBackMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightBackMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS);
-  }
-
-  /**
-   * Resets the odometry to the specified pose.
-   * 
-   * @param pose The pose to which to set the odometry.
-   */
-  public void setOdometry(Pose2d pose) {
-    resetEncoders();
-    m_odometry.resetPosition(pose, Rotation2d.fromDegrees(getHeading()));
+    return new DifferentialDriveWheelSpeeds(  getVelocityInchesPerSecond(m_leftBackMotor), 
+                                              -getVelocityInchesPerSecond(m_rightBackMotor));
   }
 
   /**
@@ -654,7 +624,11 @@ public class Drive extends SubsystemBase {
    * @return The converted value in inches
    */
   public double  ticksToInches(double ticks) {
-    return ticks * DriveConstants.INCHES_PER_TICK;
+    if (m_isSpeedShiftHigh) {
+      return ticks * DriveConstants.INCHES_PER_TICK_HIGH;
+    } else {
+      return ticks * DriveConstants.INCHES_PER_TICK_LOW;
+    }
   }
 
   /**
@@ -663,7 +637,11 @@ public class Drive extends SubsystemBase {
    * @return The converted value in ticks
    */
   public double inchesToTicks(double inches) {
-    return inches * DriveConstants.TICKS_PER_INCH;
+    if (m_isSpeedShiftHigh) {
+      return inches * DriveConstants.TICKS_PER_INCH_HIGH;
+    } else {
+      return inches * DriveConstants.TICKS_PER_INCH_LOW;
+    }
   }
 
   /**
@@ -683,46 +661,145 @@ public class Drive extends SubsystemBase {
   public double metersToInches(double meters) {
     return meters * DriveConstants.INCHES_PER_METER;
   }
-  
-  /*
-   * Plays Music!
-   */
-  public void playSong() {
-    m_orchestra.play();
-  }
 
-  /**
-   * Selects a song to play!
-   * @param song The name of the song to be played
-   */
-  public void selectSong(String song) {
-    SmartDashboard.putString("Selected Song", song);
-    m_orchestra.loadMusic(song);
-  }
-  /**
-   * 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.kForward);
-		}
-		if (state == false) {
-			m_speedShift.set(DoubleSolenoid.Value.kReverse);
-		}
-  }
+  public void setRightMotorGains(boolean isHighGear) {
+    if (!isHighGear) {
+      m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
+      m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
 
-  /**
-   * 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);
+      m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
+      m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, m_gainsTurningLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, m_gainsTurningLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, m_gainsTurningLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, m_gainsTurningLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, m_gainsTurningLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
+
+      m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY);
+      m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, m_gainsDistanceLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, m_gainsDistanceLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, m_gainsDistanceLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, m_gainsDistanceLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_DISTANCE, m_gainsDistanceLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
+
+      m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY);
+      m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.configMotionSCurveStrength(0, DriveConstants.DRIVE_TIMEOUT_MS);
+    } else {
+      m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
+      m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocityHigh.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityHigh.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocityHigh.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocityHigh.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityHigh.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
+
+      m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
+      m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, m_gainsTurningHigh.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, m_gainsTurningHigh.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, m_gainsTurningHigh.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, m_gainsTurningHigh.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, m_gainsTurningHigh.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
+
+      m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY);
+      m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, m_gainsDistanceHigh.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, m_gainsDistanceHigh.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, m_gainsDistanceHigh.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, m_gainsDistanceHigh.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_DISTANCE, m_gainsDistanceHigh.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
+
+      m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY);
+      m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicHigh.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicHigh.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicHigh.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicHigh.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicHigh.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY_HIGH, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION_HIGH, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.configMotionSCurveStrength(0, DriveConstants.DRIVE_TIMEOUT_MS);
     }
   }
 
+  /**
+   * Sets Motors to a NeutralMode.
+   * 
+   * @param mode NeutralMode to set motors to
+   */
+  public void setDriveTrainNeutralMode(NeutralMode mode) {
+    m_leftFrontMotor.setNeutralMode(mode);
+    m_rightFrontMotor.setNeutralMode(mode);
+    m_leftBackMotor.setNeutralMode(mode);
+    m_rightBackMotor.setNeutralMode(mode);
+  }
+
+  public void updateSmartDashboard() {
+    try {
+      //SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw());
+      //SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch());
+      //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 Motor Velocity Raw", m_leftBackMotor.getSelectedSensorVelocity());
+      //SmartDashboard.putNumber("Right Back Motor Velocity Raw", m_rightBackMotor.getSelectedSensorVelocity());
+      //SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition());
+      //SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0));
+      
+      //SmartDashboard.putNumber("Right Motor Velocity Int Sensor", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
+      //SmartDashboard.putNumber("Left Motor Velocity Int Sensor", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
+      SmartDashboard.putNumber("Left Motor Pos Inches", getDistanceInches(m_rightFrontMotor));
+      SmartDashboard.putNumber("Right Motor Pos Inches", getDistanceInches(m_leftFrontMotor));
+
+      /*SmartDashboard.putNumber("Right Front Velocity", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
+      SmartDashboard.putNumber("Left Front Velocity", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
+      SmartDashboard.putNumber("Right Back Velocity", m_rightBackMotor.getSensorCollection().getIntegratedSensorVelocity());
+      SmartDashboard.putNumber("Left Back Velocity", m_leftBackMotor.getSensorCollection().getIntegratedSensorVelocity());
+      */
+
+      SmartDashboard.putNumber("Right Motor Temp", m_rightFrontMotor.getTemperature());
+      SmartDashboard.putNumber("Left Motor Temp", m_leftFrontMotor.getTemperature());
+
+      SmartDashboard.putNumber("Right Front Motor Current Supply", m_rightFrontMotor.getSupplyCurrent());
+      SmartDashboard.putNumber("Left Front Motor Current Supply", m_leftFrontMotor.getSupplyCurrent());
+      SmartDashboard.putNumber("Right Back Motor Current Supply", m_rightBackMotor.getSupplyCurrent());
+      SmartDashboard.putNumber("Left Back Motor Current Supply", m_leftBackMotor.getSupplyCurrent());
+      
+      SmartDashboard.putNumber("Right Front Motor Current Stator ", m_rightFrontMotor.getStatorCurrent());
+      SmartDashboard.putNumber("Left Front Motor Current Stator", m_leftFrontMotor.getStatorCurrent());
+      SmartDashboard.putNumber("Right Back Motor Current Stator ", m_rightBackMotor.getStatorCurrent());
+      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 0 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_PRIMARY));
+      //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));
+
+      //SmartDashboard.putString("Odometry Values Meters", getPose().toString());
+      //SmartDashboard.putNumber("Odometry Heading", getHeading());
+
+      //SmartDashboard.putNumber("Time Seconds", m_currentTimeSec);
+      //SmartDashboard.putNumber("Delta Time", m_deltaTime);
+
+      if (m_currentSong != m_songChooser.getSelected()){
+        m_currentSong = m_songChooser.getSelected();
+        selectSong(m_currentSong);
+        //System.err.println(m_currentSong);
+      }
+    } catch (Exception e) {
+      System.err.println("Error while using Drive SmartDashboard");
+      // e.printStackTrace(System.err);
+    }
+  }
 }
diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java
index c4487ce..9bca6c9 100644
--- a/src/main/java/frc4388/robot/subsystems/Shooter.java
+++ b/src/main/java/frc4388/robot/subsystems/Shooter.java
@@ -24,6 +24,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Gains;
 import frc4388.robot.Constants.ShooterConstants;
+import frc4388.utility.ShooterTables;
 import frc4388.utility.controller.IHandController;
 
 public class Shooter extends SubsystemBase {
@@ -48,6 +49,9 @@ public class Shooter extends SubsystemBase {
 
   double velP;
   double input;
+
+  ShooterTables m_shooterTable;
+  
   public boolean velReached;
   public double m_fireVel;
   public double m_fireAngle;
@@ -74,6 +78,18 @@ public class Shooter extends SubsystemBase {
     int closedLoopTimeMs = 1;
     m_shooterFalcon.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS);
     m_shooterFalcon.configClosedLoopPeriod(1, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS);
+
+    m_shooterTable = new ShooterTables();
+
+    SmartDashboard.putNumber("CSV 10", m_shooterTable.getVelocity(10));
+    SmartDashboard.putNumber("CSV 200", m_shooterTable.getVelocity(200));
+    SmartDashboard.putNumber("CSV 250", m_shooterTable.getVelocity(250));
+    SmartDashboard.putNumber("CSV 500", m_shooterTable.getVelocity(500));
+
+    SmartDashboard.putNumber("CSV A -30", m_shooterTable.getAngleDisplacement(-30));
+    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));
   }
 
   @Override
diff --git a/src/main/java/frc4388/utility/ShooterTables.java b/src/main/java/frc4388/utility/ShooterTables.java
new file mode 100644
index 0000000..638f140
--- /dev/null
+++ b/src/main/java/frc4388/utility/ShooterTables.java
@@ -0,0 +1,149 @@
+/*----------------------------------------------------------------------------*/
+/* 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.utility;
+
+import java.io.BufferedReader;
+import java.io.File;
+import java.io.FileNotFoundException;
+import java.io.FileReader;
+import java.io.IOException;
+
+import edu.wpi.first.wpilibj.Filesystem;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * Add your docs here.
+ */
+public class ShooterTables {
+    double[][] m_distance = new double[50][3];
+    double[][] m_angle = new double[50][2];
+
+    final int m_columnDistance = 0;
+    final int m_columnHood = 1;
+    final int m_columnVel = 2;
+    final int m_columnAngle = 0;
+    final int m_columnDisplacement = 1;
+
+    int m_distanceLength;
+    int m_angleLength;
+
+    public ShooterTables() {
+        int lineNum = 0;
+
+        File distanceCSV = new File(Filesystem.getDeployDirectory().getAbsolutePath() + "/Robot Data - Distances.csv");
+        File angleCSV = new File(Filesystem.getDeployDirectory().getAbsolutePath() + "/Robot Data - Angles.csv");
+        
+        try {
+
+
+            BufferedReader distanceReader = new BufferedReader(new FileReader(distanceCSV));
+            BufferedReader angleReader = new BufferedReader(new FileReader(angleCSV));
+            String line = "";
+
+            while ((line = distanceReader.readLine()) != null) {
+
+                if (lineNum == 0) {
+                    lineNum ++;
+                } else {
+                    String[] values = line.split(",");
+
+                    m_distance[lineNum - 1][m_columnDistance] = Double.parseDouble(values[0]);
+                    m_distance[lineNum - 1][m_columnHood] = Double.parseDouble(values[1]);
+                    m_distance[lineNum - 1][m_columnVel] = Double.parseDouble(values[2]);
+                    
+                    lineNum ++;
+                }
+
+            }
+
+            m_distanceLength = lineNum-1;
+            lineNum = 0;
+
+            while ((line = angleReader.readLine()) != null) {
+
+                if (lineNum == 0) {
+                    lineNum ++;
+                } else {
+                    String[] values = line.split(",");
+
+                    m_angle[lineNum - 1][m_columnAngle] = Double.parseDouble(values[0]);
+                    m_angle[lineNum - 1][m_columnDisplacement] = Double.parseDouble(values[1]);
+                    
+                    lineNum ++;
+                }
+            }
+
+            m_angleLength = lineNum-1;
+
+        } catch (FileNotFoundException e) {
+            e.printStackTrace();
+        } catch (IOException e) {
+            e.printStackTrace();
+        }
+        SmartDashboard.putNumber("Row 2 Column 1", m_angle[1][0]);
+        SmartDashboard.putNumber("Row 4 Column 2", m_angle[3][1]);
+        SmartDashboard.putNumber("m_distanceLength", m_distanceLength);
+        SmartDashboard.putNumber("Distance last row 0", m_distance[m_distanceLength-1][0]);
+        SmartDashboard.putNumber("Distance last row 1", m_distance[m_distanceLength-1][1]);
+        SmartDashboard.putNumber("Distance last row 2", m_distance[m_distanceLength-1][2]);
+    }
+
+    public double getHood(double distance) {
+        int i = 0;
+        while ((i < m_distanceLength) && (m_distance[i][m_columnDistance] < distance)) {
+            i ++;
+        }
+        if ((i < m_distanceLength) && (m_distance[i][m_columnDistance] == distance)) {
+            return m_distance[i][m_columnHood];
+        } else {
+            if (i >= m_distanceLength) {
+                i = m_distanceLength - 1;
+            }
+            return linearInterpolation(i, distance, m_columnHood, m_distance);
+        }
+    }
+
+    public double getVelocity(double distance) {
+        int i = 0;
+        while ((i < m_distanceLength) && (m_distance[i][m_columnDistance] < distance)) {
+            i ++;
+        }
+        if ((i < m_distanceLength) && (m_distance[i][m_columnDistance] == distance)) {
+            return m_distance[i][m_columnVel];
+        } else {
+            if (i >= m_distanceLength) {
+                i = m_distanceLength - 1;
+            }
+            return linearInterpolation(i, distance, m_columnVel, m_distance);
+        }
+    }
+
+    public double getAngleDisplacement(double angle) {
+        int i = 0;
+        while ((i < m_angleLength) && (m_angle[i][m_columnAngle] < angle)) {
+            i ++;
+        }
+        if ((i < m_angleLength) && (m_angle[i][m_columnAngle] == angle)) {
+            return m_angle[i][m_columnDisplacement];
+        } else {
+            if (i >= m_angleLength) {
+                i = m_angleLength - 1;
+            }
+            return linearInterpolation(i, angle, m_columnDisplacement, m_angle);
+        }
+    }
+
+    public double linearInterpolation(int i, double value, int column, double[][] table) {
+        if (i != 0) {
+            double slope = (table[i][column] - table[i-1][column]) / (table[i][0] - table[i-1][0]);
+            value = slope * (value - table[i-1][0]) + table[i-1][column];
+            return value;
+        }
+        return 0.0;
+    }
+}
\ No newline at end of file