Auto Changes

tweeks to auto scale and all switch pos work

.classpath

@@ -12,5 +12,6 @@
 	<classpathentry kind="var" path="wpiutil" sourcepath="wpiutil.sources"/>
 	<classpathentry kind="var" path="USERLIBS_DIR/CTRE_Phoenix-sources.jar"/>
 	<classpathentry kind="var" path="USERLIBS_DIR/CTRE_Phoenix.jar"/>
+	<classpathentry kind="var" path="USERLIBS_DIR/niVisionWPI.jar"/>
 	<classpathentry kind="output" path="bin"/>
 </classpath>

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

@@ -33,8 +33,8 @@ public class Constants {
     public static double kElevatorInchesOfTravelPerRev = 3.75;
 //	public static double kElevatorEncoderTicksPerInch = (double)kElevatorEncoderTickesPerRev / kElevatorInchesOfTravelPerRev;
 	public static double kElevatorEncoderTicksPerInch = 126.36;
-	public static double kElevatorBasicPercentOutputUp = -.9;
+	public static double kElevatorBasicPercentOutputUp = -.8;
-	public static double kElevatorBasicPercentOutputDown =.9;
+	public static double kElevatorBasicPercentOutputDown =.7;
 	
     // CONTROL LOOP GAINS
 

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

@@ -193,7 +193,6 @@ public class OI
 	        
 	        
 			SmartDashboard.putData("move elevator", new ElevatorBasic(20));
-			SmartDashboard.putData("test", new RightSwitchAuton());
 	        
 			///SmartDashboard.putData("Move Down", new ElevatorSetSpeed(ElevatorAuton.LOWER_ELEVATOR_SPEED));
 			///SmartDashboard.putData("Move UP", new ElevatorSetSpeed(ElevatorAuton.STOP_ELEVATOR_SPEED));

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

@@ -127,7 +127,7 @@ public class Robot extends IterativeRobot
 		RRautonTaskChooser.addObject("Left to Right Switch", new LeftStartRightScore());
 		
 		RRautonTaskChooser.addObject("Center to Right Switch", new CenterRight());
-		
+		RRautonTaskChooser.addObject("Center to Right 2 cube", new CenterRight2Cube());
 
 		RRautonTaskChooser.addObject("Right to Right Switch", new RightSwitchAuton());
 		RRautonTaskChooser.addObject("Right to Right Scale", new ScaleFrom3());
@@ -148,6 +148,7 @@ public class Robot extends IterativeRobot
 		RLautonTaskChooser.addObject("Left to Left Scale", new ScaleFrom1());
 
 		RLautonTaskChooser.addObject("Center to Right Switch", new CenterRight());
+		RLautonTaskChooser.addObject("Center to Right 2 cube", new CenterRight2Cube());
 		
 		RLautonTaskChooser.addObject("Right to Right Switch", new RightSwitchAuton());
 		
@@ -168,6 +169,7 @@ public class Robot extends IterativeRobot
 		
 
 		LLautonTaskChooser.addObject("Center to Left Switch", new CenterLeft());
+		LLautonTaskChooser.addObject("Center to Left Switch 2 cube", new CenterLeft2Cube());
 		
 
 		LLautonTaskChooser.addObject("Right to Left Switch", new RightStartLeftScore());
@@ -183,6 +185,7 @@ public class Robot extends IterativeRobot
 		LRautonTaskChooser.addObject("Left to Left Switch", new LeftSwitchAuton());
 
 		LRautonTaskChooser.addObject("Center to Left Switch", new CenterLeft());
+		LRautonTaskChooser.addObject("Center to Left Switch 2 cube", new CenterLeft2Cube());
 		
 		LRautonTaskChooser.addObject("Right to Left Switch", new RightStartLeftScore());
 		LRautonTaskChooser.addObject("Right To Right Scale", new ScaleFrom3());

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

@@ -59,7 +59,7 @@ public class DriveStraightBasic extends Command {
 		m_lastCommandExecuteTimestamp = currentTimestamp;
     	double steer = 0.0;
     	if (m_useGyroLock) {
-    		steer = - Robot.drive.getGyroAngleDeg() / 20.0;	//TODO: tune
+    		steer = - Robot.drive.getGyroAngleDeg() / 25.0;	//TODO: tune
     	}
     	Robot.drive.rawMoveSteer(m_speed, steer);
 		//SmartDashboard.putNumber("DSB Period", m_lastCommandExecutePeriod);

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

@@ -28,18 +28,19 @@ public class CenterLeft extends CommandGroup {
     	addSequential(new IntakePosition(true));
     	addSequential(new DriveSpeedShift(true));
     	
-    	addSequential(new DriveStraightBasic(-15, 50, true, true, 0));
+    	addSequential(new DriveStraightBasic(-15, 30, true, true, 0));
-    	addSequential(new DriveTurnBasic(true, 130, 300, MPSoftwareTurnType.TANK));
+    	addSequential(new DriveTurnBasic(true, 115, 150, MPSoftwareTurnType.TANK));
-    	addSequential(new DriveStraightBasic(53, 50, true, true, 0));
+    	addSequential(new WaitCommand(.2));
-    	addSequential(new ElevatorBasic(20));
+    	addSequential(new DriveStraightBasic(80, 45, true, true, 0));
-    	addSequential(new DriveTurnBasic(true, 34.5, 300, MPSoftwareTurnType.TANK));
+    	addSequential(new ElevatorBasic(30));
-    	addSequential(new DriveStraightBasic(19, 50, true, true, 0));
+    	addSequential(new DriveTurnBasic(true, 17, 150, MPSoftwareTurnType.TANK));
+    	addSequential(new DriveStraightBasic(10, 45, true, true, 0));
     	addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED));
     	//addSequential(new WaitCommand(.1));
     	addSequential(new IntakePosition(false));
     	addSequential(new WaitCommand(.2));
     	addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));
-    	addSequential(new DriveStraightBasic(-20, 50, true, true, 0));
+    	addSequential(new DriveStraightBasic(-20, 45, true, true, 0));
       	addSequential(new ElevatorBasic(10));
     	addSequential(new DriveSpeedShift(false));
     }

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

@@ -0,0 +1,65 @@
+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 CenterLeft2Cube extends CommandGroup {
+    
+    public CenterLeft2Cube() 
+    {
+    	addSequential(new DriveGyroReset());
+    	addSequential(new IntakePosition(true));
+    	addSequential(new DriveSpeedShift(true));
+    	
+    	addSequential(new DriveStraightBasic(-15, 30, true, true, 0));
+    	addSequential(new DriveTurnBasic(true, 115, 150, MPSoftwareTurnType.TANK));
+    	addSequential(new WaitCommand(.2));
+    	addSequential(new DriveStraightBasic(80, 45, true, true, 0));
+    	addSequential(new ElevatorBasic(30));
+    	addSequential(new DriveTurnBasic(true, 17, 150, MPSoftwareTurnType.TANK));
+    	addSequential(new DriveStraightBasic(10, 45, true, true, 0));
+    	addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED));
+    	//addSequential(new WaitCommand(.1));
+    	addSequential(new IntakePosition(false));
+    	addSequential(new WaitCommand(.2));
+    	addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));
+    	addSequential(new DriveStraightBasic(-20, 45, true, true, 0));
+      	addSequential(new ElevatorBasic(10));
+      	addSequential(new WaitCommand(.2));
+      	addSequential(new ElevatorBasic(2));
+      	addSequential(new DriveTurnBasic(true, -90, 150, MPSoftwareTurnType.TANK));
+    	addSequential(new IntakeSetSpeed(Carriage.CUBE_INTAKE_SPEED));
+    	addSequential(new DriveStraightBasic(20, 45, true, true, 0));
+    	addSequential(new IntakePosition(true));
+    	addSequential(new DriveStraightBasic(-20, 45, true, true, 0));
+    	addSequential(new ElevatorBasic(25));
+      	addSequential(new DriveTurnBasic(true, 90, 150, MPSoftwareTurnType.TANK));
+    	addSequential(new DriveStraightBasic(10, 45, true, true, 0));
+    	addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED));
+    	//addSequential(new WaitCommand(.1));
+    	addSequential(new IntakePosition(false));
+    	addSequential(new WaitCommand(.2));
+    	addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));
+    	addSequential(new DriveStraightBasic(-20, 45, true, true, 0));
+    	
+    	addSequential(new DriveSpeedShift(false));
+    	
+    }
+}

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

@@ -22,25 +22,26 @@ import edu.wpi.first.wpilibj.command.WaitCommand;
  */
 public class CenterRight extends CommandGroup {
     
-    public CenterRight()
+    public CenterRight() 
     {
     	addSequential(new DriveGyroReset());
     	addSequential(new IntakePosition(true));
     	addSequential(new DriveSpeedShift(true));
     	
-    	
+    	addSequential(new DriveStraightBasic(-15, 30, true, true, 0));
-    	addSequential(new DriveStraightBasic(-15, 50, true, true, 0));
+    	addSequential(new DriveTurnBasic(true, -115, 150, MPSoftwareTurnType.TANK));
-    	addSequential(new DriveTurnBasic(true, -130, 300, MPSoftwareTurnType.TANK));
+    	addSequential(new WaitCommand(.2));
-    	addSequential(new DriveStraightBasic(53, 50, true, true, 0));
+    	addSequential(new DriveStraightBasic(80, 45, true, true, 0));
-    	addSequential(new ElevatorBasic(20));
+    	addSequential(new ElevatorBasic(25));
-    	addSequential(new DriveTurnBasic(true, -34.5, 300, MPSoftwareTurnType.TANK));
+    	addSequential(new DriveTurnBasic(true, -35, 150, MPSoftwareTurnType.TANK));
-    	addSequential(new DriveStraightBasic(19, 50, true, true, 0));
+    	addSequential(new DriveStraightBasic(25, 45, true, true, 0));
     	addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED));
     	//addSequential(new WaitCommand(.1));
     	addSequential(new IntakePosition(false));
     	addSequential(new WaitCommand(.2));
     	addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));
-    	addSequential(new DriveStraightBasic(-20, 50, true, true, 0));
+    	addSequential(new DriveStraightBasic(-25, 45, true, true, 0));
+      	addSequential(new ElevatorBasic(10));
     	addSequential(new DriveSpeedShift(false));
     }
 }

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

@@ -0,0 +1,60 @@
+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 CenterRight2Cube extends CommandGroup {
+    
+    public CenterRight2Cube() 
+    {
+    	addSequential(new DriveGyroReset());
+    	addSequential(new IntakePosition(true));
+    	addSequential(new DriveSpeedShift(true));
+    	
+    	addSequential(new DriveStraightBasic(-15, 30, true, true, 0));
+    	addSequential(new DriveTurnBasic(true, -115, 150, MPSoftwareTurnType.TANK));
+    	addSequential(new WaitCommand(.2));
+    	addSequential(new DriveStraightBasic(80, 45, true, true, 0));
+    	addSequential(new ElevatorBasic(25));
+    	addSequential(new DriveTurnBasic(true, -35, 150, MPSoftwareTurnType.TANK));
+    	addSequential(new DriveStraightBasic(15, 45, true, true, 0));
+    	addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED));
+    	//addSequential(new WaitCommand(.1));
+    	addSequential(new IntakePosition(false));
+    	addSequential(new WaitCommand(.2));
+    	addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));
+    	addSequential(new DriveStraightBasic(-25, 45, true, true, 0));
+      	addSequential(new ElevatorBasic(10));
+    	addSequential(new WaitCommand(.2));
+      	addSequential(new ElevatorBasic(2));
+      	addSequential(new DriveTurnBasic(true, 90, 150, MPSoftwareTurnType.TANK));
+    	addSequential(new IntakeSetSpeed(Carriage.CUBE_INTAKE_SPEED));
+    	addSequential(new DriveStraightBasic(20, 45, true, true, 0));
+    	addSequential(new IntakePosition(true));
+    	addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));
+    	addSequential(new DriveStraightBasic(-20, 45, true, true, 0));
+    	
+    	addSequential(new ElevatorBasic(25));
+      	addSequential(new DriveTurnBasic(true, -90, 150, MPSoftwareTurnType.TANK));
+    	addSequential(new DriveStraightBasic(10, 45, true, true, 0));
+    	addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED));
+    	//addSequential(new WaitCommand(.1));
+    	addSequential(new IntakePosition(false));
+    	addSequential(new WaitCommand(.2));
+    	addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));
+    	addSequential(new DriveStraightBasic(-20, 45, true, true, 0));
+    	
+    	addSequential(new DriveSpeedShift(false));
+    	
+    }
+}

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

@@ -25,24 +25,30 @@ public class LeftStartRightScore extends CommandGroup {
     public LeftStartRightScore() 
     {
     	addSequential(new DriveGyroReset());
+    	addSequential(new WaitCommand(.1));
     	addSequential(new DriveSpeedShift(true));
     	addSequential(new IntakePosition(true));
     	
     	
-    	addSequential(new DriveStraightBasic(215, 50, true, true, 0));
+    	addSequential(new DriveStraightBasic(-215, 40, true, true, 0));
+    	addSequential(new WaitCommand(.2));
     	addSequential(new ElevatorBasic(5));
-    	addSequential(new DriveTurnBasic(true, -87, 300, MPSoftwareTurnType.TANK));
+    	addSequential(new DriveTurnBasic(true, -73, 150, MPSoftwareTurnType.TANK));
-    	addSequential(new DriveStraightBasic(180, 50, true, true, 0));
+    	addSequential(new WaitCommand(.2));
+    	addSequential(new DriveStraightBasic(155, 60, true, true, 0));
     	addSequential(new ElevatorBasic(30));
-    	addSequential(new DriveTurnBasic(true, 90, 300, MPSoftwareTurnType.TANK));
+    	addSequential(new WaitCommand(.2));
-    	addSequential(new DriveStraightBasic(5, 40, true, true, 0));
+    	addSequential(new DriveTurnBasic(true, 73, 150, MPSoftwareTurnType.TANK));
+    	addSequential(new WaitCommand(.2));
+    	addSequential(new DriveStraightBasic(10, 60, true, true, 0));
     	addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED));
     	//addSequential(new WaitCommand(.1));
     	addSequential(new IntakePosition(false));
+    	addSequential(new DriveStraightBasic(5, 60, true, true, 0));
     	addSequential(new WaitCommand(.2));
     	addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));
-    	addSequential(new DriveStraightBasic(-10, 50, true, true, 0));
+    	addSequential(new DriveStraightBasic(-15, 60, true, true, 0));
     	addSequential(new DriveSpeedShift(false));
-    	//addSequential(new DriveStraightMP(50, 10, true, true, 0));  // 95 for 112" greenville
+    	
     }
 }

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

@@ -30,7 +30,7 @@ public class LeftSwitchAuton extends CommandGroup {
     	
     	addSequential(new DriveStraightBasic(-130, 50, true, true, 0));
     	addSequential(new ElevatorBasic(30));
-    	addSequential(new DriveTurnBasic(true, -87, 300, MPSoftwareTurnType.TANK));
+    	addSequential(new DriveTurnBasic(true, -87, 150, MPSoftwareTurnType.TANK));
     	addSequential(new ElevatorBasic(30));
     	addSequential(new DriveStraightBasic(20, 50, true, true, 0));
     	addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED));

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

@@ -25,24 +25,30 @@ public class RightStartLeftScore extends CommandGroup {
     public RightStartLeftScore()  //////////TUNEDISH AND ALMOST GOOD
     {
     	addSequential(new DriveGyroReset());
+    	addSequential(new WaitCommand(.1));
     	addSequential(new DriveSpeedShift(true));
     	addSequential(new IntakePosition(true));
     	
     	
-    	addSequential(new DriveStraightBasic(-215, 50, true, true, 0));
+    	addSequential(new DriveStraightBasic(-215, 40, true, true, 0));
+    	addSequential(new WaitCommand(.2));
     	addSequential(new ElevatorBasic(5));
-    	addSequential(new DriveTurnBasic(true, 87, 300, MPSoftwareTurnType.TANK));
+    	addSequential(new DriveTurnBasic(true, 73, 150, MPSoftwareTurnType.TANK));
-    	addSequential(new DriveStraightBasic(180, 50, true, true, 0));
+    	addSequential(new WaitCommand(.2));
+    	addSequential(new DriveStraightBasic(155, 60, true, true, 0));
     	addSequential(new ElevatorBasic(30));
-       	addSequential(new DriveTurnBasic(true, -90, 300, MPSoftwareTurnType.TANK));
+    	addSequential(new WaitCommand(.2));
-    	addSequential(new DriveStraightBasic(10, 50, true, true, 0));
+    	addSequential(new DriveTurnBasic(true, -73, 150, MPSoftwareTurnType.TANK));
+    	addSequential(new WaitCommand(.2));
+    	addSequential(new DriveStraightBasic(10, 60, true, true, 0));
     	addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED));
     	//addSequential(new WaitCommand(.1));
     	addSequential(new IntakePosition(false));
+    	addSequential(new DriveStraightBasic(5, 60, true, true, 0));
     	addSequential(new WaitCommand(.2));
     	addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));
-    	addSequential(new DriveStraightBasic(-10, 50, true, true, 0));
+    	addSequential(new DriveStraightBasic(-15, 60, true, true, 0));
     	addSequential(new DriveSpeedShift(false));
-    	//addSequential(new DriveStraightMP(50, 10, true, true, 0));  // 95 for 112" greenville
+    	
     }
 }

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

@@ -28,11 +28,12 @@ public class RightSwitchAuton extends CommandGroup {
     	addSequential(new IntakePosition(true));
     	
     	
-    	addSequential(new DriveStraightBasic(-130, 50, true, true, 0));
+    	addSequential(new DriveStraightBasic(-140, 50, true, true, 0));
     	addSequential(new ElevatorBasic(30));
-    	addSequential(new DriveTurnBasic(true, 87, 300, MPSoftwareTurnType.TANK));
+    	addSequential(new WaitCommand(.2));
+    	addSequential(new DriveTurnBasic(true, 87, 150, MPSoftwareTurnType.TANK));
     	addSequential(new ElevatorBasic(30));
-    	addSequential(new DriveStraightBasic(20, 50, true, true, 0));
+    	addSequential(new DriveStraightBasic(8, 50, true, true, 0));
     	addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED));
     	//addSequential(new WaitCommand(.1));
     	addSequential(new IntakePosition(false));

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

@@ -37,8 +37,9 @@ public class ScaleFrom1 extends CommandGroup {
     	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 DriveStraightBasic(-25, 20, true, true, 0));
     	addSequential(new ElevatorBasic(10));
+    	
     	addSequential(new DriveSpeedShift(false));
 
     	//addSequential(new DriveStraightMP(50, 10, true, true, 0));  // 95 for 112" greenville

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

@@ -28,20 +28,24 @@ public class ScaleFrom3 extends CommandGroup {
     	addSequential(new IntakePosition(true));
     	
     	
-    	addSequential(new DriveStraightBasic(-290, 50, true, true, 0));
+    	addSequential(new DriveStraightBasic(-10, 45, true, true, 0));
+    	//addSequential(new DriveTurnBasic(true, -2, 100, MPSoftwareTurnType.TANK));
+    	addSequential(new DriveStraightBasic(-270, 45, true, true, 0));
     	addSequential(new ElevatorBasic(70));
+    	addSequential(new DriveStraightBasic(-30, 20, true, true, 0)); 
     	addSequential(new DriveTurnBasic(true, 90, 100, MPSoftwareTurnType.TANK));
     	//addSequential(new DriveStraightBasic(5, 20, true, true, 0));
+    	addSequential(new WaitCommand(.5));
     	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 DriveStraightBasic(-25, 20, true, true, 0));
     	addSequential(new ElevatorBasic(10));
     	addSequential(new DriveSpeedShift(false));
 
-    	//addSequential(new DriveStraightMP(50, 10, true, true, 0));  // 95 for 112" greenville
+    	//addSequential(new DriveStraightMP(60, 10, true, true, 0));  // 95 for 112" greenville
        
     }
-}
+}

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

@@ -35,7 +35,7 @@ public class Carriage extends Subsystem {
 	private WPI_TalonSRX carriageRight;
 	public static enum CarriageControlMode { JOYSTICK, MP_STRAIGHT, HOLD, MANUAL};
 	public static final double CUBE_INTAKE_SPEED = 0.40;
-	public static final double CUBE_EJECT_SPEED = -1.0;
+	public static final double CUBE_EJECT_SPEED = -0.8;
 	public static final double CUBE_STOP_SPEED = 0;
 	/////^^^^^^^^^ replace this line with the modes we need
 	

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

@@ -25,6 +25,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 import com.ctre.phoenix.motorcontrol.FeedbackDevice;
 import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
+import com.ctre.phoenix.motorcontrol.RemoteLimitSwitchSource;
 import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.SensorCollection;
@@ -100,14 +101,34 @@ public class Elevator extends Subsystem implements ControlLoopable
 			//Setting left elevator motor as follower
     		elevatorLeft.set(ControlMode.Follower, elevatorRight.getDeviceID());
     		elevatorLeft.setInverted(false);
-    		elevatorRight.setSensorPhase(true);
     		elevatorLeft.setNeutralMode(NeutralMode.Brake);
     		elevatorRight.setNeutralMode(NeutralMode.Brake);
+    		elevatorRight.setSensorPhase(true);
     		//Limit Switch Left
     		//elevatorLeft.overrideLimitSwitchesEnable(true);
     		elevatorLeft.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
     		elevatorLeft.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
     		
+    		//Limit Switch Right
+    		//elevatorRight.overrideLimitSwitchesEnable(true);
+			//elevatorRight.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
+    		//elevatorRight.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
+    		
+    		
+    		//Change This boi
+    		
+    	//	elevatorRight.configForwardSoftLimitThreshold(10000, 0); //right here
+    		//elevatorRight.configReverseSoftLimitThreshold(5, 0);
+    		//elevatorRight.configForwardSoftLimitEnable(true, 0);
+    		//elevatorRight.configReverseSoftLimitEnable(true, 0);
+    		
+    		//sos
+    		//elevatorRight.enableLimitSwitch(true, true);
+
+    		
+    		
+    		
+    		
     	}
     	catch(Exception e)
     	{
@@ -211,14 +232,15 @@ public class Elevator extends Subsystem implements ControlLoopable
      	{
      		elevatorRight.set(moveElevatorInput);
      	}
-    	/*
+     		
+     		/*
      		if(elevatorPos <= elevatorSafeZone && elevatorPos >= 0)
      		{
      			elevatorRight.set(moveElevatorInput);
      		}
      		else if(elevatorPos > elevatorSafeZone)
      		{
-     			elevatorRight.set(moveElevatorInput * 0.8);
+     			elevatorRight.set(moveElevatorInput * 0.65);
      			
      			
      			if(holdButtonPressed == true)
@@ -229,17 +251,19 @@ public class Elevator extends Subsystem implements ControlLoopable
              	{
              		elevatorRight.set(moveElevatorInput * 0.75);
              	}
-             	*/
+             	
-     		//}
+     		}
-     		/*
+     		
      		else if(elevatorPos < 0)
      		{
              	elevatorRight.set(moveElevatorInput * 0.75);
      		}
      		*/
+     	}
+
      	
 //     	System.out.println(elevatorPos);		//-6.9 to 1.9   total: 8.8 range
-	}
+	
     
 	//PID encoder position
 	public double getEncoderElevatorPosition()
@@ -295,7 +319,7 @@ public class Elevator extends Subsystem implements ControlLoopable
 		elevatorRight.set(/*ControlMode.PercentOutput,*/ output);
 	}
 	
-	/*public void holdInPos()
+	public void holdInPos()
 	{
 		elevatorRight.set(-0.43 * 0.2);
 	}
@@ -326,9 +350,9 @@ public class Elevator extends Subsystem implements ControlLoopable
 			
 			pressed = false;
 			}
-		}*/
+		}
 
-	
+	}
 		
 		//pressed = (isPressed.isFwdLimitSwitchClosed() == true) ? true : false;
 	

src/org/usfirst/frc4388/utility/CANTalonEncoder.java

@@ -8,6 +8,8 @@ public class CANTalonEncoder extends WPI_TalonSRX
 	private double encoderTicksToWorld;
 	private boolean isRight = true;
 	
+
+	
 	public CANTalonEncoder(int deviceNumber, double encoderTicksToWorld, FeedbackDevice feedbackDevice) {
 		this(deviceNumber, encoderTicksToWorld, false, feedbackDevice);
 	}
@@ -60,4 +62,4 @@ public class CANTalonEncoder extends WPI_TalonSRX
     	//return convertEncoderTicksToWorld(this.getSpeed() / 0.1);
     	return convertEncoderTicksToWorld(this.getSelectedSensorVelocity(0) / 0.1);	//TODO: verify want 0="Primary closed-loop"
     }
-}
+}