use absolute values in AutoMoveTo

Author: kevinzwang

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

@@ -24,25 +24,11 @@ public AutoMoveTo(String[] args, DrivetrainInterface dt, SmartDashboardInterface
 		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, pidMoveSrc, 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, pidMoveSrc));
+				addSequential(new PIDMove(point, dt, sd, pidMoveSrc));
 			} else {
 				throw new IllegalArgumentException();
 			}

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

@@ -42,13 +42,20 @@ public class PIDTurn extends Command implements PIDOutput {
 	 * @param sd
 	 *            the Smart Dashboard reference, or a SmartDashboardInterface for
 	 *            testing
+	 * @param absolute
+	 *            whether the target passed is absolute or relative
 	 */
-	public PIDTurn(double targ, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource ahrs) {
+	public PIDTurn(double targ, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource ahrs, boolean absolute) {
 		// Use requires() here to declare subsystem dependencies
-		target = targ;
 		this.dt = dt;
 		this.ahrs = ahrs;
 
+		if (absolute) {
+			target = targ - AutoUtils.position.getRot();
+		} else {
+			target = targ;
+		}
+
 		if (Robot.dt != null) {
 			requires(Robot.dt);
 		}
@@ -61,6 +68,28 @@ public PIDTurn(double targ, DrivetrainInterface dt, SmartDashboardInterface sd,
 		// tim = new Timer();
 	}
 
+	/**
+	 * Constructs this command with a new PIDController. Sets all of the
+	 * controller's PID constants based on SD prefs. Sets the controller's PIDSource
+	 * to the AHRS (gyro) object and sets its PIDOutput to this command which
+	 * implements PIDOutput's pidWrite() method.
+	 * 
+	 * @param targ
+	 *            the target bearing (in degrees) to turn to (so negative if turning
+	 *            left, positive if turning right)
+	 * @param dt
+	 *            the Drivetrain (for actual code) or a DrivetrainInterface (for
+	 *            testing)
+	 * @param ahrs
+	 *            the AHRS (gyro)
+	 * @param sd
+	 *            the Smart Dashboard reference, or a SmartDashboardInterface for
+	 *            testing
+	 */
+	public PIDTurn(double targ, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource ahrs) {
+		this(targ, dt, sd, ahrs, false);
+	}
+
 	/**
 	 * Constructs this command with a new PIDController. Sets all of the
 	 * controller's PID constants based on SD prefs. Sets the controller's PIDSource