Merge branch 'master' of

https://github.com/DeepBlueRobotics/RobotCode2018.git

Conflicts:
	Robot2018/lib/User_Libraries.properties
	Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/AutoUtils.java
	Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoMoveTo.java
	Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/DefaultIntake.java
	Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java
	Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java

Author: DriverStationComputer

Robot2018/lib/User_Libraries.properties

@@ -1,2 +1,2 @@
-#Sun Feb 18 17:40:43 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

@@ -26,6 +26,10 @@
  * interface to the commands and command groups that allow control of the robot.
  */
 public class OI {
+	/*
+	 * WHENEVER YOU ADD OR CHANGE WHAT A BUTTON OR JOYSTICK DOES, indicate in
+	 * /docs/controllers.txt to keep that reference up to date.
+	 */
 
 	public Joystick leftJoy;
 	private JoystickButton shiftLowGearButton;
@@ -86,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,7 +22,6 @@
 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;
@@ -54,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" };
 
@@ -118,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);
@@ -140,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);
 	}
 
 	/**
@@ -166,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>();

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
@@ -25,6 +25,7 @@ public static Map<String, ArrayList<String[]>> parseScriptFile(String scriptFile
 		String currScriptName = "";
 
 		int count = 0;
+		int errorCount = 0;
 		for (String line : lines) {
 			count++;
 			// remove comments
@@ -75,6 +76,8 @@ public static Map<String, ArrayList<String[]>> parseScriptFile(String scriptFile
 				if (isValidCommand(instruction, args, count)) {
 					String[] command = { instruction, args };
 					currScript.add(command);
+				} else {
+					errorCount++;
 				}
 			}
 
@@ -86,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;
 	}
 
@@ -244,7 +249,7 @@ 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 {

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
 

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

@@ -17,17 +17,18 @@
  */
 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);
-				addSequential(new PIDTurn(rotation, dt, sd, pidMoveSrc, true));
+				addSequential(new PIDTurn(rotation, dt, sd, pidTurnSource, true));
 			} else if (AutoUtils.isPoint(arg)) {
 				point = AutoUtils.parsePoint(arg);
-				addSequential(new PIDTurn(point, dt, sd, pidMoveSrc));
+				addSequential(new PIDTurn(point, dt, sd, pidTurnSource));
 				addSequential(new PIDMove(point, dt, sd, pidMoveSrc));
 			} else {
 				throw new IllegalArgumentException();
@@ -36,6 +37,6 @@ public AutoMoveTo(String[] args, DrivetrainInterface dt, SmartDashboardInterface
 	}
 
 	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());
 	}
 }

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

@@ -9,6 +9,8 @@
  */
 public class DefaultIntake extends Command {
 
+	private boolean manipulatorPluggedIn = true;
+
 	public DefaultIntake() {
 		// Use requires() here to declare subsystem dependencies
 		// eg. requires(chassis);
@@ -17,13 +19,21 @@ public DefaultIntake() {
 
 	// Called just before this Command runs the first time
 	protected void initialize() {
+		try {
+			Robot.oi.manipulator.getRawAxis(1);
+		} catch (NullPointerException e) {
+			System.err.println("[ERROR] Manipulator not plugged in.");
+			manipulatorPluggedIn = false;
+		}
 	}
 
 	// Called repeatedly when this Command is scheduled to run
 	protected void execute() {
 		// 1 and 5 represent the axes' index in driver station
-		Robot.intakeEject.runLeftIntake(Robot.oi.manipulator.getRawAxis(1));
-		Robot.intakeEject.runRightIntake(Robot.oi.manipulator.getRawAxis(5));
+		if (manipulatorPluggedIn) {
+			Robot.intakeEject.runLeftIntake(Robot.oi.manipulator.getRawAxis(1));
+			Robot.intakeEject.runRightIntake(Robot.oi.manipulator.getRawAxis(5));
+		}
 	}
 
 	// Make this return true when this Command no longer needs to run execute()

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

@@ -21,6 +21,8 @@ public class PIDMove extends Command implements PIDOutput {
 	private PIDController moveController;
 	private PIDSource avg;
 	private SmartDashboardInterface sd;
+	private double pointX;
+	private double pointY;
 
 	/**
 	 * Constructs this command with a new PIDController. Sets all of the
@@ -71,12 +73,9 @@ public PIDMove(double targ, DrivetrainInterface dt, SmartDashboardInterface sd,
 	 *            the PIDSorceAverage of the DT's two Encoders
 	 */
 	public PIDMove(double[] point, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource avg) {
-		double dx = point[0] - AutoUtils.position.getX();
-		double dy = point[1] - AutoUtils.position.getY();
+		pointX = point[0];
+		pointY = point[1];
 
-		double dist = Math.sqrt(dx * dx + dy * dy); // pythagorean theorem to find distance
-
-		this.target = dist;
 		this.dt = dt;
 		this.avg = avg;
 		this.sd = sd;
@@ -86,6 +85,7 @@ public PIDMove(double[] point, DrivetrainInterface dt, SmartDashboardInterface s
 		double kf = 1 / (dt.getCurrentMaxSpeed() * sd.getConst("Default PID Update Time", 0.05));
 		moveController = new PIDController(sd.getConst("MovekP", 1), sd.getConst("MovekI", 0), sd.getConst("MovekD", 0),
 				kf, avg, this);
+		sd.putData("Move PID", moveController);
 	}
 
 	/**
@@ -94,6 +94,12 @@ public PIDMove(double[] point, DrivetrainInterface dt, SmartDashboardInterface s
 	 */
 	@Override
 	public void initialize() {
+		double dx = pointX - AutoUtils.position.getX();
+		double dy = pointY - AutoUtils.position.getY();
+
+		double dist = Math.sqrt(dx * dx + dy * dy); // pythagorean theorem to find distance
+		this.target = dist;
+
 		dt.resetDistEncs();
 		moveController.disable();
 		// input is in inches
@@ -102,6 +108,7 @@ public void initialize() {
 		moveController.setOutputRange(-1.0, 1.0);
 		moveController.setContinuous(false);
 		moveController.setAbsoluteTolerance(Robot.getConst("MoveTolerance", 0.1));
+		System.out.println("move target = " + target);
 		moveController.setSetpoint(target);
 
 		moveController.enable();
@@ -146,10 +153,13 @@ protected void end() {
 
 		double angle = Math.toRadians(AutoUtils.position.getRot());
 		double dist = avg.pidGet();
-		double x = Math.cos(angle) * dist;
-		double y = Math.sin(angle) * dist;
+		// x and y are switched because we are using bearings
+		double y = Math.cos(angle) * dist;
+		double x = Math.sin(angle) * dist;
 		AutoUtils.position.changeX(x);
 		AutoUtils.position.changeY(y);
+
+		AutoUtils.position.setRot(dt.getAHRSAngle());
 	}
 
 	/**

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

@@ -25,6 +25,10 @@ public class PIDTurn extends Command implements PIDOutput {
 	private Timer tim;
 	private double lastTime;
 	private SmartDashboardInterface sd;
+	private double pointX;
+	private double pointY;
+	private boolean turnToPoint;
+	private boolean absoluteRotation;
 
 	/**
 	 * Constructs this command with a new PIDController. Sets all of the
@@ -52,11 +56,9 @@ public PIDTurn(double targ, DrivetrainInterface dt, SmartDashboardInterface sd,
 		this.ahrs = ahrs;
 		this.sd = sd;
 
-		if (absolute) {
-			target = targ - AutoUtils.position.getRot();
-		} else {
-			target = targ;
-		}
+		turnToPoint = false;
+		target = targ;
+		absoluteRotation = absolute;
 
 		if (Robot.dt != null) {
 			requires(Robot.dt);
@@ -113,12 +115,12 @@ public PIDTurn(double targ, DrivetrainInterface dt, SmartDashboardInterface sd,
 	public PIDTurn(double[] point, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource ahrs) {
 		this.dt = dt;
 		this.ahrs = ahrs;
+		this.sd = sd;
 
-		double dx = point[0] - AutoUtils.position.getX();
-		double dy = point[1] - AutoUtils.position.getY();
+		pointX = point[0];
+		pointY = point[1];
 
-		double absTurn = Math.toDegrees(Math.atan2(dy, dx));
-		target = absTurn - AutoUtils.position.getRot();
+		turnToPoint = true;
 
 		if (Robot.dt != null) {
 			requires(Robot.dt);
@@ -139,6 +141,26 @@ public PIDTurn(double[] point, DrivetrainInterface dt, SmartDashboardInterface s
 	 */
 	@Override
 	protected void initialize() {
+
+		if (!turnToPoint) {
+			if (absoluteRotation) {
+				target -= AutoUtils.position.getRot();
+			}
+		} else {
+			double dx = pointX - AutoUtils.position.getX();
+			double dy = pointY - AutoUtils.position.getY();
+
+			System.out.println("x = " + dx + ", y = " + dy);
+
+			// x and y are switched because we are using bearings
+			double absTurn = Math.toDegrees(Math.atan2(dx, dy));
+			target = absTurn - AutoUtils.position.getRot();
+			System.out.println("current bearing = " + AutoUtils.position.getRot());
+			System.out.println("target bearing = " + target);
+		}
+
+		System.out.println("Turn to point: " + turnToPoint);
+
 		turnController.disable();
 		// dt.enableVelocityPIDs();
 		System.out.println("initialize2s");
@@ -155,6 +177,7 @@ protected void initialize() {
 		while (Math.abs(newSetPoint) > 180) {
 			newSetPoint = newSetPoint - Math.signum(newSetPoint) * 360;
 		}
+		System.out.println("set point = " + newSetPoint);
 		turnController.setSetpoint(newSetPoint);
 
 		turnController.enable();
@@ -209,7 +232,7 @@ protected void end() {
 		sd.putNumber("Turn PID Error", turnController.getError());
 		// turnController.free();
 
-		AutoUtils.position.changeRot(dt.getAHRSAngle());
+		AutoUtils.position.setRot(dt.getAHRSAngle());
 	}
 
 	/**

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

@@ -14,7 +14,13 @@
 public class RunScript extends CommandGroup {
 
 	public RunScript(String scriptName) {
-		ArrayList<String[]> script = Robot.autoScripts.getOrDefault(scriptName, new ArrayList<String[]>());
+		ArrayList<String[]> script;
+		if (Robot.autoScripts.containsKey(scriptName)) {
+			script = Robot.autoScripts.get(scriptName);
+		} else {
+			System.err.println("[ERROR] auto scripts file does not contain script `" + scriptName + "`.");
+			return;
+		}
 
 		outerloop: for (String[] cmd : script) {
 			String cmdName = cmd[0];