resolve merge conflicts

;)

Author: Gabe-Mitnick

AAAScripts/scripts.txt

@@ -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/lib/User_Libraries.properties

@@ -94,19 +94,23 @@ public OI() {
 		moveLiftDownButton = new JoystickButton(rightJoy, getButton("Run Lift Motor Down", 11));
 		moveLiftUpButton.whileHeld(new RunLift(Robot.lift, true));
 		moveLiftDownButton.whileHeld(new RunLift(Robot.lift, false));
-
-// 		manipulator = new Joystick(2);
-// 		closeIntakeButton = new JoystickButton(manipulator, getButton("Close Intake Button", 1));
-// 		closeIntakeButton.whenPressed(new CloseIntake());
-// 		openIntakeButton = new JoystickButton(manipulator, getButton("Open Intake Button", 2));
-// 		openIntakeButton.whenPressed(new OpenIntake());
-// 		raiseIntakeButton = new JoystickButton(manipulator, getButton("Raise Intake Button", 3));
-// 		raiseIntakeButton.whenPressed(new RaiseIntake());
-// 		lowerIntakeButton = new JoystickButton(manipulator, getButton("Lower Intake Button", 4));
-// 		lowerIntakeButton.whenPressed(new LowerIntake());
-// 		intakeCubeButton = new JoystickButton(manipulator, getButton("Intake Button", 5));
-// 		intakeCubeButton.whenPressed(new IntakeCube());
-// 		outtakeCubeButton = new JoystickButton(manipulator, getButton("Outtake Button", 6));
-// 		outtakeCubeButton.whenPressed(new OuttakeCube());
+		
+		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());
 	}
 }

Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.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/Robot.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/RobotMap.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/SmartDashboardInterface.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/AutoUtils.java

@@ -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
 

Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/scriptupload/scriptupload.py

@@ -17,39 +17,26 @@
  */
 public class AutoMoveTo extends CommandGroup {
 
-	public AutoMoveTo(String[] args, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource pidMoveSrc) {
+	public AutoMoveTo(String[] args, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource pidMoveSrc,
+			PIDSource pidTurnSource) {
 		// requires(Drivetrain);
 		double rotation;
 		double[] point = { 0, 0 };
 		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, pidTurnSource, 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, pidTurnSource));
+				addSequential(new PIDMove(point, dt, sd, pidMoveSrc));
 			} else {
 				throw new IllegalArgumentException();
 			}
 		}
 	}
 
 	public AutoMoveTo(String[] args) {
-		this(args, Robot.dt, Robot.sd, Robot.dt.getDistEncAvg());
+		this(args, Robot.dt, Robot.sd, Robot.dt.getDistEncAvg(), Robot.dt.getGyro());
 	}
 }