Auto testing (#82)

* address issues #44 and 45

change problems in script parsing from warning to error and allow
coordinate points with spaces between

* use replaceAll instead of for loop

* modify changing and getting AutoUtils values

PIDTurn and PIDMove can now move to an absolute value and they also
update the position and angle based on encoder and gyro values instead
of the assumed values

* add documentation

* use absolute values in AutoMoveTo

* modify RunScript to use absolute values

* fix what alex fixed in his code

* modify atan to atan2

* modify atan2 to pass in the right values

* initial ds computer changes (#1)

* added more functionality to intakeEject

* add requires to default intake command

* add end and interrupted to defaultIntake

* changes to make intake work

* start testing auto

* use sd interface instead of sd in PIDMove and PIDTurn

* testing auto changes

relative vs absolute = problems
moved math in PIDMove and PIDTurn to initialize instead of in the
constructor
added try catch for manipulator in DefaultIntake command
(Daniel P, Kevin W, Laura M, Dean B)

* fixed PIDMove and other tiny random things

PIDMove was calculating as if numbers were normal angles when in fact
they are bearings so we switched x and y

* fix AutoMoveTo test

Author: kevinzwang

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
+#Tue Feb 20 09:44:13 PST 2018
 C\:\\Users\\dean\\git\\RobotCode2018\\Robot2018\\lib\\libpathfinderjava.so=4087ee38c338446b8305f67716260df4

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

@@ -90,7 +90,7 @@ public OI() {
 		MoveLiftUpButton.whileHeld(new RunLift(Robot.lift, true));
 		MoveLiftDownButton.whileHeld(new RunLift(Robot.lift, false));
 
-		// manipulator = new Joystick(2);
+		manipulator = new Joystick(2);
 		// closeIntake = new JoystickButton(manipulator, getButton("Close Intake
 		// Button", 1));
 		// closeIntake.whenPressed(new CloseIntake());

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

@@ -12,8 +12,8 @@
 import java.util.Map;
 
 import org.usfirst.frc.team199.Robot2018.autonomous.AutoUtils;
+import org.usfirst.frc.team199.Robot2018.autonomous.Position;
 import org.usfirst.frc.team199.Robot2018.commands.Autonomous;
-import org.usfirst.frc.team199.Robot2018.commands.Autonomous.Position;
 import org.usfirst.frc.team199.Robot2018.commands.Autonomous.Strategy;
 import org.usfirst.frc.team199.Robot2018.commands.ShiftLowGear;
 import org.usfirst.frc.team199.Robot2018.subsystems.Climber;
@@ -22,9 +22,9 @@
 import org.usfirst.frc.team199.Robot2018.subsystems.IntakeEject;
 import org.usfirst.frc.team199.Robot2018.subsystems.Lift;
 
-import edu.wpi.first.wpilibj.CameraServer;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.IterativeRobot;
+import edu.wpi.first.wpilibj.PIDController;
 import edu.wpi.first.wpilibj.Preferences;
 import edu.wpi.first.wpilibj.command.Command;
 import edu.wpi.first.wpilibj.command.Scheduler;
@@ -53,7 +53,7 @@ public class Robot extends IterativeRobot {
 	public static Map<String, ArrayList<String[]>> autoScripts;
 
 	Command autonomousCommand;
-	SendableChooser<Position> posChooser = new SendableChooser<Position>();
+	SendableChooser<Autonomous.Position> posChooser = new SendableChooser<Autonomous.Position>();
 	Map<String, SendableChooser<Strategy>> stratChoosers = new HashMap<String, SendableChooser<Strategy>>();
 	String[] fmsPossibilities = { "LL", "LR", "RL", "RR" };
 
@@ -69,6 +69,14 @@ public double getConst(String key, double def) {
 			}
 			return pref.getDouble("Const/" + key, def);
 		}
+
+		public void putData(String string, PIDController controller) {
+			SmartDashboard.putData(string, controller);
+		}
+
+		public void putNumber(String string, double d) {
+			SmartDashboard.putNumber(string, d);
+		}
 		/*
 		 * if (!SmartDashboard.containsKey("Const/" + key)) { if
 		 * (!SmartDashboard.putNumber("Const/" + key, def)) {
@@ -109,7 +117,7 @@ public void robotInit() {
 		oi = new OI();
 
 		// auto position chooser
-		for (Position p : Position.values()) {
+		for (Autonomous.Position p : Autonomous.Position.values()) {
 			posChooser.addObject(p.getSDName(), p);
 		}
 		SmartDashboard.putData("Starting Position", posChooser);
@@ -131,8 +139,8 @@ public void robotInit() {
 		autoScripts = AutoUtils.parseScriptFile(Preferences.getInstance().getString("autoscripts", ""));
 
 		listen = new Listener();
-		CameraServer.getInstance().startAutomaticCapture(0);
-		CameraServer.getInstance().startAutomaticCapture(1);
+		// CameraServer.getInstance().startAutomaticCapture(0);
+		// CameraServer.getInstance().startAutomaticCapture(1);
 	}
 
 	/**
@@ -157,9 +165,11 @@ public void disabledPeriodic() {
 	 */
 	@Override
 	public void autonomousInit() {
+		dt.resetAHRS();
+		AutoUtils.position = new Position(0, 0, 0);
 		Scheduler.getInstance().add(new ShiftLowGear());
 		String fmsInput = DriverStation.getInstance().getGameSpecificMessage();
-		Position startPos = posChooser.getSelected();
+		Autonomous.Position startPos = posChooser.getSelected();
 		double autoDelay = SmartDashboard.getNumber("Auto Delay", 0);
 
 		Map<String, Strategy> strategies = new HashMap<String, Strategy>();
@@ -169,8 +179,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/SmartDashboardInterface.java

@@ -1,5 +1,11 @@
 package org.usfirst.frc.team199.Robot2018;
 
+import edu.wpi.first.wpilibj.PIDController;
+
 public interface SmartDashboardInterface {
 	public double getConst(String key, double def);
+
+	public void putData(String string, PIDController controller);
+
+	public void putNumber(String string, double d);
 }

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

@@ -6,7 +6,7 @@
 
 public class AutoUtils {
 
-	public static Position position = new Position(0, 0, 0);
+	public static Position position;
 
 	/**
 	 * Parses the inputted script file into a map of scripts
@@ -24,23 +24,25 @@ public static Map<String, ArrayList<String[]>> parseScriptFile(String scriptFile
 		ArrayList<String[]> currScript = new ArrayList<String[]>();
 		String currScriptName = "";
 
-		int count = 1;
+		int count = 0;
+		int errorCount = 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);
 			}
@@ -74,9 +76,11 @@ public static Map<String, ArrayList<String[]>> parseScriptFile(String scriptFile
 				if (isValidCommand(instruction, args, count)) {
 					String[] command = { instruction, args };
 					currScript.add(command);
+				} else {
+					errorCount++;
 				}
 			}
-			count++;
+
 		}
 
 		// puts the last script in
@@ -85,6 +89,8 @@ public static Map<String, ArrayList<String[]>> parseScriptFile(String scriptFile
 		// remove the stray one in the beginning
 		autoScripts.remove("");
 
+		System.out.println("[INFO] Successfuly parsed " + count + " lines with " + errorCount + " errors.");
+
 		return autoScripts;
 	}
 
@@ -187,7 +193,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);
 	}
 
@@ -243,16 +249,16 @@ public static double[] parsePoint(String cmdArgs) {
 		double[] point = new double[2];
 		String parentheseless;
 		String[] pointparts;
-		if (AutoUtils.isPoint(cmdArgs)) {
+		if (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/autonomous/scriptupload/scriptupload.py

@@ -1,6 +1,7 @@
 #nt setup
 from networktables import NetworkTables
 import __future__
+import time
 NetworkTables.initialize(server='10.1.99.2')
 prefs = NetworkTables.getTable("Preferences")
 
@@ -30,6 +31,8 @@
 prefs.putString("autoscripts", oneline)
 print("Uploading %s as a String[] to key \"autoscripts\"" % filename)
 
+time.sleep(0.1)
+
 #checks if the key has been filled
 tester = "" #a variable to check if autoscripts is None