Update auto routines and add java paths

Author: notmattlythgoe

Diff for: jackson-annotations-2.9.4.jar

@@ -2,24 +2,36 @@
 
 import org.usfirst.frc.team2363.robot.commands.elevator.RaiseElevator;
 import org.usfirst.frc.team2363.robot.commands.gripper.EjectCube;
-import org.usfirst.frc.team2363.robot.commands.gripper.LowerWrist;
 import org.usfirst.frc.team2363.robot.subsystems.Elevator.Height;
+import org.usfirst.frc.team319.models.SrxTrajectory;
+import org.usfirst.frc.team319.robot.commands.FollowTrajectory;
 
+import edu.wpi.first.wpilibj.command.Command;
 import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
 /**
  *
  */
 public class AutoGroup extends CommandGroup {
+	
+	 public AutoGroup(SrxTrajectory path, Height elevatorHeight, double elevatorDelay) {
+		 this(path, elevatorHeight, elevatorDelay, null);
+	 }
 
-    public AutoGroup(String path, Height elevatorHeight) {
-        addParallel(new RaiseElevator(elevatorHeight));
-        addParallel(new LowerWrist());
-//        addSequential(new PathRunner(path));
+    public AutoGroup(SrxTrajectory path, Height elevatorHeight, double elevatorDelay, Command phase2) {
+        addParallel(new ElevatorCommand(elevatorHeight, elevatorDelay));
+        addSequential(new FollowTrajectory(path));
         addSequential(new EjectCube());
+        if (phase2 != null) {
+        	addSequential(phase2);
+        }
+    }
+    
+    private class ElevatorCommand extends CommandGroup {
+    	
+    	public ElevatorCommand(Height height, double delay) {
+    		addSequential(new WaitCommand(delay));
+    		addSequential(new RaiseElevator(height));
+    	}
     }
-//    public AutoGroup(BoTHTrajectory path, Height elevatorHeight, boolean reverse) {
-//        addParallel(new RaiseElevator(elevatorHeight));
-//        addSequential(new PathRunner(path, reverse));
-//        addSequential(new EjectCube());
-//    }
 }

Diff for: jackson-core-2.9.4.jar

@@ -1,183 +1,157 @@
 package org.usfirst.frc.team2363.robot.commands.autonomous;
 
-import java.util.HashMap;
-import java.util.Map;
-
+import org.iif.th.util.logger.HelixEvents;
 import org.usfirst.frc.team2363.robot.subsystems.Elevator.Height;
+import org.usfirst.frc.team319.models.GameState;
+import org.usfirst.frc.team319.models.GameState.Side;
 import org.usfirst.frc.team319.models.SrxTrajectory;
-import org.usfirst.frc.team319.utils.SrxTrajectoryImporter;
+import org.usfirst.frc.team319.paths.CenterSwitch;
+import org.usfirst.frc.team319.paths.OppositeSideScale;
+import org.usfirst.frc.team319.paths.SameSideScale;
+import org.usfirst.frc.team319.paths.SameSideSwitch;
 
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.command.Command;
 
 public class AutoRoutines {
 
 	// AutoType Order must match paths order below.
 	public enum AutoType {
-		CENTER_SWITCH("CameraSwitch"),
-		SAME_SIDE_SWITCH("SameSideSwitch"),
-		SAME_SIDE_SCALE("SameSideScale"),
-		OPPOSITE_SIDE_SCALE("OpposideSideScale"),
-		BASELINE("Baseline"),
-		PATH_TO_CUBE("PathToCube"),
-		SHORT_PATH("ShortPath");
+		CENTER_SWITCH(Height.SWITCH, 0, null),
+		SAME_SIDE_SWITCH(Height.SWITCH, 0, null),
+		SAME_SIDE_SCALE(Height.SWITCH, 0, null),
+		OPPOSITE_SIDE_SCALE(Height.SWITCH, 0, null),
+		BASELINE(Height.GROUND, 0, null);
+		
+		private Height height;
+		private double delay;
+		private Command phase2;
+		
+		private AutoType(Height height,  double delay, Command phase2) {
+			this.height = height;
+			this.delay = delay;
+			this.phase2 = phase2;
+		}
 
-		private String fileName;
+		public Height getHeight() {
+			return height;
+		}
 
-		AutoType(String fileName) {
-			this.fileName = fileName;
+		public double getDelay() {
+			return delay;
 		}
 
-		public String getFileName() {
-			return fileName;
+		public Command getPhase2() {
+			return phase2;
 		}
 	}
 
-	private SrxTrajectoryImporter importer = new SrxTrajectoryImporter("/home/lvuser");
-	
-	private AutoType autoType = AutoType.BASELINE;
-	private String gameData;
-    char ourSwitch, opponentSwitch, scale, robotPosition;
-	private Height height;
-	private Boolean reverse = false;
-	
-	// Hash map allowing look ups of path object based on autonomous path file name. 
-	Map<AutoType, SrxTrajectory> autoMap = new HashMap<AutoType, SrxTrajectory>();
-	
-	public AutoRoutines() {
-		loadPaths();
+	enum AutoMode {
+		CENTER_SWITCH,
+		OUR_SIDE_ONLY,
+		SWITCH_SCALE_SCALE,
+		SCALE_ONLY;
 	}
 
-	/* 
-	 * Read the autonomous digital switch selections &  Load all the autonomous paths to save time during autonomous.  
-	 * This routine should be called in RobotInit.
-	 */
-	public void loadPaths() {
-		try {
-			for (AutoType path: AutoType.values()) {
-				autoMap.put(path, importer.importSrxTrajectory(path.getFileName()));
-			}
-		} catch(Exception e) {
-			e.printStackTrace();
-		}
-	}
+    private static DigitalInput left = new DigitalInput(0);
+    private static DigitalInput right = new DigitalInput(1);
+    private static DigitalInput centerSwitch = new DigitalInput(2);
+    private static DigitalInput ourSideOnly = new DigitalInput(3);
+    private static DigitalInput switchScaleScale = new DigitalInput(4);
+    private static DigitalInput scaleOnly = new DigitalInput(5);  // default
 
-	/* 
-	 * Get the locations of the switches and scale with respect to our alliance wall. 
-	 */
-	public void obtainPlateStates() {
-
-		gameData = DriverStation.getInstance().getGameSpecificMessage();
-		
-		ourSwitch = gameData.charAt(0);
-		scale = gameData.charAt(1);
-		opponentSwitch = gameData.charAt(2);
-		
-		
-		// Read the switch setting and plan auto routes based on plate locations above.
-		determineAutoRoute();
-		
-		updateSmartDashboard();
-		
-	}
 
 	/* 
 	 * Base on Robot Position on the alliance wall & plates states, determines 
 	 * which auto routine to run, gripper height, and whether left & right motion
 	 * profiles need to be reverse base on field symmetry.
 	 * 
 	 */
-	public void determineAutoRoute () {
-		
-		DigitalInput Left = new DigitalInput(0);
-		DigitalInput Right = new DigitalInput(1);
-		DigitalInput CenterSwitch = new DigitalInput(2);
-		DigitalInput OurSideOnly = new DigitalInput(3);
-		DigitalInput SwitchScaleScale = new DigitalInput(4);
-		DigitalInput ScaleOnly = new DigitalInput(5);  // default
-			
-		/*
-		 *  Switch left & right motor profiles, if robot is on right side of the alliance
-		 *  wall, when facing the field  OR  if robot is at center of alliance wall & our 
-		 *  switch plate is on the right.    
-		 */
-		if (Left.get()) {
-			robotPosition = 'L';
-			reverse = false;
-		} else if (Right.get()) {
-			robotPosition = 'R';
-			reverse = true;
-		} else {   // Center Switch
-			// Motor profile reverse is based on switch location in CenterSwitch case
-			reverse = (ourSwitch == 'R')? true : false; 
-		}
-		
-		height = Height.SWITCH;	
-		
-		if (CenterSwitch.get()) {
-			autoType = AutoType.CENTER_SWITCH;
-		} else if (OurSideOnly.get()) {  // Our Side only auto
-			if (ourSwitch == robotPosition) {
-				// Switch is on our side. Go for the switch first over the scale.
-				autoType = AutoType.SAME_SIDE_SWITCH;
-			} else if (scale == robotPosition) {
-				// Switch is not on our side, but scale is.
-				autoType = AutoType.SAME_SIDE_SCALE;
-				height = Height.SCALE;
-			} else {  
-				// Neither the Switch nor the Scale are on our side.
-				autoType = AutoType.BASELINE;
-				height = Height.GROUND;
-			}
-		} else if (SwitchScaleScale.get()) { 
-			if (ourSwitch == robotPosition) {
-				// Switch is on our side. Go for the switch first over the scale.
-				autoType = AutoType.SAME_SIDE_SWITCH;
-			} else if (scale == robotPosition){
-				// Switch is not on our side, but scale is. Go for scale.
-				autoType = AutoType.SAME_SIDE_SCALE;
-				height = Height.SCALE;
-			} else { 
-				// Neither the Switch nor the Scale are on our side. Go for opposite side scale. 
-				autoType = AutoType.OPPOSITE_SIDE_SCALE;
-				height = Height.SCALE;
-			}
-		} else {  // ScaleOnly run
-			height = Height.SCALE;
-			if (scale == robotPosition){
-				autoType = AutoType.SAME_SIDE_SCALE;
-			} else {
-				autoType = AutoType.OPPOSITE_SIDE_SCALE;
-			}
-		}
+	public static AutoGroup getAutoRoute () {
+		GameState state = new GameState(DriverStation.getInstance().getGameSpecificMessage());
+		Side robotSide = getRobotSide(state);
+		AutoType selectedAutoType = getAutoType(getSelectedAutoMode(), state, robotSide);
+		HelixEvents.addEvent("Selected Auto Mode: " + selectedAutoType.name() + ", flipped: " + (robotSide == Side.RIGHT));
+		return new AutoGroup(
+				getPath(selectedAutoType, robotSide == Side.RIGHT), 
+				selectedAutoType.getHeight(),
+				selectedAutoType.getDelay(),
+				selectedAutoType.getPhase2());
 	}
 
-	public SrxTrajectory getPath () {
-		return autoMap.get(autoType);
-	}
-	
-	public SrxTrajectory getPath (AutoType autoType) {
-		return autoMap.get(autoType);
+	private static Side getRobotSide(GameState state) {
+		if (centerSwitch.get()) {
+			return state.mySwitchSide; 
+		} else {
+			if (left.get()) {
+				return Side.LEFT;
+			} else {
+				return Side.RIGHT;
+			} 
+		}
 	}
 
-	public Height getHeight() {
-		return height;
+	private static AutoMode getSelectedAutoMode() {
+		if (centerSwitch.get()) {
+			return AutoMode.CENTER_SWITCH;
+		} else if (ourSideOnly.get()) {  // Our Side only auto
+			return AutoMode.OUR_SIDE_ONLY;
+		} else if (switchScaleScale.get()) { 
+			return AutoMode.SWITCH_SCALE_SCALE;
+		} else {  // ScaleOnly run
+			return AutoMode.SCALE_ONLY;
+		}
 	}
 
-	public boolean getReverse() {
-		return reverse;
+	static AutoType getAutoType(AutoMode selectedAutoMode, GameState state, Side robotSide) {
+		switch (selectedAutoMode) {
+			case CENTER_SWITCH:
+				return AutoType.CENTER_SWITCH;
+			case OUR_SIDE_ONLY:
+				if (state.mySwitchSide == robotSide) {
+					// Switch is on our side. Go for the switch first over the scale.
+					return AutoType.SAME_SIDE_SWITCH;
+				} else if (state.scaleSide == robotSide) {
+					// Switch is not on our side, but scale is.
+					return AutoType.SAME_SIDE_SCALE;
+				} else {  
+					// Neither the Switch nor the Scale are on our side.
+					return AutoType.BASELINE;
+				}
+			case SWITCH_SCALE_SCALE:
+				if (state.mySwitchSide == robotSide) {
+					// Switch is on our side. Go for the switch first over the scale.
+					return AutoType.SAME_SIDE_SWITCH;
+				} else if (state.scaleSide == robotSide){
+					// Switch is not on our side, but scale is. Go for scale.
+					return AutoType.SAME_SIDE_SCALE;
+				} else { 
+					// Neither the Switch nor the Scale are on our side. Go for opposite side scale. 
+					return AutoType.OPPOSITE_SIDE_SCALE;
+				}
+			default:
+				if (state.scaleSide == robotSide) {
+					return AutoType.SAME_SIDE_SCALE;
+				} else {
+					return AutoType.OPPOSITE_SIDE_SCALE;
+				}
+			}
 	}
 
-	private void updateSmartDashboard () {
-		
-		SmartDashboard.putString("Game Specific Message", gameData);
-		SmartDashboard.putString("Our Switch Location", (ourSwitch == 'L')? "Left" : "Right");
-		SmartDashboard.putString("Scale Location", (scale == 'L')? "Left" : "Right");
-		SmartDashboard.putString("Opponent Switch Location", (opponentSwitch == 'L')? "Left" : "Right");
-		
-		SmartDashboard.putString("Robot Position", (robotPosition == 'L')? "Left" : (robotPosition == 'R')? "Right" : "Center");
-		SmartDashboard.putString("Auto Routine Chosen", autoType.getFileName());
-		SmartDashboard.putString("Gripper Height", height.toString());
+	private static SrxTrajectory getPath(AutoType autoType, boolean flipped) {
+		switch (autoType) {
+			case CENTER_SWITCH:
+				return new CenterSwitch();
+			case OPPOSITE_SIDE_SCALE:
+				return new OppositeSideScale(flipped);
+			case SAME_SIDE_SCALE:
+				return new SameSideScale(flipped);
+			case SAME_SIDE_SWITCH:
+				return new SameSideSwitch(flipped);
+			default:
+				break;
+		}
+		return null;
 	}
 }

Diff for: jackson-databind-2.9.4.jar

@@ -1,25 +1,27 @@
 package org.usfirst.frc.team2363.robot.commands.autonomous;
 
+import org.usfirst.frc.team319.paths.SameSideScale;
+import org.usfirst.frc.team319.paths.SameSideScalePart2;
+import org.usfirst.frc.team319.paths.SameSideScalePart3;
+import org.usfirst.frc.team319.paths.SameSideScalePart4;
+import org.usfirst.frc.team319.paths.SameSideScalePart5;
 import org.usfirst.frc.team319.robot.commands.FollowTrajectory;
-import org.usfirst.frc.team319.utils.SrxTrajectoryImporter;
 
 import edu.wpi.first.wpilibj.command.CommandGroup;
 /**
  *
  */
 public class PathTesting extends CommandGroup {
 
-	private static final SrxTrajectoryImporter importer = new SrxTrajectoryImporter("/home/lvuser/Autos");
-
     public PathTesting() {
         try {
 //        	addSequential(new FollowTrajectory(importer.importSrxTrajectory("OppositeSideScale"), false));
 //        	addSequential(new FollowTrajectory(importer.importSrxTrajectory("OppositeSideScalePart2"), false));
-			addSequential(new FollowTrajectory(importer.importSrxTrajectory("SameSideScale"), false));
-			addSequential(new FollowTrajectory(importer.importSrxTrajectory("SameSideScalePart2"), false));
-			addSequential(new FollowTrajectory(importer.importSrxTrajectory("SameSideScalePart3"), false));
-			addSequential(new FollowTrajectory(importer.importSrxTrajectory("SameSideScalePart4"), false));
-			addSequential(new FollowTrajectory(importer.importSrxTrajectory("SameSideScalePart5"), false));
+			addSequential(new FollowTrajectory(new SameSideScale()));
+			addSequential(new FollowTrajectory(new SameSideScalePart2()));
+			addSequential(new FollowTrajectory(new SameSideScalePart3()));
+			addSequential(new FollowTrajectory(new SameSideScalePart4()));
+			addSequential(new FollowTrajectory(new SameSideScalePart5()));
 		} catch (Exception e) {
 			e.printStackTrace();
 		}

Diff for: json-simple-1.1.jar

@@ -6,6 +6,8 @@
 public class SrxTrajectory {
 	public SrxMotionProfile leftProfile;
 	public SrxMotionProfile rightProfile;
+	
+	public SrxTrajectory() { }
 
 	public SrxTrajectory(SrxMotionProfile left, SrxMotionProfile right) {
 		this.leftProfile = left;