modify changing and getting AutoUtils values

PIDTurn and PIDMove can now move to an absolute value and they also
update the position and angle based on encoder and gyro values instead
of the assumed values

Author: kevinzwang

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

@@ -2,6 +2,7 @@
 
 import org.usfirst.frc.team199.Robot2018.Robot;
 import org.usfirst.frc.team199.Robot2018.SmartDashboardInterface;
+import org.usfirst.frc.team199.Robot2018.autonomous.AutoUtils;
 import org.usfirst.frc.team199.Robot2018.subsystems.DrivetrainInterface;
 
 import edu.wpi.first.wpilibj.PIDController;
@@ -32,6 +33,8 @@ public class PIDMove extends Command implements PIDOutput {
 	 * @param dt
 	 *            the Drivetrain (for actual code) or a DrivetrainInterface (for
 	 *            testing)
+	 * @param sd
+	 *            the SmartDashboard
 	 * @param avg
 	 *            the PIDSourceAverage of the DT's two Encoders
 	 */
@@ -50,6 +53,42 @@ public PIDMove(double targ, DrivetrainInterface dt, SmartDashboardInterface sd,
 				kf, avg, this);
 	}
 
+	/**
+	 * 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 encoder average object and sets its PIDOutput to this command which
+	 * implements PIDOutput's pidWrite() method.
+	 * 
+	 * @param point
+	 *            the target point in inches, absolute distance from the starting
+	 *            point
+	 * @param dt
+	 *            the Drivetrain (for actual code) or a DrivetrainInterface (for
+	 *            testing)
+	 * @param sd
+	 *            the SmartDashboard
+	 * @param avg
+	 *            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();
+
+		double dist = Math.sqrt(dx * dx + dy * dy); // pythagorean theorem to find distance
+
+		this.target = dist;
+		this.dt = dt;
+		this.avg = avg;
+		if (Robot.dt != null) {
+			requires(Robot.dt);
+		}
+		moveController = new PIDController(sd.getConst("MovekP", 1), sd.getConst("MovekI", 0), sd.getConst("MovekD", 0),
+				avg, this);
+		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);
+	}
+
 	/**
 	 * Called just before this Command runs the first time. Resets the distance
 	 * encoders, sets the moveController's settings, and then enables it.
@@ -107,6 +146,13 @@ protected void end() {
 		moveController.disable();
 		System.out.println("End");
 		// moveController.free();
+		
+		double angle = Math.toRadians(AutoUtils.position.getRot());
+		double dist = avg.pidGet();
+		double x = Math.cos(angle) * dist;
+		double y = Math.sin(angle) * dist;
+		AutoUtils.position.changeX(x);
+		AutoUtils.position.changeY(y);
 	}
 
 	/**

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

@@ -2,6 +2,7 @@
 
 import org.usfirst.frc.team199.Robot2018.Robot;
 import org.usfirst.frc.team199.Robot2018.SmartDashboardInterface;
+import org.usfirst.frc.team199.Robot2018.autonomous.AutoUtils;
 import org.usfirst.frc.team199.Robot2018.subsystems.DrivetrainInterface;
 
 import edu.wpi.first.wpilibj.PIDController;
@@ -60,6 +61,28 @@ public PIDTurn(double targ, DrivetrainInterface dt, SmartDashboardInterface sd,
 		// tim = new Timer();
 	}
 
+	public PIDTurn(double[] point, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource ahrs) {
+		this.dt = dt;
+		this.ahrs = ahrs;
+		
+		double dx = point[0] - AutoUtils.position.getX();
+		double dy = point[1] - AutoUtils.position.getY();
+		
+		double absTurn = Math.toDegrees(Math.atan(dx / dy));
+		target = absTurn - AutoUtils.position.getRot();
+		
+		if (Robot.dt != null) {
+			requires(Robot.dt);
+		}
+		// calculates the maximum turning speed in degrees/sec based on the max linear
+		// speed in inches/s and the distance (inches) between sides of the DT
+		double maxTurnSpeed = dt.getCurrentMaxSpeed() * 360 / (Math.PI * sd.getConst("Distance Between Wheels", 26.25));
+		double kf = 1 / (maxTurnSpeed * sd.getConst("Default PID Update Time", 0.05));
+		turnController = new PIDController(sd.getConst("TurnkP", 1), sd.getConst("TurnkI", 0), sd.getConst("TurnkD", 0),
+				kf, ahrs, this);
+		// tim = new Timer();
+	}
+
 	/**
 	 * Called just before this Command runs the first time. Resets the gyro, sets
 	 * the turnController's settings, and then enables it.
@@ -137,6 +160,8 @@ protected void end() {
 		SmartDashboard.putNumber("Turn PID Result", turnController.get());
 		SmartDashboard.putNumber("Turn PID Error", turnController.getError());
 		// turnController.free();
+		
+		AutoUtils.position.changeRot(dt.getAHRSAngle());
 	}
 
 	/**