diff --git a/src/main/java/org/usfirst/frc/team1701/robot/OI.java b/src/main/java/org/usfirst/frc/team1701/robot/OI.java
index bc8aead..af8acd3 100644
--- a/src/main/java/org/usfirst/frc/team1701/robot/OI.java
+++ b/src/main/java/org/usfirst/frc/team1701/robot/OI.java
@@ -33,7 +33,6 @@ public class OI {
   public static JoystickButton manualWristUp;
   public static JoystickButton manualWristDown;
   public static JoystickButton gear;
-
   public static JoystickButton manualWristReset;
 
   public static Joystick operation;
@@ -75,13 +74,13 @@ public OI() {
     climbPosition = new JoystickButton(operation, 6);
     climbPosition.whenPressed(new ClimbPosition());
     climb = new JoystickButton(operation, 5);
-    climb.whenPressed(new Climb());
+    climb.whileHeld(new Climb());
     climb.whenReleased(new StopClimb());
     manualWinchUp = new JoystickButton(operation, 3);
-    manualWinchUp.whenPressed(new WinchDown());
+    manualWinchUp.whileHeld(new WinchUp());
     manualWinchUp.whenReleased(new WinchStop());
     manualWinchDown = new JoystickButton(operation, 10);
-    manualWinchDown.whenPressed(new WinchUp());
+    manualWinchDown.whileHeld(new WinchDown());
     manualWinchDown.whenReleased(new WinchStop());
     manualWristUp = new JoystickButton(operation, 7);
     manualWristUp.whenPressed(new WristDown());
@@ -90,9 +89,10 @@ public OI() {
     manualWristDown.whenPressed(new WristUp());
     manualWristDown.whenReleased(new WristStop());
     gear = new JoystickButton(drive_FB,3);
-    gear.whenPressed(new HighGear());
+    gear.whenPressed(new AutoGear());
     gear.whenReleased(new LowGear());
 
+
     manualWristReset = new JoystickButton(operation,15);
     manualWristReset.whenPressed(new ResetWrist());
   }
diff --git a/src/main/java/org/usfirst/frc/team1701/robot/Robot.java b/src/main/java/org/usfirst/frc/team1701/robot/Robot.java
index 7dafa32..9ae79bf 100644
--- a/src/main/java/org/usfirst/frc/team1701/robot/Robot.java
+++ b/src/main/java/org/usfirst/frc/team1701/robot/Robot.java
@@ -13,7 +13,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Timer;
-import org.usfirst.frc.team1701.robot.commands.AutoCommandGroup;
+import org.usfirst.frc.team1701.robot.commands.*;
 import org.usfirst.frc.team1701.robot.subsystems.*;
 
 /*
@@ -39,44 +39,28 @@ public class Robot extends IterativeRobot {
    */
   private DriverStation ds = DriverStation.getInstance();
   private String gameCode = "";
-  public static SendableChooser<Number> autonomousLocation;
-  public static SendableChooser<Number> action;
   public static OI oi;
   public static DriveTrain driveTrain;
   public static LiftArm liftArm;
   public static Vision vision;
   public static Position position;
+  public static Shuffleboard shuffleboard;
   /*
    * This function is run when the robot is first started up.
    */
   public void robotInit() {
     RobotMap.init(); // Initialize our RobotMap.
+    Shuffleboard.init();
     driveTrain = new DriveTrain();
     vision = new Vision();
     liftArm = new LiftArm();
     position = new Position();
     oi = new OI();
-    autonomousLocation = new SendableChooser<>();
-    autonomousLocation.addObject("Left",1);
-    autonomousLocation.addObject("Left-Switch",4);
-    autonomousLocation.addDefault("Middle",2);
-    autonomousLocation.addObject("Right-Switch",5);
-    autonomousLocation.addObject("Right",3);
-    SmartDashboard.putData("Autonomous Location", autonomousLocation);
-    action = new SendableChooser<>();
-    action.addDefault("Defualt Autonomous", 1);
-    action.addObject("Forward Autonomous", 2);
-    SmartDashboard.putData("Autonomous Chooser",action);
-    SmartDashboard.putBoolean("Reversed", false);
-    SmartDashboard.putString("Current Gear","");
-    SmartDashboard.putNumber("Arm",0);
-    SmartDashboard.putBoolean("Arm Down", false);
-    SmartDashboard.putNumber("D", 0);
 
     vision.setPIPMode(2);
     driveTrain.resetEncoders();
-
-
+    driveTrain.setBrakeMode();
+    driveTrain.setLowGear();
   }
   /*
    * This function is called when the robot has been disabled.
@@ -99,7 +83,7 @@ public void disabledPeriodic() {
    */
   public void autonomousInit() {
     if(gameCode.length() == 3) {
-      CommandGroup autonomousCommand = new AutoCommandGroup(gameCode,action.getSelected(),autonomousLocation.getSelected());
+      CommandGroup autonomousCommand = new AutoCommandGroup(gameCode, Shuffleboard.action.getSelected(), Shuffleboard.autonomousLocation.getSelected());
       autonomousCommand.start();
     }
   }
@@ -128,4 +112,6 @@ public void teleopPeriodic() {
    * This function is called periodically during test mode.
    */
   public void testPeriodic() {}
+
+
 }
diff --git a/src/main/java/org/usfirst/frc/team1701/robot/RobotMap.java b/src/main/java/org/usfirst/frc/team1701/robot/RobotMap.java
index 39ab210..d03ed03 100644
--- a/src/main/java/org/usfirst/frc/team1701/robot/RobotMap.java
+++ b/src/main/java/org/usfirst/frc/team1701/robot/RobotMap.java
@@ -10,15 +10,13 @@
 import com.kauailabs.navx.frc.AHRS;
 import edu.wpi.first.wpilibj.*;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
-/*import org.apache.logging.log4j.LogManager;
-import org.apache.logging.log4j.Logger;*/
+
 
 public class RobotMap {
   /*
    * Create all static variables, most (exception of logger) filled
    * by the init() method below.
    */
-  //private static final Logger logger = LogManager.getLogger();
   public static WPI_TalonSRX _leftFrontMotor;
   public static WPI_TalonSRX _leftBackMotor;
   public static WPI_TalonSRX _rightFrontMotor;
@@ -36,18 +34,18 @@ public class RobotMap {
   public static DoubleSolenoid winchBrake;
   public static DoubleSolenoid wristBrake;
   public static AnalogInput liftArmEncoder;
+  public static AnalogInput wristEncoder;
   public static DigitalInput cubeSensor;
   public static DigitalInput armSensor;
-  public static Spark _led0;
-  public static Spark _led1;
   public static AHRS _navx;
   public static int encPidIdx;
-  
+  public static double armSpeed = 1.0;
+
   /**
    * Initialize the public values above.
    */
   public static void init() {
-    _leftFrontMotor = new WPI_TalonSRX(0); //0
+    _leftFrontMotor = new WPI_TalonSRX(3); //0
     _leftBackMotor = new WPI_TalonSRX(1); //1
     _rightFrontMotor = new WPI_TalonSRX(15); //15
     _rightBackMotor = new WPI_TalonSRX(14); //14
@@ -60,6 +58,7 @@ public static void init() {
      * Initialize all analog and digital objects
      */
     liftArmEncoder = new AnalogInput(0);
+    wristEncoder = new AnalogInput(1);
     cubeSensor = new DigitalInput(0);
     armSensor = new DigitalInput(1);
 
@@ -88,12 +87,6 @@ public static void init() {
      * Instantiate NavX.
      */
     _navx = new AHRS(SerialPort.Port.kUSB1);
-    /**
-     * LED controllers, follows Spark motor controller pattern.
-     * @see https://www.revrobotics.com/content/docs/REV-11-1105-UM.pdf, page 7
-     */
-    _led0 = new Spark(0);
-    _led1 = new Spark(1);
-
   }
+
 }
diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/CrossPlatformZone.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/CrossPlatformZone.java
deleted file mode 100644
index 21a318d..0000000
--- a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/CrossPlatformZone.java
+++ /dev/null
@@ -1,34 +0,0 @@
-/**
- * CrossPlatformZone.java
- *
- * @author Noah Husby
- * @since 2/22/18
- * @license BSD-3-Clause
- */
-package org.usfirst.frc.team1701.robot.commands.Auto;
-
-import edu.wpi.first.wpilibj.command.Command;
-import org.usfirst.frc.team1701.robot.Robot;
-
-public class CrossPlatformZone extends Command{
-    boolean isFinished = false;
-
-    public CrossPlatformZone() {
-        requires(Robot.driveTrain);
-    }
-    protected void initialize() {
-        Robot.driveTrain.resetEncoders();
-    }
-    protected void execute() {
-        if(Robot.driveTrain.getRightDistance() < Robot.position.crossPlatformZone) {
-            Robot.driveTrain.teleopControl(-Robot.position.autonomousSpeed,0);
-        } else  {
-            isFinished = true;
-        }
-    }
-    protected boolean isFinished() {
-        return isFinished;
-    }
-    protected void end() {}
-    protected void interrupted() {}
-}
diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/Drive.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/Drive.java
index 5b405cd..85387e2 100644
--- a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/Drive.java
+++ b/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/Drive.java
@@ -1,45 +1,56 @@
 package org.usfirst.frc.team1701.robot.commands.Auto;
 
 import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import org.usfirst.frc.team1701.robot.Robot;
 
 public class Drive extends Command {
     boolean isFinished = false;
     double distance = 0;
     double currentSpeed;
+    double startAngle;
     boolean setHighGear = false;
 
     public Drive(double distance,double speed) {
+        Robot.driveTrain.driveSpeed = 0;
+
         requires(Robot.driveTrain);
         this.distance = distance;
         this.currentSpeed = speed;
     }
     public Drive(double distance, double speed, boolean setHighGear) {
-        this.distance = distance;
-        this.currentSpeed = speed;
+        this(distance,speed);
         this.setHighGear = setHighGear;
-
     }
     protected void initialize() {
+        startAngle = Robot.driveTrain.getNavxAngle();
         Robot.driveTrain.resetEncoders();
     }
     protected void execute() {
 
-        if(setHighGear) {
+        if(!setHighGear) {
             Robot.driveTrain.setLowGear();
         } else {
             Robot.driveTrain.setHighGear();
         }
 
+        SmartDashboard.putNumber("Drive Train", Robot.driveTrain.getRightDistance());
+
         if(Robot.driveTrain.getRightDistance() < distance) {
-            Robot.driveTrain.teleopControl(-currentSpeed,0);
+            Robot.driveTrain.driveSpeed = -currentSpeed;
+            Robot.driveTrain.setAngle(startAngle);
+            Robot.driveTrain.startPID();
         } else {
+            Robot.driveTrain.stopPID();
+            Robot.driveTrain.stopMotors();
             isFinished = true;
         }
     }
     protected boolean isFinished() {
         return isFinished;
     }
-    protected void end() {}
+    protected void end() {
+        Robot.driveTrain.stopPID();
+    }
     protected void interrupted() {}
 }
diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/DriveForwardCommand.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/DriveForwardCommand.java
deleted file mode 100644
index e2f03ee..0000000
--- a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/DriveForwardCommand.java
+++ /dev/null
@@ -1,44 +0,0 @@
-/**
- * commands/DriveForwardCommand.java
- *
- * @author Nicholas Hubbard
- * @since 2018-01-31
- * @license BSD-3-Clause
- */
-package org.usfirst.frc.team1701.robot.commands.Auto;
-import edu.wpi.first.wpilibj.command.Command;
-import org.usfirst.frc.team1701.robot.Robot;
-
-public class DriveForwardCommand extends Command {
-  //distance to switch - length of robot / wheel circ
-  //private double forwardDistance = (168 - 38) / (4 * Math.PI);
-  private double forwardDistance = 11;
-
-  private boolean isFinished = false;
-
-  public DriveForwardCommand() {
-
-  }
-  protected void initialize() {
-  }
-  protected void execute() {
-
-    if(Robot.driveTrain.getRightDistance() < forwardDistance) {
-      Robot.driveTrain.setLowGear();
-      Robot.driveTrain.leftDriveControl(-0.6);
-      Robot.driveTrain.rightDriveControl(0.6);
-  } else {
-      Robot.driveTrain.stopMotors();
-      isFinished = true;
-    }
-  }
-  protected boolean isFinished() {
-    return isFinished;
-  }
-  protected void end() {
-    Robot.driveTrain.stopMotors();
-  }
-  protected void interrupted() {
-    Robot.driveTrain.stopMotors();
-  }
-}
diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/MiddleToSwitchTurn.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/MiddleToSwitchTurn.java
deleted file mode 100644
index 943b68b..0000000
--- a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/MiddleToSwitchTurn.java
+++ /dev/null
@@ -1,39 +0,0 @@
-/**
- * MiddleToSwitchTurn.java
- *
- * @author Noah Husby
- * @since 2/22/18
- * @license BSD-3-Clause
- */
-package org.usfirst.frc.team1701.robot.commands.Auto;
-
-import edu.wpi.first.wpilibj.command.Command;
-import org.usfirst.frc.team1701.robot.Robot;
-
-public class MiddleToSwitchTurn extends Command {
-    boolean isFinished = false;
-    int count;
-
-    public MiddleToSwitchTurn() {
-        requires(Robot.driveTrain);
-    }
-    protected void initialize() {
-        Robot.driveTrain.resetEncoders();
-        Robot.driveTrain.stopMotors();
-        count = 0;
-    }
-    protected void execute() {
-        isFinished = false;
-        if(Robot.driveTrain.getRightDistance() <= Robot.position.centerToSide && count < 20) {
-            Robot.driveTrain.teleopControl(-1,0);
-            count++;
-        } else  {
-            isFinished = true;
-        }
-    }
-    protected boolean isFinished() {
-        return isFinished;
-    }
-    protected void end() {}
-    protected void interrupted() {}
-}
diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/PlatformToScale.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/PlatformToScale.java
deleted file mode 100644
index a903cd2..0000000
--- a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/PlatformToScale.java
+++ /dev/null
@@ -1,34 +0,0 @@
-/**
- * PlatformToScale.java
- *
- * @author Noah Husby
- * @since 2/22/18
- * @license BSD-3-Clause
- */
-package org.usfirst.frc.team1701.robot.commands.Auto;
-
-import edu.wpi.first.wpilibj.command.Command;
-import org.usfirst.frc.team1701.robot.Robot;
-
-public class PlatformToScale extends Command {
-    boolean isFinished = false;
-
-    public PlatformToScale() {
-        requires(Robot.driveTrain);
-    }
-    protected void initialize() {
-        Robot.driveTrain.resetEncoders();
-    }
-    protected void execute() {
-        if(Robot.driveTrain.getRightDistance() < Robot.position.platformToScale) {
-            Robot.driveTrain.teleopControl(-Robot.position.autonomousSpeed,0);
-        } else  {
-            isFinished = true;
-        }
-    }
-    protected boolean isFinished() {
-        return isFinished;
-    }
-    protected void end() {}
-    protected void interrupted() {}
-}
diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/ReverseScale.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/ReverseScale.java
deleted file mode 100644
index 0651c27..0000000
--- a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/ReverseScale.java
+++ /dev/null
@@ -1,35 +0,0 @@
-/**
- * ReverseScale.java
- *
- * @author Noah Husby
- * @since 2/22/18
- * @license BSD-3-Clause
- */
-package org.usfirst.frc.team1701.robot.commands.Auto;
-
-import edu.wpi.first.wpilibj.command.Command;
-import org.usfirst.frc.team1701.robot.Robot;
-
-public class ReverseScale extends Command {
-    boolean isFinished = false;
-
-    public ReverseScale() {
-        requires(Robot.driveTrain);
-    }
-    protected void initialize() {
-        Robot.driveTrain.resetEncoders();
-    }
-    protected void execute() {
-        if(Robot.driveTrain.getRightDistance() < Robot.position.scaleReverse) {
-            Robot.driveTrain.teleopControl(-Robot.position.autonomousSpeed,0);
-        } else  {
-            isFinished = true;
-        }
-    }
-    protected boolean isFinished() {
-        return isFinished;
-    }
-    protected void end() {}
-    protected void interrupted() {}
-
-}
diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/SlightLeft.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/SlightLeft.java
deleted file mode 100644
index 3882403..0000000
--- a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/SlightLeft.java
+++ /dev/null
@@ -1,30 +0,0 @@
-package org.usfirst.frc.team1701.robot.commands.Auto;
-
-import edu.wpi.first.wpilibj.command.Command;
-import org.usfirst.frc.team1701.robot.Robot;
-
-public class SlightLeft extends Command {
-    boolean isFinished = false;
-    public double navxStart;
-
-    public SlightLeft() {
-        requires(Robot.driveTrain);
-    }
-    protected void initialize() {
-        navxStart = Robot.driveTrain.getNavxAngle();
-    }
-    protected void execute() {
-        if(Robot.driveTrain.getNavxAngle() > navxStart + Robot.position.slightLeftAngle) {
-            Robot.driveTrain.teleopControl(0,0.75);
-        } else {
-            isFinished = true;
-        }
-    }
-    protected boolean isFinished() {
-        return isFinished;
-    }
-    protected void end() {
-        Robot.driveTrain.resetEncoders();
-    }
-    protected void interrupted() {}
-}
diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/SlightRight.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/SlightRight.java
deleted file mode 100644
index 5e355ef..0000000
--- a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/SlightRight.java
+++ /dev/null
@@ -1,30 +0,0 @@
-package org.usfirst.frc.team1701.robot.commands.Auto;
-
-import edu.wpi.first.wpilibj.command.Command;
-import org.usfirst.frc.team1701.robot.Robot;
-
-public class SlightRight extends Command{
-    boolean isFinished = false;
-    public double navxStart;
-
-    public SlightRight() {
-        requires(Robot.driveTrain);
-    }
-    protected void initialize() {
-        navxStart = Robot.driveTrain.getNavxAngle();
-    }
-    protected void execute() {
-        if(Robot.driveTrain.getNavxAngle() > navxStart - Robot.position.slightRightAngle) {
-            Robot.driveTrain.teleopControl(0,-0.75);
-        } else {
-            isFinished = true;
-        }
-    }
-    protected boolean isFinished() {
-        return isFinished;
-    }
-    protected void end() {
-        Robot.driveTrain.resetEncoders();
-    }
-    protected void interrupted() {}
-}
diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/WallToMiddleLeft.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/Turn.java
similarity index 53%
rename from src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/WallToMiddleLeft.java
rename to src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/Turn.java
index efdc71e..5e79c03 100644
--- a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/WallToMiddleLeft.java
+++ b/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/Turn.java
@@ -1,32 +1,29 @@
-/**
- * WallToMiddleRight.java
- *
- * @author Noah Husby
- * @since 2/22/18
- * @license BSD-3-Clause
- */
 package org.usfirst.frc.team1701.robot.commands.Auto;
 
 import edu.wpi.first.wpilibj.command.Command;
 import org.usfirst.frc.team1701.robot.Robot;
 
-public class WallToMiddleLeft extends Command {
+public class Turn extends Command {
 
+    double startAngle = 0;
     boolean isFinished = false;
 
-    public WallToMiddleLeft() {
+    public Turn(double angle) {
         requires(Robot.driveTrain);
+        this.startAngle = angle + Robot.driveTrain.getNavxAngle();
     }
     protected void initialize() {
-        Robot.driveTrain.resetEncoders();
     }
     protected void execute() {
-        isFinished = false;
-        if(Robot.driveTrain.getRightDistance() < Robot.position.wallToCubeLeft) {
-            Robot.driveTrain.teleopControl(-1,0);
-        } else  {
+        Robot.driveTrain.driveSpeed = 0;
+        Robot.driveTrain.setAngle(startAngle);
+        Robot.driveTrain.startPID();
+        if(Robot.driveTrain.onTarget()) {
+            Robot.driveTrain.stopPID();
+            Robot.driveTrain.stopMotors();
             isFinished = true;
         }
+
     }
     protected boolean isFinished() {
         return isFinished;
diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/TurnLeft.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/TurnLeft.java
deleted file mode 100644
index 9f2c722..0000000
--- a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/TurnLeft.java
+++ /dev/null
@@ -1,38 +0,0 @@
-/**
- * TurnLeft.java
- *
- * @author Noah Husby
- * @since 2/22/18
- * @license BSD-3-Clause
- */
-package org.usfirst.frc.team1701.robot.commands.Auto;
-
-import edu.wpi.first.wpilibj.command.Command;
-import org.usfirst.frc.team1701.robot.Robot;
-
-public class TurnLeft extends Command {
-    boolean isFinished = false;
-
-    public double navxStart;
-
-    public TurnLeft() {
-        requires(Robot.driveTrain);
-    }
-    protected void initialize() {
-        navxStart = Robot.driveTrain.getNavxAngle();
-    }
-    protected void execute() {
-        if(Robot.driveTrain.getNavxAngle() < navxStart + Robot.position.leftAngle) {
-            Robot.driveTrain.teleopControl(0,0.75);
-        } else {
-            isFinished = true;
-        }
-    }
-    protected boolean isFinished() {
-        return isFinished;
-    }
-    protected void end() {
-        Robot.driveTrain.resetEncoders();
-    }
-    protected void interrupted() {}
-}
diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/TurnRight.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/TurnRight.java
deleted file mode 100644
index 6e09011..0000000
--- a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/TurnRight.java
+++ /dev/null
@@ -1,37 +0,0 @@
-/**
- * TurnRight.java
- *
- * @author Noah Husby
- * @since 2/22/18
- * @license BSD-3-Clause
- */
-package org.usfirst.frc.team1701.robot.commands.Auto;
-
-import edu.wpi.first.wpilibj.command.Command;
-import org.usfirst.frc.team1701.robot.Robot;
-
-public class TurnRight extends Command {
-    boolean isFinished = false;
-    public double navxStart;
-
-    public TurnRight() {
-        requires(Robot.driveTrain);
-    }
-    protected void initialize() {
-        navxStart = Robot.driveTrain.getNavxAngle();
-    }
-    protected void execute() {
-        if(Robot.driveTrain.getNavxAngle() > navxStart - Robot.position.rightAngle) {
-            Robot.driveTrain.teleopControl(0,-0.75);
-        } else {
-            isFinished = true;
-        }
-    }
-    protected boolean isFinished() {
-        return isFinished;
-    }
-    protected void end() {
-        Robot.driveTrain.resetEncoders();
-    }
-    protected void interrupted() {}
-}
diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/TurnToSwitch.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/TurnToSwitch.java
deleted file mode 100644
index 16dbf87..0000000
--- a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/TurnToSwitch.java
+++ /dev/null
@@ -1,34 +0,0 @@
-/**
- * TurnToSwitch.java
- *
- * @author Noah Husby
- * @since 2/22/18
- * @license BSD-3-Clause
- */
-package org.usfirst.frc.team1701.robot.commands.Auto;
-
-import edu.wpi.first.wpilibj.command.Command;
-import org.usfirst.frc.team1701.robot.Robot;
-
-public class TurnToSwitch extends Command {
-    boolean isFinished = false;
-
-    public TurnToSwitch() {
-        requires(Robot.driveTrain);
-    }
-    protected void initialize() {
-        Robot.driveTrain.resetEncoders();
-    }
-    protected void execute() {
-        if(Robot.driveTrain.getRightDistance() < Robot.position.turnToSwitch) {
-            Robot.driveTrain.teleopControl(-Robot.position.autonomousSpeed,0);
-        } else  {
-            isFinished = true;
-        }
-    }
-    protected boolean isFinished() {
-        return isFinished;
-    }
-    protected void end() {}
-    protected void interrupted() {}
-}
diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/TurnToSwitchRight.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/TurnToSwitchRight.java
deleted file mode 100644
index 9ef4c32..0000000
--- a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/TurnToSwitchRight.java
+++ /dev/null
@@ -1,34 +0,0 @@
-/**
- * TurnToSwitch.java
- *
- * @author Noah Husby
- * @since 2/22/18
- * @license BSD-3-Clause
- */
-package org.usfirst.frc.team1701.robot.commands.Auto;
-
-import edu.wpi.first.wpilibj.command.Command;
-import org.usfirst.frc.team1701.robot.Robot;
-
-public class TurnToSwitchRight extends Command {
-    boolean isFinished = false;
-
-    public TurnToSwitchRight() {
-        requires(Robot.driveTrain);
-    }
-    protected void initialize() {
-        Robot.driveTrain.resetEncoders();
-    }
-    protected void execute() {
-        if(Robot.driveTrain.getRightDistance() < Robot.position.turnToSwitchRight) {
-            Robot.driveTrain.teleopControl(-Robot.position.autonomousSpeed,0);
-        } else  {
-            isFinished = true;
-        }
-    }
-    protected boolean isFinished() {
-        return isFinished;
-    }
-    protected void end() {}
-    protected void interrupted() {}
-}
diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/WallToMiddleRight.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/WallToMiddleRight.java
deleted file mode 100644
index fc12d36..0000000
--- a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/WallToMiddleRight.java
+++ /dev/null
@@ -1,36 +0,0 @@
-/**
- * WallToMiddleRight.java
- *
- * @author Noah Husby
- * @since 2/22/18
- * @license BSD-3-Clause
- */
-package org.usfirst.frc.team1701.robot.commands.Auto;
-
-import edu.wpi.first.wpilibj.command.Command;
-import org.usfirst.frc.team1701.robot.Robot;
-
-public class WallToMiddleRight extends Command {
-
-    boolean isFinished = false;
-
-    public WallToMiddleRight() {
-        requires(Robot.driveTrain);
-    }
-    protected void initialize() {
-        Robot.driveTrain.resetEncoders();
-    }
-    protected void execute() {
-        isFinished = false;
-        if(Robot.driveTrain.getRightDistance() < Robot.position.wallToCubeRight) {
-            Robot.driveTrain.teleopControl(-1,0);
-        } else  {
-            isFinished = true;
-        }
-    }
-    protected boolean isFinished() {
-        return isFinished;
-    }
-    protected void end() {}
-    protected void interrupted() {}
-}
diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/WallToPlatformZone.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/WallToPlatformZone.java
deleted file mode 100644
index 526e1a4..0000000
--- a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/WallToPlatformZone.java
+++ /dev/null
@@ -1,34 +0,0 @@
-/**
- * WallToPlatformZone.java
- *
- * @author Noah Husby
- * @since 2/22/18
- * @license BSD-3-Clause
- */
-package org.usfirst.frc.team1701.robot.commands.Auto;
-
-import edu.wpi.first.wpilibj.command.Command;
-import org.usfirst.frc.team1701.robot.Robot;
-
-public class WallToPlatformZone extends Command {
-    boolean isFinished = false;
-
-    public WallToPlatformZone() {
-        requires(Robot.driveTrain);
-    }
-    protected void initialize() {
-        Robot.driveTrain.resetEncoders();
-    }
-    protected void execute() {
-        if(Robot.driveTrain.getRightDistance() < Robot.position.wallToPlatformZone) {
-            Robot.driveTrain.teleopControl(-Robot.position.autonomousSpeed,0);
-        } else  {
-            isFinished = true;
-        }
-    }
-    protected boolean isFinished() {
-        return isFinished;
-    }
-    protected void end() {}
-    protected void interrupted() {}
-}
diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/WallToScale.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/WallToScale.java
deleted file mode 100644
index 56bf4dd..0000000
--- a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/WallToScale.java
+++ /dev/null
@@ -1,34 +0,0 @@
-/**
- * WallToScale.java
- *
- * @author Noah Husby
- * @since 2/22/18
- * @license BSD-3-Clause
- */
-package org.usfirst.frc.team1701.robot.commands.Auto;
-
-import edu.wpi.first.wpilibj.command.Command;
-import org.usfirst.frc.team1701.robot.Robot;
-
-public class WallToScale extends Command {
-    boolean isFinished = false;
-
-    public WallToScale() {
-        requires(Robot.driveTrain);
-    }
-    protected void initialize() {
-        Robot.driveTrain.resetEncoders();
-    }
-    protected void execute() {
-        if(Robot.driveTrain.getRightDistance() < Robot.position.wallToScale) {
-            Robot.driveTrain.teleopControl(-Robot.position.autonomousSpeed,0);
-        } else {
-            isFinished = true;
-        }
-    }
-    protected boolean isFinished() {
-        return isFinished;
-    }
-    protected void end() {}
-    protected void interrupted() {}
-}
diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/WallToStraightSwitch.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/WallToStraightSwitch.java
deleted file mode 100644
index f1f8f98..0000000
--- a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/WallToStraightSwitch.java
+++ /dev/null
@@ -1,28 +0,0 @@
-package org.usfirst.frc.team1701.robot.commands.Auto;
-
-import edu.wpi.first.wpilibj.command.Command;
-import org.usfirst.frc.team1701.robot.Robot;
-
-public class WallToStraightSwitch extends Command {
-    boolean isFinished = false;
-
-    public WallToStraightSwitch() {
-        requires(Robot.driveTrain);
-    }
-    protected void initialize() {
-        Robot.driveTrain.resetEncoders();
-    }
-    protected void execute() {
-        if(Robot.driveTrain.getRightDistance() < Robot.position.wallToStraightSwitch) {
-            Robot.driveTrain.teleopControl(-Robot.position.autonomousSpeed,0);
-        } else {
-            isFinished = true;
-        }
-    }
-    protected boolean isFinished() {
-        return isFinished;
-    }
-    protected void end() {}
-    protected void interrupted() {}
-
-}
diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/AutoCommandGroup.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/AutoCommandGroup.java
index 62125bd..5cd483c 100644
--- a/src/main/java/org/usfirst/frc/team1701/robot/commands/AutoCommandGroup.java
+++ b/src/main/java/org/usfirst/frc/team1701/robot/commands/AutoCommandGroup.java
@@ -33,11 +33,12 @@ public AutoCommandGroup(String gameCode, Number action, Number autoLocation) {
 
         switch ((int) action) {
             case 1:
-                placeCubeAutonomous((int) Robot.autonomousLocation.getSelected());
+                placeCubeAutonomous((int) Robot.shuffleboard.autonomousLocation.getSelected());
+                SmartDashboard.putString("GameCode", gameCode);
                 break;
 
             case 2:
-                addSequential(new DriveForwardCommand());
+                addSequential(new Drive(11,0.6));
                 break;
         }
 
@@ -45,58 +46,66 @@ public AutoCommandGroup(String gameCode, Number action, Number autoLocation) {
 
     private void placeCubeAutonomous(int autoLocation) {
         if (autoLocation == 1) {
+            //Left
             switch (scalePosition) {
                 case 'L':
-                    addSequential(new WallToScale());
-                    addSequential(new StowPosition());
-                    addSequential(new TurnLeft());
+                    Robot.driveTrain.setAutoGear(false);
+                    Robot.driveTrain.setLowGear();
+                    addSequential(new Drive(20,0.82));
+                    addSequential(new Turn(135));
                     addSequential(new ScalePosition());
-                    addSequential(new ReverseScale());
                     addSequential(new ReleaseAndPunch());
                     break;
                 case 'R':
-                    addSequential(new WallToPlatformZone());
-                    addParallel(new StowPosition());
-                    addSequential(new TurnRight());
-                    addSequential(new CrossPlatformZone());
-                    addSequential(new TurnLeft());
-                    addParallel(new ScalePosition());
-                    addSequential(new PlatformToScale());
-                    addSequential(new ReleaseAndPunch());
+                    addSequential(new Drive(10,0.8));
                     break;
             }
         } else if (autoLocation == 2) {
+            //Middle
             switch (switchPosition) {
                 case 'L':
-                    addSequential(new WallToMiddleLeft());
-                    addSequential(new StowPosition());
-                    addSequential(new SlightLeft());
                     addSequential(new SwitchPosition());
-                    addSequential(new TurnToSwitch());
+                    addSequential(new Drive(1,0.8));
+                    addSequential(new Turn(29));
+                    addSequential(new Drive(7,.9));
                     addSequential(new ReleaseAndPunch());
                     break;
                 case 'R':
-                    addSequential(new WallToMiddleRight());
-                    addSequential(new StowPosition());
-                    addSequential(new SlightRight());
                     addSequential(new SwitchPosition());
-                    addSequential(new TurnToSwitchRight());
+                    addSequential(new Drive(1,0.8));
+                    addSequential(new Turn(-27));
+                    addSequential(new Drive(7,.9));
                     addSequential(new ReleaseAndPunch());
+
                     break;
             }
         } else if (autoLocation == 3) {
+            //Right
+            switch (scalePosition) {
+                case 'L':
+                    addSequential(new Drive(10,0.8));
+                    break;
+                case 'R':
+                    Robot.driveTrain.setAutoGear(false);
+                    Robot.driveTrain.setLowGear();
+                    addSequential(new Drive(20,0.82));
+                    addSequential(new Turn(-135));
+                    addSequential(new ScalePosition());
+                    addSequential(new ReleaseAndPunch());
+                    break;
+            }
 
         } else if (autoLocation == 4) {
             //Left-Switch
             switch (switchPosition) {
                 case 'L':
                     addSequential(new SwitchPosition());
-                    addSequential(new WallToStraightSwitch());
+                    addSequential(new Drive(Robot.position.wallToStraightSwitch,Robot.position.autonomousSpeed));
                     addSequential(new ReleaseCube());
                     break;
                 case 'R':
                     addSequential(new SwitchPosition());
-                    addSequential(new WallToStraightSwitch());
+                    addSequential(new Drive(Robot.position.wallToStraightSwitch,Robot.position.autonomousSpeed));
                     break;
             }
         } else if (autoLocation == 5) {
@@ -104,14 +113,16 @@ private void placeCubeAutonomous(int autoLocation) {
             switch (switchPosition) {
                 case 'L':
                     addSequential(new SwitchPosition());
-                    addSequential(new WallToStraightSwitch());
+                    addSequential(new Drive(Robot.position.wallToStraightSwitch,Robot.position.autonomousSpeed));
                     break;
                 case 'R':
                     addSequential(new SwitchPosition());
-                    addSequential(new WallToStraightSwitch());
+                    addSequential(new Drive(Robot.position.wallToStraightSwitch,Robot.position.autonomousSpeed));
                     addSequential(new ReleaseCube());
                     break;
             }
+        } else if (autoLocation == 6) {
+            addSequential(new Turn(-90));
         }
     }
 }
diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/ClimbPositionArm.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/ClimbPositionArm.java
index 9e9381a..3c671e5 100644
--- a/src/main/java/org/usfirst/frc/team1701/robot/commands/ClimbPositionArm.java
+++ b/src/main/java/org/usfirst/frc/team1701/robot/commands/ClimbPositionArm.java
@@ -2,6 +2,7 @@
 
 import edu.wpi.first.wpilibj.command.Command;
 import org.usfirst.frc.team1701.robot.Robot;
+import org.usfirst.frc.team1701.robot.RobotMap;
 
 public class ClimbPositionArm extends Command {
     public ClimbPositionArm() {
@@ -9,8 +10,6 @@ public ClimbPositionArm() {
     }
 
     private boolean isFinished;
-
-
     protected void initialize() {}
     protected void execute() {
 
@@ -20,16 +19,16 @@ protected void execute() {
         Robot.liftArm.setGrabber(true);
 
         if(Robot.liftArm.getArmAngle()> Robot.position.armClimb + 100) {
-            Robot.liftArm.setLiftArm(0.90); }
+            Robot.liftArm.setLiftArm(RobotMap.armSpeed); }
         else if(Robot.liftArm.getArmAngle()< Robot.position.armClimb - 100) {
-            Robot.liftArm.setLiftArm(-0.90);
+            Robot.liftArm.setLiftArm(-RobotMap.armSpeed);
         } else {
             Robot.liftArm.stopLiftArm();
             Robot.liftArm.disableWristBrake();
-            if(Robot.liftArm.getWristAngle() > Robot.position.wristClimb + 50) {
+            if(Robot.liftArm.getWristAngle() > Robot.position.wristClimb + 15) {
                 Robot.liftArm.setWrist(0.50);
             }
-            else if(Robot.liftArm.getWristAngle() < Robot.position.wristClimb - 50) {
+            else if(Robot.liftArm.getWristAngle() < Robot.position.wristClimb - 15) {
                 Robot.liftArm.setWrist(-0.50);
             }
             else {
diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/GrabCube.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/GrabCube.java
index 93144db..573491b 100644
--- a/src/main/java/org/usfirst/frc/team1701/robot/commands/GrabCube.java
+++ b/src/main/java/org/usfirst/frc/team1701/robot/commands/GrabCube.java
@@ -9,6 +9,7 @@
 
 import edu.wpi.first.wpilibj.command.Command;
 import org.usfirst.frc.team1701.robot.Robot;
+import org.usfirst.frc.team1701.robot.RobotMap;
 
 public class GrabCube extends Command{
 
@@ -19,11 +20,14 @@ protected void initialize() {
 
     }
     protected void execute() {
+        //RobotMap.cancelAll = true;
         Robot.liftArm.setGrabber(true);
     }
     protected boolean isFinished() {
         return true;
     }
-    protected void end() {}
+    protected void end() {
+
+    }
     protected void interrupted() {}
 }
diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/GrabPosition.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/GrabPosition.java
index 14a3cd3..69d6ada 100644
--- a/src/main/java/org/usfirst/frc/team1701/robot/commands/GrabPosition.java
+++ b/src/main/java/org/usfirst/frc/team1701/robot/commands/GrabPosition.java
@@ -8,6 +8,7 @@
 package org.usfirst.frc.team1701.robot.commands;
 import edu.wpi.first.wpilibj.command.CommandGroup;
 
+
 public class GrabPosition extends CommandGroup {
 
     public GrabPosition() {
diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/GrabPositionArm.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/GrabPositionArm.java
index 0e654c6..8dde2a0 100644
--- a/src/main/java/org/usfirst/frc/team1701/robot/commands/GrabPositionArm.java
+++ b/src/main/java/org/usfirst/frc/team1701/robot/commands/GrabPositionArm.java
@@ -10,6 +10,7 @@
 
 import edu.wpi.first.wpilibj.command.Command;
 import org.usfirst.frc.team1701.robot.Robot;
+import org.usfirst.frc.team1701.robot.RobotMap;
 
 public class GrabPositionArm extends Command {
     public GrabPositionArm() {
@@ -18,27 +19,27 @@ public GrabPositionArm() {
 
     private boolean isFinished;
 
-
     protected void initialize() {}
     protected void execute() {
+
         Robot.liftArm.disableWristBrake();
         Robot.liftArm.winchHighGear();
         isFinished = false;
         Robot.liftArm.setGrabber(true);
 
-        if(Robot.liftArm.getArmAngle()> Robot.position.armGrab + 100) {
-            Robot.liftArm.setLiftArm(0.90); }
-        else if(Robot.liftArm.getArmAngle()< Robot.position.armGrab - 100) {
-            Robot.liftArm.setLiftArm(-0.90);
+        if(Robot.liftArm.getArmAngle()> Robot.position.armGrab + 8) {
+            Robot.liftArm.setLiftArm(RobotMap.armSpeed); }
+        else if(Robot.liftArm.getArmAngle()< Robot.position.armGrab - 8) {
+            Robot.liftArm.setLiftArm(-RobotMap.armSpeed);
         }
         else {
             Robot.liftArm.stopLiftArm();
             Robot.liftArm.disableWristBrake();
-            if(Robot.liftArm.getWristAngle() > Robot.position.wristGrab + 50)
+            if(Robot.liftArm.getWristAngle() > Robot.position.wristGrab + 15)
             {
                 Robot.liftArm.setWrist(0.50);
             }
-            else if(Robot.liftArm.getWristAngle() < Robot.position.wristGrab - 50)
+            else if(Robot.liftArm.getWristAngle() < Robot.position.wristGrab - 15)
             {
                 Robot.liftArm.setWrist(-0.50);
             }
diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/ReleaseAndPunch.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/ReleaseAndPunch.java
index aa5ccdc..0dcd130 100644
--- a/src/main/java/org/usfirst/frc/team1701/robot/commands/ReleaseAndPunch.java
+++ b/src/main/java/org/usfirst/frc/team1701/robot/commands/ReleaseAndPunch.java
@@ -10,6 +10,7 @@
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.command.Command;
 import org.usfirst.frc.team1701.robot.Robot;
+import org.usfirst.frc.team1701.robot.RobotMap;
 
 public class ReleaseAndPunch extends Command {
 
@@ -20,14 +21,18 @@ protected void initialize() {
 
     }
     protected void execute() {
+        //RobotMap.cancelAll = true;
         Robot.liftArm.setGrabber(false);
         Robot.liftArm.extendPuncher();
         Timer.delay(0.5);
         Robot.liftArm.retractPuncher();
+
     }
     protected boolean isFinished() {
         return true;
     }
-    protected void end() {}
+    protected void end() {
+
+    }
     protected void interrupted() {}
 }
diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/ReleaseCube.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/ReleaseCube.java
index 508350a..0923197 100644
--- a/src/main/java/org/usfirst/frc/team1701/robot/commands/ReleaseCube.java
+++ b/src/main/java/org/usfirst/frc/team1701/robot/commands/ReleaseCube.java
@@ -9,6 +9,7 @@
 
 import edu.wpi.first.wpilibj.command.Command;
 import org.usfirst.frc.team1701.robot.Robot;
+import org.usfirst.frc.team1701.robot.RobotMap;
 
 public class ReleaseCube extends Command{
 
@@ -20,10 +21,12 @@ protected void initialize() {
     protected void execute() {
         Robot.position.isReleasePressed = true;
         Robot.liftArm.setGrabber(false);
+
     }
     protected boolean isFinished() {
         return true;
     }
-    protected void end() {}
+    protected void end() {
+    }
     protected void interrupted() {}
 }
diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/ScalePositionArm.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/ScalePositionArm.java
index 11a7d94..b2187c8 100644
--- a/src/main/java/org/usfirst/frc/team1701/robot/commands/ScalePositionArm.java
+++ b/src/main/java/org/usfirst/frc/team1701/robot/commands/ScalePositionArm.java
@@ -10,6 +10,7 @@
 
 import edu.wpi.first.wpilibj.command.Command;
 import org.usfirst.frc.team1701.robot.Robot;
+import org.usfirst.frc.team1701.robot.RobotMap;
 
 public class ScalePositionArm extends Command {
     public ScalePositionArm() {
@@ -17,6 +18,9 @@ public ScalePositionArm() {
     }
 
     private boolean isFinished;
+
+
+
     private void stowWrist() {
 
         Robot.liftArm.setGrabber(true);
@@ -30,9 +34,9 @@ private void stowWrist() {
         }
     }
     private void grabWrist() {
-        if(Robot.liftArm.getWristAngle() > Robot.position.wristScale + 50) {
+        if(Robot.liftArm.getWristAngle() > Robot.position.wristScale + 15) {
             Robot.liftArm.setWrist(0.50);
-        } else if(Robot.liftArm.getWristAngle() < Robot.position.wristScale - 50) {
+        } else if(Robot.liftArm.getWristAngle() < Robot.position.wristScale - 15) {
             Robot.liftArm.setWrist(-0.50);
         } else {
             Robot.liftArm.stopWrist();
@@ -47,10 +51,10 @@ protected void execute() {
         Robot.liftArm.setGrabber(true);
 
         if(Robot.liftArm.getArmAngle()> Robot.position.armScale + 100) {
-            Robot.liftArm.setLiftArm(0.80);
+            Robot.liftArm.setLiftArm(RobotMap.armSpeed);
             stowWrist();
         } else if(Robot.liftArm.getArmAngle()< Robot.position.armScale - 100) {
-            Robot.liftArm.setLiftArm(-0.80);
+            Robot.liftArm.setLiftArm(-RobotMap.armSpeed);
             stowWrist();
         } else {
             Robot.liftArm.stopLiftArm();
diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/StowPosition.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/StowPosition.java
index c586058..9d79eec 100644
--- a/src/main/java/org/usfirst/frc/team1701/robot/commands/StowPosition.java
+++ b/src/main/java/org/usfirst/frc/team1701/robot/commands/StowPosition.java
@@ -15,15 +15,20 @@ public StowPosition() {
         requires(Robot.liftArm);
     }
     private boolean isFinished;
+
+    public void finish() {
+        isFinished = true;
+    }
+
     protected void initialize() {}
     protected void execute() {
         Robot.liftArm.disableWristBrake();
         isFinished = false;
         Robot.liftArm.setGrabber(true);
 
-        if(Robot.liftArm.getWristAngle() > Robot.position.wristStow + 50) {
+        if(Robot.liftArm.getWristAngle() > Robot.position.wristStow + 15) {
             Robot.liftArm.setWrist(0.75);
-        } else if(Robot.liftArm.getWristAngle() < Robot.position.wristStow - 50) {
+        } else if(Robot.liftArm.getWristAngle() < Robot.position.wristStow - 15) {
             Robot.liftArm.setWrist(-0.75);
         } else {
             isFinished = true;
diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/SwitchPositionArm.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/SwitchPositionArm.java
index 4be8e97..08c5483 100644
--- a/src/main/java/org/usfirst/frc/team1701/robot/commands/SwitchPositionArm.java
+++ b/src/main/java/org/usfirst/frc/team1701/robot/commands/SwitchPositionArm.java
@@ -9,6 +9,7 @@
 
 import edu.wpi.first.wpilibj.command.Command;
 import org.usfirst.frc.team1701.robot.Robot;
+import org.usfirst.frc.team1701.robot.RobotMap;
 
 public class SwitchPositionArm extends Command {
     public SwitchPositionArm() {
@@ -17,22 +18,11 @@ public SwitchPositionArm() {
 
     private boolean isFinished;
 
-    private void stowWrist() {
 
-        Robot.liftArm.setGrabber(true);
-
-        if(Robot.liftArm.getWristAngle() > Robot.position.wristStow + 50) {
-            Robot.liftArm.setWrist(0.50);
-        } else if(Robot.liftArm.getWristAngle() < Robot.position.wristStow - 50) {
-            Robot.liftArm.setWrist(-0.50);
-        } else {
-            Robot.liftArm.stopWrist();
-        }
-    }
     private void grabWrist() {
-        if(Robot.liftArm.getWristAngle() > Robot.position.wristSwitch + 50) {
+        if(Robot.liftArm.getWristAngle() > Robot.position.wristSwitch + 15) {
             Robot.liftArm.setWrist(0.50);
-        } else if(Robot.liftArm.getWristAngle() < Robot.position.wristSwitch - 50) {
+        } else if(Robot.liftArm.getWristAngle() < Robot.position.wristSwitch - 15) {
             Robot.liftArm.setWrist(-0.50);
         } else {
             Robot.liftArm.stopWrist();
@@ -47,11 +37,9 @@ protected void execute() {
         Robot.liftArm.setGrabber(true);
 
         if(Robot.liftArm.getArmAngle()> Robot.position.armSwitch + 40) {
-            Robot.liftArm.setLiftArm(0.90);
-            stowWrist();
+            Robot.liftArm.setLiftArm(RobotMap.armSpeed);
         } else if(Robot.liftArm.getArmAngle()< Robot.position.armSwitch - 40) {
-            Robot.liftArm.setLiftArm(-0.90);
-            stowWrist();
+            Robot.liftArm.setLiftArm(-RobotMap.armSpeed);
         } else {
             Robot.liftArm.stopLiftArm();
             grabWrist();
diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/SwitchShootPositionArm.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/SwitchShootPositionArm.java
index 95e1c73..2533a4e 100644
--- a/src/main/java/org/usfirst/frc/team1701/robot/commands/SwitchShootPositionArm.java
+++ b/src/main/java/org/usfirst/frc/team1701/robot/commands/SwitchShootPositionArm.java
@@ -9,6 +9,7 @@
 
 import edu.wpi.first.wpilibj.command.Command;
 import org.usfirst.frc.team1701.robot.Robot;
+import org.usfirst.frc.team1701.robot.RobotMap;
 
 public class SwitchShootPositionArm extends Command {
 
@@ -18,22 +19,11 @@ public SwitchShootPositionArm() {
 
     private boolean isFinished;
 
-    private void stowWrist() {
 
-        Robot.liftArm.setGrabber(true);
-
-        if(Robot.liftArm.getWristAngle() > Robot.position.wristStow + 50) {
-            Robot.liftArm.setWrist(0.50);
-        } else if(Robot.liftArm.getWristAngle() < Robot.position.wristStow - 50) {
-            Robot.liftArm.setWrist(-0.50);
-        } else {
-            Robot.liftArm.stopWrist();
-        }
-    }
     private void grabWrist() {
-        if(Robot.liftArm.getWristAngle() > Robot.position.wristSwitchShoot + 50) {
+        if(Robot.liftArm.getWristAngle() > Robot.position.wristSwitchShoot + 15) {
             Robot.liftArm.setWrist(0.50);
-        } else if(Robot.liftArm.getWristAngle() < Robot.position.wristSwitchShoot - 50) {
+        } else if(Robot.liftArm.getWristAngle() < Robot.position.wristSwitchShoot - 15) {
             Robot.liftArm.setWrist(-0.50);
         } else {
             Robot.liftArm.stopWrist();
@@ -42,17 +32,16 @@ private void grabWrist() {
     }
     protected void initialize() {}
     protected void execute() {
+
         Robot.liftArm.winchHighGear();
         Robot.liftArm.disableWristBrake();
         isFinished = false;
         Robot.liftArm.setGrabber(true);
 
         if(Robot.liftArm.getArmAngle()> Robot.position.armSwitchShoot + 40) {
-            Robot.liftArm.setLiftArm(0.70);
-            stowWrist();
+            Robot.liftArm.setLiftArm(RobotMap.armSpeed);
         } else if(Robot.liftArm.getArmAngle()< Robot.position.armSwitchShoot - 40) {
-            Robot.liftArm.setLiftArm(-0.70);
-            stowWrist();
+            Robot.liftArm.setLiftArm(-RobotMap.armSpeed);
         } else {
             Robot.liftArm.stopLiftArm();
             grabWrist();
diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/TeleopDrive.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/TeleopDrive.java
index 521c155..162acd7 100644
--- a/src/main/java/org/usfirst/frc/team1701/robot/commands/TeleopDrive.java
+++ b/src/main/java/org/usfirst/frc/team1701/robot/commands/TeleopDrive.java
@@ -19,26 +19,23 @@ public TeleopDrive() {
     requires(Robot.driveTrain);
   }
   protected void initialize() {
+    Robot.driveTrain.stopPID();
+    //Robot.driveTrain.setLowGear();
+    Robot.driveTrain.setCoastMode();
+
   }
   protected void execute() {
 
-    SmartDashboard.putBoolean("Reversed", Robot.driveTrain.getReverse());
-    SmartDashboard.putNumber("Arm",Robot.liftArm.getArmAngle());
-    SmartDashboard.putNumber("Wrist", Robot.liftArm.getWristAngle());
-    SmartDashboard.putBoolean("Arm Down", RobotMap.armSensor.get());
-    SmartDashboard.putNumber("Drive Train", Robot.driveTrain.getRightDistance());
-
-
     if(Robot.liftArm.getCubeSensor() && !Robot.position.isReleasePressed) {
       Robot.liftArm.setGrabber(true);
     }
 
-
+    Robot.shuffleboard.updateDashboard();
     double deadConst = .10;
-    double fBInput = checkDeadZone(OI.drive_FB.getY(), deadConst);
-    double tInput =  -1 * checkDeadZone(OI.drive_T.getX(), deadConst);
+    double fBInput = 2 * checkDeadZone(OI.drive_FB.getY(), deadConst);
+    double tInput =  -1 * 2 * checkDeadZone(OI.drive_T.getX(), deadConst);
     Robot.driveTrain.teleopControl(fBInput, tInput);
-    Robot.driveTrain.autoGear(OI.drive_FB.getY(),deadConst,Robot.driveTrain.getRightDistance(),7);
+    Robot.driveTrain.autoGear(OI.drive_FB.getY(),deadConst,Robot.driveTrain.getRightDistance(),3);
 
   }
   protected boolean isFinished() {
diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/WinchDown.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/WinchDown.java
index 72d7fef..cb661df 100644
--- a/src/main/java/org/usfirst/frc/team1701/robot/commands/WinchDown.java
+++ b/src/main/java/org/usfirst/frc/team1701/robot/commands/WinchDown.java
@@ -13,7 +13,7 @@
 public class WinchDown extends Command {
     protected void execute() {
         Robot.liftArm.winchLowGear();
-        Robot.liftArm.setLiftArm(-1);
+        Robot.liftArm.setLiftArm(1);
     }
     protected boolean isFinished() {
         return true;
diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/WinchUp.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/WinchUp.java
index 0e86550..a617396 100644
--- a/src/main/java/org/usfirst/frc/team1701/robot/commands/WinchUp.java
+++ b/src/main/java/org/usfirst/frc/team1701/robot/commands/WinchUp.java
@@ -13,7 +13,7 @@
 public class WinchUp extends Command {
     protected void execute() {
         Robot.liftArm.winchLowGear();
-        Robot.liftArm.setLiftArm(1);
+        Robot.liftArm.setLiftArm(-1);
     }
     protected boolean isFinished() {
         return true;
diff --git a/src/main/java/org/usfirst/frc/team1701/robot/subsystems/DriveTrain.java b/src/main/java/org/usfirst/frc/team1701/robot/subsystems/DriveTrain.java
index 219909a..3cd6fcb 100644
--- a/src/main/java/org/usfirst/frc/team1701/robot/subsystems/DriveTrain.java
+++ b/src/main/java/org/usfirst/frc/team1701/robot/subsystems/DriveTrain.java
@@ -6,14 +6,18 @@
  * @license BSD-3-Clause
  */
 package org.usfirst.frc.team1701.robot.subsystems;
+import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.PIDController;
+import edu.wpi.first.wpilibj.PIDOutput;
+import edu.wpi.first.wpilibj.command.PIDCommand;
+import edu.wpi.first.wpilibj.command.PIDSubsystem;
 import edu.wpi.first.wpilibj.command.Subsystem;
-import org.usfirst.frc.team1701.robot.Robot;
 import org.usfirst.frc.team1701.robot.RobotMap;
 import org.usfirst.frc.team1701.robot.commands.TeleopDrive;
 import com.kauailabs.navx.frc.AHRS;
-public class DriveTrain extends Subsystem {
+public class DriveTrain extends PIDSubsystem {
   /*
    * Set of motors.
    */
@@ -31,13 +35,14 @@ public class DriveTrain extends Subsystem {
    */
   private final DoubleSolenoid driveShift = RobotMap.driveShift;
   /*
-   * NavX.
+   * NavX. 
    */
   private final AHRS navx = RobotMap._navx;
   /*
    * Special math stuffs.
    */
   private final int encPidIdx = RobotMap.encPidIdx;
+  private final int rawToRotation = 22000;
 
 
   /*
@@ -46,13 +51,41 @@ public class DriveTrain extends Subsystem {
   private boolean reversed = false;
   private boolean deadStick = false;
   public boolean autoGear = false;
+  public double driveSpeed = 0;
+
+  public DriveTrain() {
+    super(1.1,0,0);
+    this.setInputRange(-360,360);
+    this.setOutputRange(-0.9,0.9);
+    this.setAbsoluteTolerance(.5);
+    this.getPIDController().setContinuous(true);
+  }
+
+  /**
+   * Sets angle for robot to turn to using PIDSubsystem
+   * @param startAngle angle to turn to
+   */
+  public void setAngle(double startAngle) {
+    this.setSetpoint(startAngle);
+  }
+
+  public void startPID() {
+    this.getPIDController().enable();
+  }
+
+  public void stopPID() {
+    this.getPIDController().disable();
+  }
+
+
+
 
   /**
    * Return left distance.
    * @return double of left side distance.
    */
   public double getLeftDistance() {
-    return -leftEncTalon.getSelectedSensorPosition(encPidIdx) / 22000;
+    return -leftEncTalon.getSelectedSensorPosition(encPidIdx) / rawToRotation;
   }
   /**
    * Reset left side encoder.
@@ -75,7 +108,7 @@ public void stopMotors()
    * @return double of right side distance.
    */
   public double getRightDistance() {
-    return rightEncTalon.getSelectedSensorPosition(encPidIdx) / 22000;
+    return rightEncTalon.getSelectedSensorPosition(encPidIdx) / rawToRotation;
   }
   /**
    * Reset right side encoder.
@@ -114,7 +147,6 @@ public double getNavxAngle()
   {
     return -navx.getAngle();
   }
-
   /**
    * Checks current reverse boolean
    * @return State of reversed
@@ -122,12 +154,6 @@ public double getNavxAngle()
   public boolean getReverse() {
     return reversed;
   }
-  /**
-   * Set status of reverse mode.
-   */
-  public void toggleReverse() {
-    this.reversed = !this.reversed;
-  }
   /**
    * Initialize teleoperated control.
    * @param forwardsBackwardsAxis See DifferentialDrive.arcadeDrive.
@@ -168,17 +194,29 @@ public void setLowGear() {
     driveShift.set(DoubleSolenoid.Value.kReverse);
   }
 
+  /**
+   * Sets the drive train's thrust to move in the opposite direction
+   * @param reversedValue Enables/Disables reversed status
+   */
   public void setReverse(boolean reversedValue) {
       this.reversed = reversedValue;
   }
 
   /**
-   * Set state of autoGear
-   * @param value Set Boolean
+   * Enables/Disables auto gear. This is helpful for a manual switch where you can turn auto gear on and off
+   * @param value Sets state of auto gear
    */
   public void setAutoGear(boolean value) {
     this.autoGear = value;
   }
+
+  /**
+   * Automatically shifts the gear after a set distance from when the drive stick last went to dead stick
+   * @param tInput Forward/Backwards Speed from Joystick
+   * @param deadConst The value that sets where and when dead stick should enable. BY DEFAULT: 0.1 or -10 % to 10%
+   * @param encoderValue The raw input that will deliver the current distance
+   * @param distanceTrigger The value that determines the distance that high gear is set
+   */
   public void autoGear(double tInput, double deadConst, double encoderValue, double distanceTrigger) {
     if(autoGear) {
 
@@ -201,15 +239,44 @@ public void autoGear(double tInput, double deadConst, double encoderValue, doubl
         }
       }
 
-      if(encoderValue > distanceTrigger) {
+      if(Math.abs(encoderValue) > distanceTrigger) {
         setHighGear();
       }
 
-
     }
   }
 
+  /**
+   * Checks current deadStick boolean that is determine by autoGear()
+   * @return Checks weather the thrust joystick is moving or not
+   */
   public boolean getDeadStick() {
     return this.deadStick;
   }
+
+
+
+  @Override
+  protected double returnPIDInput() {
+    return -navx.getAngle();
+  }
+
+  @Override
+  protected void usePIDOutput(double output) {
+    teleopControl(driveSpeed,output);
+  }
+
+  public void setBrakeMode() {
+    left_1.setNeutralMode(NeutralMode.Brake);
+    left_2.setNeutralMode(NeutralMode.Brake);
+    right_1.setNeutralMode(NeutralMode.Brake);
+    right_2.setNeutralMode(NeutralMode.Brake);
+  }
+
+  public void setCoastMode() {
+    left_1.setNeutralMode(NeutralMode.Coast);
+    left_2.setNeutralMode(NeutralMode.Coast);
+    right_1.setNeutralMode(NeutralMode.Coast);
+    right_2.setNeutralMode(NeutralMode.Coast);
+  }
 }
diff --git a/src/main/java/org/usfirst/frc/team1701/robot/subsystems/LiftArm.java b/src/main/java/org/usfirst/frc/team1701/robot/subsystems/LiftArm.java
index e713fef..29c7571 100644
--- a/src/main/java/org/usfirst/frc/team1701/robot/subsystems/LiftArm.java
+++ b/src/main/java/org/usfirst/frc/team1701/robot/subsystems/LiftArm.java
@@ -83,7 +83,8 @@ public double getArmAngle() {
    * @return wrist angle as double.
    */
   public double getWristAngle() {
-    return wristEncoder.getSelectedSensorPosition(encPidIdx);
+    //return wristEncoder.getSelectedSensorPosition(encPidIdx);
+    return RobotMap.wristEncoder.getValue();
   }
   /**
    * Set the clamp position.
@@ -103,8 +104,8 @@ public void setGrabber(boolean State) {
    */
   public void setLiftArm(double input) {
     disableWinchBrake();
-    winch1.set(input);
-    winch2.set(input);
+    winch1.set(checkArmZone(input));
+    winch2.set(checkArmZone(input));
 
   }
   /**
@@ -153,11 +154,7 @@ public boolean getCubeSensor()
     return RobotMap.cubeSensor.get();
   }
   public double checkArmZone(double speed) {
-    if(Robot.position.armSafetyMin >= Robot.liftArm.getArmAngle()) {
-      return 0;
-    }
-
-    if(Robot.position.armSafetyMax <= Robot.liftArm.getArmAngle()) {
+    if(RobotMap.armSensor.get() && speed > 0) {
       return 0;
     }
     return speed;
diff --git a/src/main/java/org/usfirst/frc/team1701/robot/subsystems/Position.java b/src/main/java/org/usfirst/frc/team1701/robot/subsystems/Position.java
index 8eafb41..6253b64 100644
--- a/src/main/java/org/usfirst/frc/team1701/robot/subsystems/Position.java
+++ b/src/main/java/org/usfirst/frc/team1701/robot/subsystems/Position.java
@@ -4,51 +4,31 @@
 
 public class Position extends Subsystem {
 
-    /*
-     * Minimum and maximum traveling distance for wrist and arm
-     */
-    public double armSafetyMin = 98;
-    public double armSafetyMax = 2202;
-    public double wristSafetyMin = 0;
-    public double wristSafetyMax = 0;
-
     /*
      * Wrist values for positions
      */
-    public double wristStow = -201;
-    public double wristGrab = 1119;
-    public double wristScale = 911;
-    public double wristClimb = 1289;
-    public double wristSwitch = -1512;
-    public double wristSwitchShoot = 461;
+    public double wristStow = -44;
+    public double wristGrab = 1980;
+    public double wristScale = 1958;
+    public double wristClimb = 2007;
+    public double wristSwitch = 1723;
+    public double wristSwitchShoot = 1911;
+    public double startPosition = 1868;
 
     /*
      * Arm value for positions
      */
-    public double armGrab = 355;
-    public double armScale = 2400;
-    public double armClimb = 2396;
-    public double armSwitch = 1373;
-    public double armSwitchShoot = 563;
+    public double armGrab = 48;
+    public double armScale = 1994;
+    public double armClimb = 1645;
+    public double armSwitch = 1185;
+    public double armSwitchShoot = 224;
 
     /*
      * Values for autonomous
      */
-    public double wallToCubeRight = 0.1;
-    public double wallToCubeLeft = 0.1;
-    public double centerToSide = 5.00;
-    public double turnToSwitch = 4.2;
-    public double turnToSwitchRight = 4.4;
-    public double wallToScale = 22.75;
-    public double wallToPlatformZone = 15.69;
-    public double scaleReverse = 0.31;
+
     public double autonomousSpeed = 0.90;
-    public double crossPlatformZone = 13.93;
-    public double platformToScale = 2.1;
-    public double leftAngle = 81;
-    public double rightAngle = 81;
-    public double slightLeftAngle = 45;
-    public double slightRightAngle = 30;
     public double wallToStraightSwitch = 7;
 
     public boolean isReleasePressed = false;
diff --git a/src/main/java/org/usfirst/frc/team1701/robot/subsystems/Shuffleboard.java b/src/main/java/org/usfirst/frc/team1701/robot/subsystems/Shuffleboard.java
new file mode 100644
index 0000000..9feb692
--- /dev/null
+++ b/src/main/java/org/usfirst/frc/team1701/robot/subsystems/Shuffleboard.java
@@ -0,0 +1,76 @@
+/**
+ * Shuffleboard.java
+ *
+ * @author Noah Husby
+ * @since 3/8/18
+ * @license BSD-3-Clause
+ */
+package org.usfirst.frc.team1701.robot.subsystems;
+
+import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import org.usfirst.frc.team1701.robot.Robot;
+import org.usfirst.frc.team1701.robot.RobotMap;
+
+public class Shuffleboard {
+
+    public static SendableChooser<Number> autonomousLocation;
+    public static SendableChooser<Number> action;
+
+    /**
+     * Creates our shuffleboard objects
+     */
+    public static void init() {
+        /*
+         * Initialize Autonomous Location Chooser;
+         * Allows operator/technician to select robot start position before match
+         */
+        autonomousLocation = new SendableChooser<>();
+        autonomousLocation.addObject("Left",1);
+        autonomousLocation.addObject("Left-Switch",4);
+        autonomousLocation.addDefault("Middle",2);
+        autonomousLocation.addObject("Right-Switch",5);
+        autonomousLocation.addObject("Right",3);
+        autonomousLocation.addObject("Magic",6);
+        SmartDashboard.putData("Autonomous Location", autonomousLocation);
+        /*
+         * Initialize Autonomous Chooser
+         * Allows operator/technician to select type of autonomous to be run
+         */
+        action = new SendableChooser<>();
+        action.addDefault("Defualt Autonomous", 1);
+        action.addObject("Forward Autonomous", 2);
+        SmartDashboard.putData("Autonomous Chooser",action);
+
+        /*
+         * Shuffleboard elements to be updated during teleop period
+         */
+        SmartDashboard.putBoolean("Reversed", false);
+        SmartDashboard.putString("Current Gear","");
+        SmartDashboard.putNumber("Arm",0);
+        SmartDashboard.putBoolean("Arm Trigger", false);
+        SmartDashboard.putNumber("Drive Train", 0);
+        SmartDashboard.putString("GameCode", "");
+        SmartDashboard.putNumber("NewEnc",0);
+
+    }
+
+    /**
+     * Method called during teleop to update shuffleboard elements live
+     */
+    public static void updateDashboard() {
+        SmartDashboard.putBoolean("Reversed", Robot.driveTrain.getReverse());
+        SmartDashboard.putNumber("Arm",Robot.liftArm.getArmAngle());
+        SmartDashboard.putNumber("Wrist", Robot.liftArm.getWristAngle());
+        SmartDashboard.putBoolean("Arm Trigger", RobotMap.armSensor.get());
+        SmartDashboard.putNumber("Drive Train", Robot.driveTrain.getRightDistance());
+        SmartDashboard.putNumber("NewEnc",RobotMap.wristEncoder.getValue());
+
+    }
+
+
+
+
+
+}