Add toggle functionality to the left and right intake

AAAScripts/scripts.txt

@@ -36,7 +36,7 @@ RxLx:
 #-------------------------------------------------
 
 #Exchange
-RxxE
+RxxE:
 	moveto (0, 188.4) (-210.6, 188.4) (-210.6, 44.4) (-123.6, 44.4) (-123.6, -39.6) 180 #Curve around switch for baseline #then go to exchange
 	exchange
 
@@ -144,7 +144,7 @@ LxRx:
 #-------------------------------------------------
 
 #Exchange
-LxxE
+LxxE:
 	moveto (0, 188.4) (210.6, 188.4) (210.6, 3.7) (124.8, 3.7) (124.8, -3.3) 180 #Curve around switch for baseline then go to exchange
 	exchange
 
@@ -244,13 +244,13 @@ moveto (48,80.375)  #cross baseline
 CRxx:
 moveto (0,60) 90 #5 feet forward, turn 90
 moveto (48,60) -90 #4 feet right,turn -90
-moveto(48,104.375) #7 feet forward until against switch enclosure, compensating for robot length
+moveto (48,104.375) #7 feet forward until against switch enclosure, compensating for robot length
 switch #deploy switch 
 
 CLxx:
 moveto (0,60) -90 #5 feet forward, turn -90
 moveto (-48,60) 90 #4 feet left,turn 90
-moveto(-48,104.375) #7 feet forward until against switch enclosure,compensating for robot length
+moveto (-48,104.375) #7 feet forward until against switch enclosure,compensating for robot length
 switch #deploy switch 
 
 
@@ -275,7 +275,7 @@ scale #deploy scale
 CRLx:
 moveto (0,60) 90 #5 feet forward, turn 90
 moveto (48,60) -90 #4 feet right,turn -90
-moveto(48,104.375) #7 feet forward until against switch enclosure
+moveto (48,104.375) #7 feet forward until against switch enclosure
 switch #deploy switch 
 move -39 #move 39 inches back
 turn -90 #turn -90 degrees
@@ -294,7 +294,7 @@ scale #deploy scale2
 CRRx:
 moveto (0,60) 90 #5 feet forward, turn 90
 moveto (48,60) -90 #4 feet right,turn -90
-moveto(48,104.375) #7 feet forward until against switch enclosure
+moveto (48,104.375) #7 feet forward until against switch enclosure
 switch #deploy switch 
 move -39 #move 39 inches back
 turn -90 #turn -90 degrees
@@ -313,7 +313,7 @@ scale #deploy scale
 CLRx:
 moveto (0,60) -90 #5 feet forward, turn -90
 moveto (-48,60) 90 #4 feet left,turn 90
-moveto(-48,104.375) #7 feet forward until against switch enclosure
+moveto (-48,104.375) #7 feet forward until against switch enclosure
 switch #deploy switch 
 move -39 #move 39 inches back
 turn -90 #turn -90 degrees
@@ -331,7 +331,7 @@ scale #deploy scale
 CLLx:
 moveto (0,60) -90 #5 feet forward, turn -90
 moveto (-48,60) 90 #4 feet left,turn 90
-moveto(-48,104.375) #7 feet forward until against switch enclosure
+moveto (-48,104.375) #7 feet forward until against switch enclosure
 switch #deploy switch 
 move -39 #move 39 inches back
 turn 90 #turn 90 degrees
@@ -352,7 +352,7 @@ scale #deploy scale2
 CRxE:
 moveto (0,60) 90 #5 feet forward, turn 90
 moveto (48,60) -90 #4 feet right,turn -90
-moveto(48,104.375) #7 feet forward until against switch enclosure, compensating for robot length
+moveto (48,104.375) #7 feet forward until against switch enclosure, compensating for robot length
 switch #deploy switch 
 move -24 # 2 feet back
 turn -90
@@ -368,7 +368,7 @@ exchange
 CLxE:
 moveto (0,60) -90 #5 feet forward, turn -90
 moveto (-48,60) 90 #3 feet left,turn 90
-moveto(-48,104.375) #7 feet forward until against switch enclosure,compensating for robot length
+moveto (-48,104.375) #7 feet forward until against switch enclosure,compensating for robot length
 switch #deploy switch
 move -24
 turn 90
@@ -383,7 +383,7 @@ exchange
 CxxE:
 moveto (0,60) -90 #5 feet forward, turn -90
 moveto (-48,60) 90 #3 feet left,turn 90
-moveto(-48,104.375) #7 feet forward until against switch enclosure,compensating for robot length
+moveto (-48,104.375) #7 feet forward until against switch enclosure,compensating for robot length
 move -96 #move 8 feet back
 turn 180 #turn 180 degrees to face the exchange
 exchange

Robot2018/lib/User_Libraries.properties

@@ -1,2 +1,2 @@
-#Sun Feb 18 15:40:25 PST 2018
+#Sun Feb 18 17:40:43 PST 2018
 C\:\\Users\\dean\\git\\RobotCode2018\\Robot2018\\lib\\libpathfinderjava.so=4087ee38c338446b8305f67716260df4

Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java

@@ -7,6 +7,11 @@
 
 package org.usfirst.frc.team199.Robot2018;
 
+import org.usfirst.frc.team199.Robot2018.commands.CloseIntake;
+import org.usfirst.frc.team199.Robot2018.commands.IntakeCube;
+import org.usfirst.frc.team199.Robot2018.commands.LowerIntake;
+import org.usfirst.frc.team199.Robot2018.commands.OpenIntake;
+import org.usfirst.frc.team199.Robot2018.commands.OutakeCube;
 import org.usfirst.frc.team199.Robot2018.commands.PIDMove;
 import org.usfirst.frc.team199.Robot2018.commands.PIDTurn;
 import org.usfirst.frc.team199.Robot2018.commands.ResetEncoders;
@@ -15,6 +20,8 @@
 import org.usfirst.frc.team199.Robot2018.commands.ShiftDriveType;
 import org.usfirst.frc.team199.Robot2018.commands.ShiftHighGear;
 import org.usfirst.frc.team199.Robot2018.commands.ShiftLowGear;
+import org.usfirst.frc.team199.Robot2018.commands.ToggleLeftIntake;
+import org.usfirst.frc.team199.Robot2018.commands.ToggleRightIntake;
 import org.usfirst.frc.team199.Robot2018.commands.UpdatePIDConstants;
 
 import edu.wpi.first.wpilibj.Joystick;
@@ -50,6 +57,8 @@ public class OI {
 	private JoystickButton lowerIntake;
 	private JoystickButton intake;
 	private JoystickButton outake;
+	private JoystickButton toggleLeftIntake;
+	private JoystickButton toggleRightIntake;
 
 	public int getButton(String key, int def) {
 		if (!SmartDashboard.containsKey("Button/" + key)) {
@@ -90,22 +99,24 @@ public OI() {
 		MoveLiftUpButton.whileHeld(new RunLift(Robot.lift, true));
 		MoveLiftDownButton.whileHeld(new RunLift(Robot.lift, false));
 
-		// manipulator = new Joystick(2);
-		// closeIntake = new JoystickButton(manipulator, getButton("Close Intake
-		// Button", 1));
-		// closeIntake.whenPressed(new CloseIntake());
-		// openIntake = new JoystickButton(manipulator, getButton("Open Intake Button",
-		// 2));
-		// openIntake.whenPressed(new OpenIntake());
+		manipulator = new Joystick(2);
+		closeIntake = new JoystickButton(manipulator, getButton("Close Intake Button", 1));
+		closeIntake.whenPressed(new CloseIntake());
+		openIntake = new JoystickButton(manipulator, getButton("Open Intake Button", 2));
+		openIntake.whenPressed(new OpenIntake());
 		// raiseIntake = new JoystickButton(manipulator, getButton("Raise Intake
 		// Button", 3));
 		// raiseIntake.whenPressed(new RaiseIntake());
 		// lowerIntake = new JoystickButton(manipulator, getButton("Lower Intake
 		// Button", 4));
-		// lowerIntake.whenPressed(new LowerIntake());
-		// intake = new JoystickButton(manipulator, getButton("Intake Button", 5));
-		// intake.whenPressed(new IntakeCube());
-		// outake = new JoystickButton(manipulator, getButton("Outake Button", 6));
-		// outake.whenPressed(new OutakeCube());
+		lowerIntake.whenPressed(new LowerIntake());
+		intake = new JoystickButton(manipulator, getButton("Intake Button", 5));
+		intake.whenPressed(new IntakeCube());
+		outake = new JoystickButton(manipulator, getButton("Outake Button", 6));
+		outake.whenPressed(new OutakeCube());
+		toggleLeftIntake = new JoystickButton(manipulator, getButton("Toggle Left Intake Button", 3));
+		toggleLeftIntake.whenPressed(new ToggleLeftIntake());
+		toggleRightIntake = new JoystickButton(manipulator, getButton("Toggle Right Intake Button", 4));
+		toggleRightIntake.whenPressed(new ToggleRightIntake());
 	}
 }

Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java

@@ -169,8 +169,8 @@ public void autonomousInit() {
 			strategies.put(key, chooser.getSelected());
 		}
 
-		Autonomous auto = new Autonomous(startPos, strategies, autoDelay, fmsInput, false);
-		auto.start();
+		Scheduler.getInstance().add(new Autonomous(startPos, strategies, autoDelay, fmsInput, false));
+		// auto.start();
 	}
 
 	/**

Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java

@@ -130,12 +130,10 @@ public RobotMap() {
 
 		leftIntakeMotor = new VictorSP(getPort("IntakeLeftVictorSP", 9));
 		rightIntakeMotor = new VictorSP(getPort("IntakeRightVictorSP", 8));
-		// leftIntakeHorizontalSolenoid = new
-		// DoubleSolenoid(getPort("IntakeLeftHorizontalSolenoidPort1", 2),
-		// getPort("IntakeLeftHorizontalSolenoidPort2", 3));
-		// rightIntakeHorizontalSolenoid = new
-		// DoubleSolenoid(getPort("IntakeRightHorizontalSolenoidPort1", 4),
-		// getPort("IntakeRightHorizontalSolenoidPort2", 5));
+		leftIntakeHorizontalSolenoid = new DoubleSolenoid(getPort("IntakeLeftHorizontalSolenoidPort1", 2),
+				getPort("IntakeLeftHorizontalSolenoidPort2", 3));
+		rightIntakeHorizontalSolenoid = new DoubleSolenoid(getPort("IntakeRightHorizontalSolenoidPort1", 4),
+				getPort("IntakeRightHorizontalSolenoidPort2", 5));
 		// leftIntakeVerticalSolenoid = new
 		// DoubleSolenoid(getPort("IntakeLeftVerticalSolenoidPort1", 6),
 		// getPort("IntakeLeftVerticalSolenoidPort2", 7));

Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/AutoUtils.java

@@ -24,23 +24,24 @@ public static Map<String, ArrayList<String[]>> parseScriptFile(String scriptFile
 		ArrayList<String[]> currScript = new ArrayList<String[]>();
 		String currScriptName = "";
 
-		int count = 1;
+		int count = 0;
 		for (String line : lines) {
+			count++;
 			// remove comments
 			int commentIndex = line.indexOf("#");
 			if (commentIndex != -1)
 				line = line.substring(0, commentIndex);
 
 			// trim and remove extra whitespace just to make it neater
-			line = line.trim().replaceAll("\\s+", " ");	
+			line = line.trim().replaceAll("\\s+", " ");
 			// make coordinates with spaces also work
 			int parenIndex = line.indexOf("(");
 			while (parenIndex != -1) {
 				// removes all spaces between the parentheses
 				int endParenIndex = line.indexOf(")", parenIndex);
 				String coord = line.substring(parenIndex + 1, endParenIndex);
 				line = line.substring(0, parenIndex + 1) + coord.replaceAll(" ", "") + line.substring(endParenIndex);
-				
+
 				// finds next parentheses
 				parenIndex = line.indexOf("(", parenIndex + 1);
 			}
@@ -76,7 +77,7 @@ public static Map<String, ArrayList<String[]>> parseScriptFile(String scriptFile
 					currScript.add(command);
 				}
 			}
-			count++;
+
 		}
 
 		// puts the last script in
@@ -187,7 +188,7 @@ else if (instruction.equals("jump")) {
 	 *            the message to log
 	 */
 
-	private static void logWarning (int lineNumber, String message) {
+	private static void logWarning(int lineNumber, String message) {
 		System.err.println("[ERROR] Line " + lineNumber + ": " + message);
 	}
 
@@ -246,13 +247,13 @@ public static double[] parsePoint(String cmdArgs) {
 		if (AutoUtils.isPoint(cmdArgs)) {
 			parentheseless = cmdArgs.substring(1, cmdArgs.length() - 1);
 			pointparts = parentheseless.split(",");
-            try {
-			    point[0] = Double.parseDouble(pointparts[0]);
-			    point[1] = Double.parseDouble(pointparts[1]);
-            } catch (Exception e) {
-                point[0] = 1;
-                point[1] = 1;
-            }
+			try {
+				point[0] = Double.parseDouble(pointparts[0]);
+				point[1] = Double.parseDouble(pointparts[1]);
+			} catch (Exception e) {
+				point[0] = 1;
+				point[1] = 1;
+			}
 		}
 		return point;
 	}

Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoMoveTo.java

@@ -24,25 +24,11 @@ public AutoMoveTo(String[] args, DrivetrainInterface dt, SmartDashboardInterface
 		for (String arg : args) {
 			if (AutoUtils.isDouble(arg)) {
 				rotation = Double.valueOf(arg);
-				double relrotation = rotation - AutoUtils.position.getRot();
-				addSequential(new PIDTurn(relrotation, dt, sd, pidMoveSrc));
-				AutoUtils.position.setRot(rotation);
+				addSequential(new PIDTurn(rotation, dt, sd, pidMoveSrc, true));
 			} else if (AutoUtils.isPoint(arg)) {
 				point = AutoUtils.parsePoint(arg);
-				double deltaX = point[0] - AutoUtils.position.getX();
-				double deltaY = point[1] - AutoUtils.position.getY();
-				double atan = Math.toDegrees(Math.atan(deltaX / deltaY));
-				double relrotation = atan - AutoUtils.position.getRot();
-				addSequential(new PIDTurn(relrotation, dt, sd, pidMoveSrc));
-				double dX2 = deltaX * deltaX;
-				double dY2 = deltaY * deltaY;
-				double distance = Math.sqrt(dX2 + dY2);
-				addSequential(new PIDMove(distance, dt, sd, pidMoveSrc));
-				double x = AutoUtils.position.getX();
-				double y = AutoUtils.position.getY();
-				AutoUtils.position.setX(point[0]);
-				AutoUtils.position.setY(point[1]);
-				AutoUtils.position.setRot(Math.toDegrees(Math.atan((point[0] - x) / (point[1] - y))));
+				addSequential(new PIDTurn(point, dt, sd, pidMoveSrc));
+				addSequential(new PIDMove(point, dt, sd, pidMoveSrc));
 			} else {
 				throw new IllegalArgumentException();
 			}

Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/IntakeCube.java

@@ -20,7 +20,7 @@ protected void initialize() {
 
 	// Called repeatedly when this Command is scheduled to run
 	protected void execute() {
-		Robot.intakeEject.runIntake(1);
+		Robot.intakeEject.runIntake(-1);
 	}
 
 	// Make this return true when this Command no longer needs to run execute()