Simple constructor for AMT, make math readable

Author: MysteryPig

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

@@ -1,5 +1,6 @@
 package org.usfirst.frc.team199.Robot2018.commands;
 
+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;
@@ -23,18 +24,53 @@ public AutoMoveTo(String[] args, DrivetrainInterface dt, SmartDashboardInterface
 		for (String arg : args) {
 			if (AutoUtils.isDouble(arg)) {
 				rotation = Double.valueOf(arg);
-				addSequential(new PIDTurn(rotation - AutoUtils.position.getRot(), dt, sd, pidMoveSrc));
+				double relrotation = rotation - AutoUtils.position.getRot();
+				addSequential(new PIDTurn(relrotation, dt, sd, pidMoveSrc));
 				AutoUtils.position.setRot(rotation);
 			} else if (AutoUtils.isPoint(arg)) {
 				point = AutoUtils.parsePoint(arg);
-				addSequential(new PIDTurn(Math.toDegrees(
-						Math.atan((point[0] - AutoUtils.position.getX()) / (point[1] - AutoUtils.position.getY()))
-								- AutoUtils.position.getRot()),
-						dt, sd, pidMoveSrc));
-				addSequential(new PIDMove(
-						Math.sqrt(((point[0] - AutoUtils.position.getX()) * (point[0] - AutoUtils.position.getX())
-								+ ((point[1] - AutoUtils.position.getY()) * (point[1] - AutoUtils.position.getY())))),
-						dt, sd, pidMoveSrc));
+				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))));
+			} else {
+				throw new IllegalArgumentException();
+			}
+		}
+	}
+
+	public AutoMoveTo(String[] args) {
+		// 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, Robot.dt, Robot.sd, Robot.dt.getDistEncAvg()));
+				AutoUtils.position.setRot(rotation);
+			} 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, Robot.dt, Robot.sd, Robot.dt.getDistEncAvg()));
+
+				double dX2 = deltaX * deltaX;
+				double dY2 = deltaY * deltaY;
+				double distance = Math.sqrt(dX2 + dY2);
+				addSequential(new PIDMove(distance, Robot.dt, Robot.sd, Robot.dt.getDistEncAvg()));
 				double x = AutoUtils.position.getX();
 				double y = AutoUtils.position.getY();
 				AutoUtils.position.setX(point[0]);

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

@@ -4,7 +4,6 @@
 
 import org.usfirst.frc.team199.Robot2018.Robot;
 import org.usfirst.frc.team199.Robot2018.autonomous.AutoUtils;
-import org.usfirst.frc.team199.Robot2018.autonomous.PIDSourceAverage;
 
 import edu.wpi.first.wpilibj.command.CommandGroup;
 import edu.wpi.first.wpilibj.command.WaitCommand;
@@ -23,19 +22,19 @@ public RunScript(String scriptName) {
 
 			switch (cmdName) {
 			case "moveto":
-				addSequential(new AutoMoveTo(cmdArgs.split(" "), Robot.dt, Robot.sd, new PIDSourceAverage(null, null)));
+				addSequential(new AutoMoveTo(cmdArgs.split(" ")));
 				break;
 			case "turn":
 				double rotation = Double.parseDouble(cmdArgs);
 				addSequential(new PIDTurn(rotation, Robot.dt, Robot.sd, Robot.dt.getGyro()));
-				AutoUtils.setRot(rotation);
+				AutoUtils.position.setRot(rotation);
 				break;
 			case "move":
 				double distance = Double.parseDouble(cmdArgs);
 
-				addSequential(new PIDMove(distance, Robot.dt, Robot.sd, new PIDSourceAverage(null, null)));
-				AutoUtils.setX(distance * Math.sin(Math.toRadians(AutoUtils.getRot())));
-				AutoUtils.setY(distance * Math.cos(Math.toRadians(AutoUtils.getRot())));
+				addSequential(new PIDMove(distance, Robot.dt, Robot.sd, Robot.dt.getDistEncAvg()));
+				AutoUtils.position.setX(distance * Math.sin(Math.toRadians(AutoUtils.position.getRot())));
+				AutoUtils.position.setY(distance * Math.cos(Math.toRadians(AutoUtils.position.getRot())));
 				break;
 			case "switch":
 				addSequential(new EjectToSwitch());