Changes Befor Finals

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

@@ -33,13 +33,22 @@ public class CenterLeft extends CommandGroup {
     	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 DriveTurnBasic(true, 12, 150, MPSoftwareTurnType.TANK));
-    	addSequential(new DriveStraightBasic(10, 45, true, true, 0));
+    	addSequential(new DriveStraightBasic(12, 45, true, true, 0));
+    	//addParallel(new TimeoutBecaseYea());
+    	//addSequential(new WaitCommand(3));
+    	
+    	
+    	
     	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 DriveSpeedShift(false));

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

@@ -34,12 +34,24 @@ public class CenterRight extends CommandGroup {
     	addSequential(new DriveStraightBasic(80, 45, true, true, 0));
     	addSequential(new ElevatorBasic(25));
     	addSequential(new DriveTurnBasic(true, -35, 150, MPSoftwareTurnType.TANK));
+    	addParallel(new TimeoutBecaseYea());
     	addSequential(new DriveStraightBasic(25, 45, true, true, 0));
+    	addSequential(new WaitCommand(3));
+    	
+    	
+    	/*
     	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 DriveSpeedShift(false));

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

@@ -0,0 +1,67 @@
+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 Cube2Right extends CommandGroup {
+    
+    public Cube2Right() {
+    	addSequential(new DriveGyroReset());
+    	addSequential(new DriveSpeedShift(true));
+    	addSequential(new IntakePosition(true));
+    	
+    	
+    	addSequential(new DriveStraightBasic(-140, 50, true, true, 0));
+    	addSequential(new ElevatorBasic(30));
+    	addSequential(new WaitCommand(.2));
+    	addSequential(new DriveTurnBasic(true, 87, 150, MPSoftwareTurnType.TANK));
+    	addSequential(new ElevatorBasic(30));
+    	addSequential(new DriveStraightBasic(8, 50, true, true, 0));
+    	addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED));
+    	//addSequential(new WaitCommand(.1));
+    	addSequential(new IntakePosition(false));
+    	addSequential(new WaitCommand(.4));
+    	addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));
+    	addSequential(new DriveStraightBasic(-10, 50, true, true, 0));
+    	addSequential(new DriveTurnBasic(true, 40, 150, MPSoftwareTurnType.TANK));
+    	addSequential(new DriveStraightBasic(-30, 50, true, true, 0));
+    	addSequential(new DriveTurnBasic(true, -10, 150, MPSoftwareTurnType.TANK));
+     	addSequential(new IntakeSetSpeed(Carriage.CUBE_INTAKE_SPEED));
+    	addSequential(new DriveStraightBasic(30, 50, true, true, 0));
+    	addSequential(new IntakePosition(true));
+    	addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));
+    	addSequential(new DriveStraightBasic(-30, 50, true, true, 0));
+    	addSequential(new DriveTurnBasic(true, 30, 150, MPSoftwareTurnType.TANK));
+    	addSequential(new ElevatorBasic(70));
+    	addSequential(new DriveStraightBasic(-20, 20, true, true, 0));
+    	addSequential(new DriveTurnBasic(true, 87, 150, MPSoftwareTurnType.TANK));
+    	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 DriveSpeedShift(false));
+
+    	      
+    }
+}

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

@@ -42,7 +42,6 @@ public class RightSwitchAuton extends CommandGroup {
     	addSequential(new DriveStraightBasic(-20, 50, 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/ScaleFrom1.java

@@ -28,10 +28,14 @@ public class ScaleFrom1 extends CommandGroup {
     	addSequential(new IntakePosition(true));
     	
     	
-    	addSequential(new DriveStraightBasic(-290, 50, true, true, 0));
+    	addSequential(new DriveStraightBasic(-10, 45, true, true, 0));
+    	
+    	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));
@@ -42,7 +46,7 @@ public class ScaleFrom1 extends CommandGroup {
     	
     	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/commands/auton/ScaleFrom3.java

@@ -29,7 +29,7 @@ public class ScaleFrom3 extends CommandGroup {
     	
     	
     	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)); 

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

@@ -0,0 +1,35 @@
+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 TimeoutBecaseYea extends CommandGroup {
+    
+    public TimeoutBecaseYea() 
+    {
+    	addSequential(new WaitCommand(2));
+    	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));
+
+    }
+}