Auton And FMS Update

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

@@ -9,6 +9,9 @@ import buttons.XBoxTriggerButton;
 import org.usfirst.frc4388.controller.IHandController;
 import org.usfirst.frc4388.controller.XboxController;
 import org.usfirst.frc4388.robot.commands.*;
+import org.usfirst.frc4388.robot.commands.auton.CrossTheBaseLine;
+import org.usfirst.frc4388.robot.commands.auton.RightSwitchAuton;
+
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.Joystick;
@@ -190,6 +193,7 @@ 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

@@ -123,16 +123,16 @@ public class Robot extends IterativeRobot
 		
 		RRautonTaskChooser.addDefault("Choose RR Program", new CrossTheBaseLine());
 		
-		RRautonTaskChooser.addObject("1", new CrossTheBaseLine());
+
 		RRautonTaskChooser.addObject("Left to Right Switch", new LeftStartRightScore());
 		
-		RRautonTaskChooser.addObject("2", new CrossTheBaseLine());
 		RRautonTaskChooser.addObject("Center to Left Switch", new CenterLeft());
 		RRautonTaskChooser.addObject("Center to Right Switch", new CenterRight());
 		
-		RRautonTaskChooser.addObject("3", new CrossTheBaseLine());
+
 		RRautonTaskChooser.addObject("Right to Right Switch", new RightSwitchAuton());
 		RRautonTaskChooser.addObject("Right to Left Switch", new RightStartLeftScore());
+		RRautonTaskChooser.addObject("Scale3", new ScaleFrom3());
 
 		
 		SmartDashboard.putData("RRAuton Task", RRautonTaskChooser);
@@ -145,14 +145,14 @@ public class Robot extends IterativeRobot
 		
 		RLautonTaskChooser.addDefault("Choose RL Program", new CrossTheBaseLine());
 		
-		RLautonTaskChooser.addObject("1", new CrossTheBaseLine());
+
 		RLautonTaskChooser.addObject("Left to Right Switch", new LeftStartRightScore());
 		
-		RLautonTaskChooser.addObject("2", new CrossTheBaseLine());
+
 		RLautonTaskChooser.addObject("Center to Left Switch", new CenterLeft());
 		RLautonTaskChooser.addObject("Center to Right Switch", new CenterRight());
 		
-		RLautonTaskChooser.addObject("3", new CrossTheBaseLine());
+
 		RLautonTaskChooser.addObject("Right to Right Switch", new RightSwitchAuton());
 		RLautonTaskChooser.addObject("Right to Left Switch", new RightStartLeftScore());
 
@@ -168,14 +168,14 @@ public class Robot extends IterativeRobot
 		
 		LLautonTaskChooser.addDefault("Choose LL Program", new CrossTheBaseLine());
 		
-		LLautonTaskChooser.addObject("1", new CrossTheBaseLine());
+
 		LLautonTaskChooser.addObject("Left to Right Switch", new LeftStartRightScore());
 		
-		LLautonTaskChooser.addObject("2", new CrossTheBaseLine());
+
 		LLautonTaskChooser.addObject("Center to Left Switch", new CenterLeft());
 		LLautonTaskChooser.addObject("Center to Right Switch", new CenterRight());
 		
-		LLautonTaskChooser.addObject("3", new CrossTheBaseLine());
+
 		LLautonTaskChooser.addObject("Right to Right Switch", new RightSwitchAuton());
 		LLautonTaskChooser.addObject("Right to Left Switch", new RightStartLeftScore());
 
@@ -189,15 +189,13 @@ public class Robot extends IterativeRobot
 		LRautonTaskChooser = new SendableChooser<Command>();
 		
 		LRautonTaskChooser.addDefault("Choose LR Program", new CrossTheBaseLine());
-		
+	
-		LRautonTaskChooser.addObject("1", new CrossTheBaseLine());
 		LRautonTaskChooser.addObject("Left to Right Switch", new LeftStartRightScore());
 		
-		LRautonTaskChooser.addObject("2", new CrossTheBaseLine());
+
 		LRautonTaskChooser.addObject("Center to Left Switch", new CenterLeft());
 		LRautonTaskChooser.addObject("Center to Right Switch", new CenterRight());
 		
-		LRautonTaskChooser.addObject("3", new CrossTheBaseLine());
 		LRautonTaskChooser.addObject("Right to Right Switch", new RightSwitchAuton());
 		LRautonTaskChooser.addObject("Right to Left Switch", new RightStartLeftScore());
 
@@ -251,28 +249,31 @@ public class Robot extends IterativeRobot
         
     	String gameData;
 		gameData = DriverStation.getInstance().getGameSpecificMessage();
-                if(gameData.length() > 0)
+        if (gameData.length() > 0) {
-                {
+        	if (gameData.charAt(0) == 'L') {
-		  if(gameData.charAt(0) == 'L')
+        		if (gameData.charAt(1) == 'L') {
-		  {
+        			if (LLautonomousCommand != null) {
-			  if(gameData.charAt(1) == 'L')
+        				LLautonomousCommand.start();
-			  {
+        			}
-				  if (LLautonomousCommand != null) LLautonomousCommand.start();
+        		} else {
-			  } else {
+        			if (LRautonomousCommand != null) {
-				  if (LRautonomousCommand != null) LRautonomousCommand.start();
+        				LRautonomousCommand.start();
-			  }
+        			}
-	                }
+        		}
-		  } else {
+        	} else {
-			  if(gameData.charAt(1) == 'L')
+        		if (gameData.charAt(1) == 'L') {
-			  {
+  				  	if (RLautonomousCommand != null) {
-				  if (RLautonomousCommand != null) RLautonomousCommand.start();
+  				  		RLautonomousCommand.start();
-			  } else {
+  				  	}
-				  if (RRautonomousCommand != null) RRautonomousCommand.start();
+        		} else {
-			  }
+        			if (RRautonomousCommand != null) {
-	                }
+        				RRautonomousCommand.start();
-			
+        			}
-		  }
+  			  	}
-                
+        	}
+		}
+	}
+
     
     public void autonomousPeriodic() {
         Scheduler.getInstance().run();

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

@@ -0,0 +1,43 @@
+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;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+
+/**
+ *
+ */
+public class ScaleFrom3 extends CommandGroup {
+    
+    public ScaleFrom3() {
+    	addSequential(new DriveGyroReset());
+    	addSequential(new DriveSpeedShift(true));
+    	addSequential(new IntakePosition(true));
+    	addSequential(new DriveStraightBasic(-200, 60, true, true, 0));
+    	addSequential(new ElevatorBasic(10));
+    	addSequential(new DriveTurnBasic(true, 150, 300, MPSoftwareTurnType.TANK));
+    	addSequential(new ElevatorBasic(70));
+    	addSequential(new DriveStraightBasic(10, 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 DriveStraightMP(60, 10, true, true, 0));  // 95 for 112" greenville
+       
+    }
+}