Auto testing

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

@@ -86,7 +86,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