Merge branch 'master' of https://github.com/DriverStationComputer/RobotCode2018

Author: kevinzwang

Robot2018/lib/.gitignore

@@ -1 +1 @@
-/User_Libraries.properties
+User_Libraries.properties

Robot2018/lib/User_Libraries.properties

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

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

@@ -9,7 +9,6 @@
 import edu.wpi.first.wpilibj.PIDOutput;
 import edu.wpi.first.wpilibj.PIDSource;
 import edu.wpi.first.wpilibj.command.Command;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 /**
  * Drives the robot a certain target distance using PID. Implements PIDOutput in
@@ -21,6 +20,9 @@ public class PIDMove extends Command implements PIDOutput {
 	private DrivetrainInterface dt;
 	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
@@ -43,13 +45,14 @@ public PIDMove(double targ, DrivetrainInterface dt, SmartDashboardInterface sd,
 		target = targ;
 		this.dt = dt;
 		this.avg = avg;
+		this.sd = sd;
 		if (Robot.dt != null) {
 			requires(Robot.dt);
 		}
 		double kf = 1 / (dt.getCurrentMaxSpeed() * sd.getConst("Default PID Update Time", 0.05));
 		moveController = new PIDController(sd.getConst("MovekP", 0.1), sd.getConst("MovekI", 0),
 				sd.getConst("MovekD", 0), kf, avg, this);
-		SmartDashboard.putData("Move PID", moveController);
+		sd.putData("Move PID", moveController);
 	}
 
 	/**
@@ -70,20 +73,19 @@ 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;
 		if (Robot.dt != null) {
 			requires(Robot.dt);
 		}
 		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);
 	}
 
 	/**
@@ -92,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
@@ -100,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();
@@ -114,8 +123,8 @@ public void initialize() {
 	@Override
 	protected void execute() {
 		System.out.println("Enc Avg Dist: " + avg.pidGet());
-		SmartDashboard.putNumber("Move PID Result", moveController.get());
-		SmartDashboard.putNumber("Move PID Error", moveController.getError());
+		sd.putNumber("Move PID Result", moveController.get());
+		sd.putNumber("Move PID Error", moveController.getError());
 	}
 
 	/**
@@ -144,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());
 	}
 
 	/**
@@ -173,6 +185,6 @@ protected void interrupted() {
 	@Override
 	public void pidWrite(double output) {
 		dt.arcadeDrive(output, 0);
-		SmartDashboard.putNumber("Move PID Output", output);
+		sd.putNumber("Move PID Output", output);
 	}
 }

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

@@ -152,10 +152,11 @@ protected void initialize() {
 
 			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("position rotation = " + AutoUtils.position.getRot());
-			System.out.println("target = " + target);
+			System.out.println("current bearing = " + AutoUtils.position.getRot());
+			System.out.println("target bearing = " + target);
 		}
 
 		System.out.println("Turn to point: " + turnToPoint);

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

@@ -0,0 +1,60 @@
+controller indices
+	manipulator
+		buttons
+			1	A
+			2	B
+			3	X
+			4	Y
+			5	LB
+			6	RB
+			7	back
+			8	start
+			9	N/a
+			10	N/a
+		press mode to toggle mode light
+		axes
+			0	left thumbstick X axis (D-pad if mode light on)
+			1	left thumbstick Y axis (D-pad if mode light on)
+			2	left trigger
+			3	right trigger
+			4	right thumbstick X axis
+			5	right thumbstick Y axis
+				mode light off:
+		POV
+			D-pad (left joystick if mode light on)
+		rumble
+			fake news
+	left joystick
+		buttons
+			1
+			2
+			3
+			4
+			5
+			6
+			7
+			8
+			9
+			10
+			11
+		axes
+			0	X axis
+			1	Y axis (forward is negative)
+			2	Z axis (knob at base)
+	right joystick
+		buttons
+			1
+			2
+			3
+			4
+			5
+			6
+			7
+			8
+			9
+			10
+			11
+		axes
+			0	X axis
+			1	Y axis (forward is negative)
+			2	Z axis (knob at base)