Auto Fixes

src/org/usfirst/frc4388/robot/Robot.java

@@ -126,13 +126,11 @@ public class Robot extends IterativeRobot
 
 		RRautonTaskChooser.addObject("Left to Right Switch", new LeftStartRightScore());
 		
-		RRautonTaskChooser.addObject("Center to Left Switch", new CenterLeft());
 		RRautonTaskChooser.addObject("Center to Right Switch", new CenterRight());
 		
 
 		RRautonTaskChooser.addObject("Right to Right Switch", new RightSwitchAuton());
-		RRautonTaskChooser.addObject("Right to Left Switch", new RightStartLeftScore());
+		RRautonTaskChooser.addObject("Right to Right Scale", new ScaleFrom3());
-		RRautonTaskChooser.addObject("Scale3", new ScaleFrom3());
 
 		
 		SmartDashboard.putData("RRAuton Task", RRautonTaskChooser);
@@ -147,15 +145,11 @@ public class Robot extends IterativeRobot
 		
 
 		RLautonTaskChooser.addObject("Left to Right Switch", new LeftStartRightScore());
-		
+		RLautonTaskChooser.addObject("Left to Left Scale", new ScaleFrom1());
 
-		RLautonTaskChooser.addObject("Center to Left Switch", new CenterLeft());
 		RLautonTaskChooser.addObject("Center to Right Switch", new CenterRight());
 		
-
 		RLautonTaskChooser.addObject("Right to Right Switch", new RightSwitchAuton());
-		RLautonTaskChooser.addObject("Right to Left Switch", new RightStartLeftScore());
-
 		
 		SmartDashboard.putData("RLAuton Task", RLautonTaskChooser);
 		
@@ -169,36 +163,29 @@ public class Robot extends IterativeRobot
 		LLautonTaskChooser.addDefault("Choose LL Program", new CrossTheBaseLine());
 		
 
-		LLautonTaskChooser.addObject("Left to Right Switch", new LeftStartRightScore());
+		LLautonTaskChooser.addObject("Left to Left Scale", new ScaleFrom1());
+		LLautonTaskChooser.addObject("Left to Left Switch", new LeftSwitchAuton());
 		
 
 		LLautonTaskChooser.addObject("Center to Left Switch", new CenterLeft());
-		LLautonTaskChooser.addObject("Center to Right Switch", new CenterRight());
 		
 
-		LLautonTaskChooser.addObject("Right to Right Switch", new RightSwitchAuton());
 		LLautonTaskChooser.addObject("Right to Left Switch", new RightStartLeftScore());
 
 		
 		SmartDashboard.putData("LLAuton Task", LLautonTaskChooser);
 		
 		
-		
-		
-		
 		LRautonTaskChooser = new SendableChooser<Command>();
 		
 		LRautonTaskChooser.addDefault("Choose LR Program", new CrossTheBaseLine());
 	
-		LRautonTaskChooser.addObject("Left to Right Switch", new LeftStartRightScore());
+		LRautonTaskChooser.addObject("Left to Left Switch", new LeftSwitchAuton());
-		
 
 		LRautonTaskChooser.addObject("Center to Left Switch", new CenterLeft());
-		LRautonTaskChooser.addObject("Center to Right Switch", new CenterRight());
 		
-		LRautonTaskChooser.addObject("Right to Right Switch", new RightSwitchAuton());
 		LRautonTaskChooser.addObject("Right to Left Switch", new RightStartLeftScore());
-
+		LRautonTaskChooser.addObject("Right To Right Scale", new ScaleFrom3());
 		
 		SmartDashboard.putData("LRAuton Task", LRautonTaskChooser);
 

src/org/usfirst/frc4388/robot/commands/AutonomousCommand.java

@@ -1,59 +0,0 @@
-// RobotBuilder Version: 2.0
-//
-// This file was generated by RobotBuilder. It contains sections of
-// code that are automatically generated and assigned by robotbuilder.
-// These sections will be updated in the future when you export to
-// Java from RobotBuilder. Do not put any code or make any change in
-// the blocks indicating autogenerated code or it will be lost on an
-// update. Deleting the comments indicating the section will prevent
-// it from being updated in the future.
-
-
-package org.usfirst.frc4388.robot.commands;
-import org.usfirst.frc4388.robot.Robot;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-/**
- *
- */
-public class AutonomousCommand extends Command {
-
-    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_DECLARATIONS
- 
-    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_DECLARATIONS
-
-    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTOR
-    public AutonomousCommand() {
-
-    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTOR
-        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_SETTING
-
-        // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_SETTING
-        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES
-
-        // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES
-    }
-
-    // Called just before this Command runs the first time
-    protected void initialize() {
-    }
-
-    // Called repeatedly when this Command is scheduled to run
-    protected void execute() {
-    }
-
-    // Make this return true when this Command no longer needs to run execute()
-    protected boolean isFinished() {
-        return false;
-    }
-
-    // Called once after isFinished returns true
-    protected void end() {
-    }
-
-    // Called when another command which requires one or more of the same
-    // subsystems is scheduled to run
-    protected void interrupted() {
-    }
-}

src/org/usfirst/frc4388/robot/commands/auton/CenterLeft.java

@@ -1,17 +1,17 @@
 package org.usfirst.frc4388.robot.commands.auton;
 
-import org.usfirst.frc4388.robot.commands.DriveAbsoluteTurnMP;
+
 import org.usfirst.frc4388.robot.commands.DriveGyroReset;
 import org.usfirst.frc4388.robot.commands.DriveSpeedShift;
 import org.usfirst.frc4388.robot.commands.DriveStraightBasic;
-import org.usfirst.frc4388.robot.commands.DriveStraightMP;
+
 import org.usfirst.frc4388.robot.commands.DriveTurnBasic;
 import org.usfirst.frc4388.robot.commands.ElevatorBasic;
-import org.usfirst.frc4388.robot.commands.ElevatorSetSpeed;
+
 import org.usfirst.frc4388.robot.commands.IntakePosition;
 import org.usfirst.frc4388.robot.commands.IntakeSetSpeed;
 import org.usfirst.frc4388.robot.subsystems.Carriage;
-import org.usfirst.frc4388.robot.subsystems.Drive;
+
 import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
 
 import edu.wpi.first.wpilibj.command.CommandGroup;
@@ -25,8 +25,9 @@ public class CenterLeft extends CommandGroup {
     public CenterLeft() 
     {
     	addSequential(new DriveGyroReset());
-    	
     	addSequential(new IntakePosition(true));
+    	addSequential(new DriveSpeedShift(true));
+    	
     	addSequential(new DriveStraightBasic(-15, 60, true, true, 0));
     	addSequential(new DriveTurnBasic(true, 130, 300, MPSoftwareTurnType.TANK));
     	addSequential(new DriveStraightBasic(53, 60, true, true, 0));
@@ -38,5 +39,8 @@ public class CenterLeft extends CommandGroup {
     	addSequential(new IntakePosition(false));
     	addSequential(new WaitCommand(.2));
     	addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));
+    	addSequential(new DriveStraightBasic(-20, 60, true, true, 0));
+      	addSequential(new ElevatorBasic(10));
+    	addSequential(new DriveSpeedShift(false));
     }
 }

src/org/usfirst/frc4388/robot/commands/auton/CenterRight.java

@@ -1,17 +1,17 @@
 package org.usfirst.frc4388.robot.commands.auton;
 
-import org.usfirst.frc4388.robot.commands.DriveAbsoluteTurnMP;
+
 import org.usfirst.frc4388.robot.commands.DriveGyroReset;
 import org.usfirst.frc4388.robot.commands.DriveSpeedShift;
 import org.usfirst.frc4388.robot.commands.DriveStraightBasic;
-import org.usfirst.frc4388.robot.commands.DriveStraightMP;
+
 import org.usfirst.frc4388.robot.commands.DriveTurnBasic;
 import org.usfirst.frc4388.robot.commands.ElevatorBasic;
-import org.usfirst.frc4388.robot.commands.ElevatorSetSpeed;
+
 import org.usfirst.frc4388.robot.commands.IntakePosition;
 import org.usfirst.frc4388.robot.commands.IntakeSetSpeed;
 import org.usfirst.frc4388.robot.subsystems.Carriage;
-import org.usfirst.frc4388.robot.subsystems.Drive;
+
 import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
 
 import edu.wpi.first.wpilibj.command.CommandGroup;
@@ -25,8 +25,10 @@ public class CenterRight extends CommandGroup {
     public CenterRight()
     {
     	addSequential(new DriveGyroReset());
-    	
     	addSequential(new IntakePosition(true));
+    	addSequential(new DriveSpeedShift(true));
+    	
+    	
     	addSequential(new DriveStraightBasic(-15, 60, true, true, 0));
     	addSequential(new DriveTurnBasic(true, -130, 300, MPSoftwareTurnType.TANK));
     	addSequential(new DriveStraightBasic(53, 60, true, true, 0));
@@ -38,5 +40,7 @@ public class CenterRight extends CommandGroup {
     	addSequential(new IntakePosition(false));
     	addSequential(new WaitCommand(.2));
     	addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));
+    	addSequential(new DriveStraightBasic(-20, 60, true, true, 0));
+    	addSequential(new DriveSpeedShift(false));
     }
 }

src/org/usfirst/frc4388/robot/commands/auton/CrossTheBaseLine.java

@@ -1,20 +1,13 @@
 package org.usfirst.frc4388.robot.commands.auton;
 
-import org.usfirst.frc4388.robot.commands.DriveAbsoluteTurnMP;
+
 import org.usfirst.frc4388.robot.commands.DriveGyroReset;
 import org.usfirst.frc4388.robot.commands.DriveSpeedShift;
 import org.usfirst.frc4388.robot.commands.DriveStraightBasic;
-import org.usfirst.frc4388.robot.commands.DriveStraightMP;
+
-import org.usfirst.frc4388.robot.commands.DriveTurnBasic;
-import org.usfirst.frc4388.robot.commands.ElevatorBasic;
 import org.usfirst.frc4388.robot.commands.IntakePosition;
-import org.usfirst.frc4388.robot.commands.IntakeSetSpeed;
-import org.usfirst.frc4388.robot.subsystems.Carriage;
-import org.usfirst.frc4388.robot.subsystems.Drive;
-import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
 
 import edu.wpi.first.wpilibj.command.CommandGroup;
-import edu.wpi.first.wpilibj.command.WaitCommand;
 
 /**
  *
@@ -25,9 +18,9 @@ public class CrossTheBaseLine extends CommandGroup {
     	
     	addSequential(new DriveGyroReset());
     	addSequential(new DriveSpeedShift(true));
-    	addSequential(new DriveSpeedShift(true));
     	addSequential(new IntakePosition(true));
     	addSequential(new DriveStraightBasic(-90, 60, true, true, 0));
+    	addSequential(new DriveSpeedShift(false));
 
        
     }

src/org/usfirst/frc4388/robot/commands/auton/LeftStartRightScore.java

@@ -1,16 +1,17 @@
 package org.usfirst.frc4388.robot.commands.auton;
 
-import org.usfirst.frc4388.robot.commands.DriveAbsoluteTurnMP;
+
 import org.usfirst.frc4388.robot.commands.DriveGyroReset;
 import org.usfirst.frc4388.robot.commands.DriveSpeedShift;
 import org.usfirst.frc4388.robot.commands.DriveStraightBasic;
-import org.usfirst.frc4388.robot.commands.DriveStraightMP;
+
 import org.usfirst.frc4388.robot.commands.DriveTurnBasic;
 import org.usfirst.frc4388.robot.commands.ElevatorBasic;
+
 import org.usfirst.frc4388.robot.commands.IntakePosition;
 import org.usfirst.frc4388.robot.commands.IntakeSetSpeed;
 import org.usfirst.frc4388.robot.subsystems.Carriage;
-import org.usfirst.frc4388.robot.subsystems.Drive;
+
 import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
 
 import edu.wpi.first.wpilibj.command.CommandGroup;
@@ -25,8 +26,9 @@ public class LeftStartRightScore extends CommandGroup {
     {
     	addSequential(new DriveGyroReset());
     	addSequential(new DriveSpeedShift(true));
-    	addSequential(new DriveSpeedShift(true));
     	addSequential(new IntakePosition(true));
+    	
+    	
     	addSequential(new DriveStraightBasic(215, 60, true, true, 0));
     	addSequential(new ElevatorBasic(5));
     	addSequential(new DriveTurnBasic(true, -87, 300, MPSoftwareTurnType.TANK));
@@ -41,6 +43,7 @@ public class LeftStartRightScore extends CommandGroup {
     	addSequential(new WaitCommand(.2));
     	addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));
     	addSequential(new DriveStraightBasic(-10, 60, true, true, 0));
+    	addSequential(new DriveSpeedShift(false));
     	//addSequential(new DriveStraightMP(60, 10, true, true, 0));  // 95 for 112" greenville
     }
 }

src/org/usfirst/frc4388/robot/commands/auton/LeftSwitchAuton.java

@@ -0,0 +1,47 @@
+package org.usfirst.frc4388.robot.commands.auton;
+
+
+import org.usfirst.frc4388.robot.commands.DriveGyroReset;
+import org.usfirst.frc4388.robot.commands.DriveSpeedShift;
+import org.usfirst.frc4388.robot.commands.DriveStraightBasic;
+
+import org.usfirst.frc4388.robot.commands.DriveTurnBasic;
+import org.usfirst.frc4388.robot.commands.ElevatorBasic;
+
+import org.usfirst.frc4388.robot.commands.IntakePosition;
+import org.usfirst.frc4388.robot.commands.IntakeSetSpeed;
+import org.usfirst.frc4388.robot.subsystems.Carriage;
+
+import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+
+/**
+ *
+ */
+public class LeftSwitchAuton extends CommandGroup {
+    
+    public LeftSwitchAuton() {
+    	addSequential(new DriveGyroReset());
+    	addSequential(new DriveSpeedShift(true));
+    	addSequential(new IntakePosition(true));
+    	
+    	
+    	addSequential(new DriveStraightBasic(-130, 60, true, true, 0));
+    	addSequential(new ElevatorBasic(30));
+    	addSequential(new DriveTurnBasic(true, -87, 300, MPSoftwareTurnType.TANK));
+    	addSequential(new ElevatorBasic(30));
+    	addSequential(new DriveStraightBasic(20, 60, true, true, 0));
+    	addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED));
+    	//addSequential(new WaitCommand(.1));
+    	addSequential(new IntakePosition(false));
+    	addSequential(new WaitCommand(.5));
+    	addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));
+    	addSequential(new DriveStraightBasic(-20, 60, true, true, 0));
+    	addSequential(new DriveSpeedShift(false));
+
+    	//addSequential(new DriveStraightMP(60, 10, true, true, 0));  // 95 for 112" greenville
+       
+    }
+}

src/org/usfirst/frc4388/robot/commands/auton/RightStartLeftScore.java

@@ -1,16 +1,17 @@
 package org.usfirst.frc4388.robot.commands.auton;
 
-import org.usfirst.frc4388.robot.commands.DriveAbsoluteTurnMP;
+
 import org.usfirst.frc4388.robot.commands.DriveGyroReset;
 import org.usfirst.frc4388.robot.commands.DriveSpeedShift;
 import org.usfirst.frc4388.robot.commands.DriveStraightBasic;
-import org.usfirst.frc4388.robot.commands.DriveStraightMP;
+
 import org.usfirst.frc4388.robot.commands.DriveTurnBasic;
 import org.usfirst.frc4388.robot.commands.ElevatorBasic;
+
 import org.usfirst.frc4388.robot.commands.IntakePosition;
 import org.usfirst.frc4388.robot.commands.IntakeSetSpeed;
 import org.usfirst.frc4388.robot.subsystems.Carriage;
-import org.usfirst.frc4388.robot.subsystems.Drive;
+
 import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
 
 import edu.wpi.first.wpilibj.command.CommandGroup;
@@ -25,8 +26,9 @@ public class RightStartLeftScore extends CommandGroup {
     {
     	addSequential(new DriveGyroReset());
     	addSequential(new DriveSpeedShift(true));
-    	addSequential(new DriveSpeedShift(true));
     	addSequential(new IntakePosition(true));
+    	
+    	
     	addSequential(new DriveStraightBasic(-215, 60, true, true, 0));
     	addSequential(new ElevatorBasic(5));
     	addSequential(new DriveTurnBasic(true, 87, 300, MPSoftwareTurnType.TANK));
@@ -41,6 +43,7 @@ public class RightStartLeftScore extends CommandGroup {
     	addSequential(new WaitCommand(.2));
     	addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));
     	addSequential(new DriveStraightBasic(-10, 60, true, true, 0));
+    	addSequential(new DriveSpeedShift(false));
     	//addSequential(new DriveStraightMP(60, 10, true, true, 0));  // 95 for 112" greenville
     }
 }

src/org/usfirst/frc4388/robot/commands/auton/RightSwitchAuton.java

@@ -1,17 +1,17 @@
 package org.usfirst.frc4388.robot.commands.auton;
 
-import org.usfirst.frc4388.robot.commands.DriveAbsoluteTurnMP;
+
 import org.usfirst.frc4388.robot.commands.DriveGyroReset;
 import org.usfirst.frc4388.robot.commands.DriveSpeedShift;
 import org.usfirst.frc4388.robot.commands.DriveStraightBasic;
-import org.usfirst.frc4388.robot.commands.DriveStraightMP;
+
 import org.usfirst.frc4388.robot.commands.DriveTurnBasic;
 import org.usfirst.frc4388.robot.commands.ElevatorBasic;
-import org.usfirst.frc4388.robot.commands.ElevatorSetSpeed;
+
 import org.usfirst.frc4388.robot.commands.IntakePosition;
 import org.usfirst.frc4388.robot.commands.IntakeSetSpeed;
 import org.usfirst.frc4388.robot.subsystems.Carriage;
-import org.usfirst.frc4388.robot.subsystems.Drive;
+
 import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
 
 import edu.wpi.first.wpilibj.command.CommandGroup;
@@ -26,6 +26,8 @@ public class RightSwitchAuton extends CommandGroup {
     	addSequential(new DriveGyroReset());
     	addSequential(new DriveSpeedShift(true));
     	addSequential(new IntakePosition(true));
+    	
+    	
     	addSequential(new DriveStraightBasic(-130, 60, true, true, 0));
     	addSequential(new ElevatorBasic(30));
     	addSequential(new DriveTurnBasic(true, 87, 300, MPSoftwareTurnType.TANK));
@@ -36,6 +38,8 @@ public class RightSwitchAuton extends CommandGroup {
     	addSequential(new IntakePosition(false));
     	addSequential(new WaitCommand(.5));
     	addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));
+    	addSequential(new DriveStraightBasic(-20, 60, true, true, 0));
+    	addSequential(new DriveSpeedShift(false));
 
     	//addSequential(new DriveStraightMP(60, 10, true, true, 0));  // 95 for 112" greenville
        

src/org/usfirst/frc4388/robot/commands/auton/ScaleFrom1.java

@@ -0,0 +1,47 @@
+package org.usfirst.frc4388.robot.commands.auton;
+
+
+import org.usfirst.frc4388.robot.commands.DriveGyroReset;
+import org.usfirst.frc4388.robot.commands.DriveSpeedShift;
+import org.usfirst.frc4388.robot.commands.DriveStraightBasic;
+
+import org.usfirst.frc4388.robot.commands.DriveTurnBasic;
+import org.usfirst.frc4388.robot.commands.ElevatorBasic;
+
+import org.usfirst.frc4388.robot.commands.IntakePosition;
+import org.usfirst.frc4388.robot.commands.IntakeSetSpeed;
+import org.usfirst.frc4388.robot.subsystems.Carriage;
+
+import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+
+/**
+ *
+ */
+public class ScaleFrom1 extends CommandGroup {
+    
+    public ScaleFrom1() {
+    	addSequential(new DriveGyroReset());
+    	addSequential(new DriveSpeedShift(true));
+    	addSequential(new IntakePosition(true));
+    	
+    	
+    	addSequential(new DriveStraightBasic(-290, 60, true, true, 0));
+    	addSequential(new ElevatorBasic(70));
+    	addSequential(new DriveTurnBasic(true, -90, 100, MPSoftwareTurnType.TANK));
+    	//addSequential(new DriveStraightBasic(5, 20, true, true, 0));
+    	addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED));
+    	//addSequential(new WaitCommand(.1));
+    	addSequential(new IntakePosition(false));
+    	addSequential(new WaitCommand(.5));
+    	addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));
+    	addSequential(new DriveStraightBasic(-15, 20, true, true, 0));
+    	addSequential(new ElevatorBasic(10));
+    	addSequential(new DriveSpeedShift(false));
+
+    	//addSequential(new DriveStraightMP(60, 10, true, true, 0));  // 95 for 112" greenville
+       
+    }
+}

src/org/usfirst/frc4388/robot/commands/auton/ScaleFrom3.java

@@ -1,17 +1,17 @@
 package org.usfirst.frc4388.robot.commands.auton;
 
-import org.usfirst.frc4388.robot.commands.DriveAbsoluteTurnMP;
+
 import org.usfirst.frc4388.robot.commands.DriveGyroReset;
 import org.usfirst.frc4388.robot.commands.DriveSpeedShift;
 import org.usfirst.frc4388.robot.commands.DriveStraightBasic;
-import org.usfirst.frc4388.robot.commands.DriveStraightMP;
+
 import org.usfirst.frc4388.robot.commands.DriveTurnBasic;
 import org.usfirst.frc4388.robot.commands.ElevatorBasic;
-import org.usfirst.frc4388.robot.commands.ElevatorSetSpeed;
+
 import org.usfirst.frc4388.robot.commands.IntakePosition;
 import org.usfirst.frc4388.robot.commands.IntakeSetSpeed;
 import org.usfirst.frc4388.robot.subsystems.Carriage;
-import org.usfirst.frc4388.robot.subsystems.Drive;
+
 import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
 
 import edu.wpi.first.wpilibj.command.CommandGroup;
@@ -26,7 +26,8 @@ public class ScaleFrom3 extends CommandGroup {
     	addSequential(new DriveGyroReset());
     	addSequential(new DriveSpeedShift(true));
     	addSequential(new IntakePosition(true));
-    	addSequential(new ElevatorBasic(10));
+    	
+    	
     	addSequential(new DriveStraightBasic(-290, 60, true, true, 0));
     	addSequential(new ElevatorBasic(70));
     	addSequential(new DriveTurnBasic(true, 90, 100, MPSoftwareTurnType.TANK));
@@ -37,7 +38,8 @@ public class ScaleFrom3 extends CommandGroup {
     	addSequential(new WaitCommand(.5));
     	addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));
     	addSequential(new DriveStraightBasic(-15, 20, true, true, 0));
-    	addSequential(new ElevatorBasic(5));
+    	addSequential(new ElevatorBasic(10));
+    	addSequential(new DriveSpeedShift(false));
 
     	//addSequential(new DriveStraightMP(60, 10, true, true, 0));  // 95 for 112" greenville
        

src/org/usfirst/frc4388/robot/subsystems/Elevator.java

@@ -194,7 +194,7 @@ public class Elevator extends Subsystem implements ControlLoopable
     public void moveElevatorXbox()
     {
     	double moveElevatorInput;
-    	double elevatorSafeZone = -30; 
+    	double elevatorSafeZone = 15; 
     	
     	double elevatorPos = getEncoderElevatorPosition();