Merge branch 'develop' into develop

wh1ter0se · Feb 14, 2018 · fabe715 · fabe715
2 parents 777ebd5 + 38a58d1
commit fabe715
Show file tree

Hide file tree

Showing 4 changed files with 74 additions and 23 deletions.
diff --git a/src/org/usfirst/frc/team3695/robot/Constants.java b/src/org/usfirst/frc/team3695/robot/Constants.java
@@ -67,6 +67,19 @@ public class Constants {
 
 			HOOK_MOTOR_INVERT		  = false;
 
+	public static class Autonomous {
+		//Save distances in inches
+		public static final int DIST_TO_SWITCH_FROM_SIDE = 168;
+		public static final int DIST_PASS_PORTAL = 36;
+		public static final int DIST_CENTER_LINE_SWITCH_ALIGN = 48;
+		public static final int DIST_BLOCKS_TO_SWITCH = 42;
+		public static final int DIST_WALL_TO_LINE = 120;
+		public static final int DIST_WALL_TO_BLOCKS = 98;
+
+		public static final int ROT_90_CLOCKWISE = 90;
+		public static final int ROT_90_COUNTERCLOCKWISE = -90;
+		public static final int ROT_180 = 180;
+	}
 
 //	/** where all the static final variables for arduino get set */
 //	public static class ArduinoPatterns {

diff --git a/src/org/usfirst/frc/team3695/robot/auto/CommandGroupAuto.java b/src/org/usfirst/frc/team3695/robot/auto/CommandGroupAuto.java
@@ -1,64 +1,103 @@
 package org.usfirst.frc.team3695.robot.auto;
 
+import edu.wpi.first.wpilibj.DriverStation;
+import org.usfirst.frc.team3695.robot.Constants;
+import org.usfirst.frc.team3695.robot.Robot;
 import org.usfirst.frc.team3695.robot.commands.*; // when the commands are ready, load each individually to decrease runtime
-import org.usfirst.frc.team3695.robot.enumeration.Autonomous;
 import org.usfirst.frc.team3695.robot.enumeration.Goal;
 import org.usfirst.frc.team3695.robot.enumeration.Position;
 
 import edu.wpi.first.wpilibj.command.CommandGroup;
 
 /** the sequence of commands for autonomous */
 public class CommandGroupAuto extends CommandGroup {
-
+
+	//Stores the states of the switches and scale
+	String gameData;
+
 	public CommandGroupAuto(Position position, Goal goal) {
+		//Get the state of the switches and scale for each round
+		gameData = DriverStation.getInstance().getGameSpecificMessage();
+
 		// make sure everything is in the right state/position up here
+		Robot.SUB_CLAMP.closeArms();
 		// EX: making sure flap is closed before auto starts
-
 		switch (position) {
 			case LEFT:
 				switch (goal){
 					case RUN_FOR_IT:
+						addSequential(new CyborgCommandDriveUntilError(Position.FORWARD));
 						break;
-					case SWITCH_LEFT:
+					case SWITCH:
+						if (gameData.charAt(0) == 'L'){ //When the switch is on the left
+							addParallel(new CyborgCommandDriveDistance(Constants.Autonomous.DIST_TO_SWITCH_FROM_SIDE));
+							addSequential(new CyborgCommandGoToMid());
+							addSequential(new CyborgCommandRotateDegrees(Constants.Autonomous.ROT_90_CLOCKWISE));
+							addSequential(new ButtonCommandSpit());
+						} else { //When the switch is on the right
+
+						}
 						break;
-					case SWITCH_RIGHT:
+					case ENEMY_SWITCH:
 						break;
-					case SCALE_LEFT:
+					case SCALE:
 						break;
-					case SCALE_RIGHT:
+					case BEST_OPTION:
 						break;
 				}
 				break;
-				
+
 			case CENTER:
 				switch (goal){
 					case RUN_FOR_IT:
+						addSequential(new CyborgCommandDriveUntilError(Position.FORWARD));
 						break;
-					case SWITCH_LEFT:
+					case SWITCH:
+						//Raise the mast to midpoint and pass the portal to avoid collisions
+						addParallel(new CyborgCommandGoToMid());
+						addSequential(new CyborgCommandDriveDistance(Constants.Autonomous.DIST_PASS_PORTAL));
+						if (gameData.charAt(0) == 'L'){ //When the switch is on the left
+							addSequential(new CyborgCommandRotateDegrees(Constants.Autonomous.ROT_90_COUNTERCLOCKWISE));
+							addSequential(new CyborgCommandDriveDistance(Constants.Autonomous.DIST_CENTER_LINE_SWITCH_ALIGN));
+							addSequential(new CyborgCommandRotateDegrees(Constants.Autonomous.ROT_90_CLOCKWISE));
+							addSequential(new CyborgCommandDriveDistance(Constants.Autonomous.DIST_WALL_TO_BLOCKS
+																				+ Constants.Autonomous.DIST_BLOCKS_TO_SWITCH
+																				- Constants.Autonomous.DIST_PASS_PORTAL));
+							addSequential(new ButtonCommandSpit());
+
+						} else { //When the switch is on the right
+							addSequential(new CyborgCommandRotateDegrees(Constants.Autonomous.ROT_90_CLOCKWISE));
+							addSequential(new CyborgCommandDriveDistance(Constants.Autonomous.DIST_CENTER_LINE_SWITCH_ALIGN));
+							addSequential(new CyborgCommandRotateDegrees(Constants.Autonomous.ROT_90_COUNTERCLOCKWISE));
+							addSequential(new CyborgCommandDriveDistance(Constants.Autonomous.DIST_WALL_TO_BLOCKS
+																				+ Constants.Autonomous.DIST_BLOCKS_TO_SWITCH
+																				- Constants.Autonomous.DIST_PASS_PORTAL));
+							addSequential(new ButtonCommandSpit());
+						}
 						break;
-					case SWITCH_RIGHT:
+					case ENEMY_SWITCH:
 						break;
-					case SCALE_LEFT:
+					case SCALE:
 						break;
-					case SCALE_RIGHT:
+					case BEST_OPTION:
 						break;
 			}
 				break;
 
 			case RIGHT:
-				switch (goal){
+				switch (goal) {
 					case RUN_FOR_IT:
+						addSequential(new CyborgCommandDriveUntilError(Position.FORWARD));
 						break;
-					case SWITCH_LEFT:
+					case SWITCH:
 						break;
-					case SWITCH_RIGHT:
+					case ENEMY_SWITCH:
 						break;
-					case SCALE_LEFT:
+					case SCALE:
 						break;
-					case SCALE_RIGHT:
+					case BEST_OPTION:
 						break;
 				}
-
 			break;
 		}	
 	}

diff --git a/src/org/usfirst/frc/team3695/robot/enumeration/Goal.java b/src/org/usfirst/frc/team3695/robot/enumeration/Goal.java
@@ -3,10 +3,10 @@
 public enum Goal {
 	NOTHING("Do nothing"),
 	RUN_FOR_IT("Run for it"),
-	SWITCH_LEFT("Switch - Left Side"),
-	SWITCH_RIGHT("Switch - Right Side"),
-	SCALE_LEFT("Scale - Left"),
-	SCALE_RIGHT("Scale - Right");
+	SWITCH("Switch"),
+	ENEMY_SWITCH("Enemy Switch"),
+	SCALE("Scale"),
+	BEST_OPTION("Robot Choice");
 
 	private final String name;
 	Goal(String name) {

diff --git a/src/org/usfirst/frc/team3695/robot/subsystems/SubsystemDrive.java b/src/org/usfirst/frc/team3695/robot/subsystems/SubsystemDrive.java
@@ -225,4 +225,3 @@ public void reset() {
         rightMaster.setSelectedSensorPosition(0, Constants.RIGHT_PID, Constants.TIMEOUT_PID);
     }
 }
-
Original file line number	Diff line number	Diff line change
Expand Up		@@ -225,4 +225,3 @@ public void reset() {
		rightMaster.setSelectedSensorPosition(0, Constants.RIGHT_PID, Constants.TIMEOUT_PID);
		}
		}