Finish writing AutoMoveTo, change RunScript to use PID commands instead

of AutoMove and AutoTurn

Author: caengineers

Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/AutoUtils.java

@@ -211,7 +211,7 @@ private static void logWarning (int lineNumber, String message) {
 	 * @param s the argument
 	 * @return if the argument is a point
 	 */
-	private static boolean isPoint (String s) {
+	public static boolean isPoint (String s) {
 		// checks if it starts and ends with parentheses
 		if (!s.startsWith("(") || !s.endsWith(")"))
 			return false;

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

@@ -1,6 +1,8 @@
 package org.usfirst.frc.team199.Robot2018.commands;
 
+import org.usfirst.frc.team199.Robot2018.Robot;
 import org.usfirst.frc.team199.Robot2018.autonomous.AutoUtils;
+import org.usfirst.frc.team199.Robot2018.autonomous.PIDSourceAverage;
 
 //import org.usfirst.frc.team199.Robot2018.subsystems.Drivetrain;
 
@@ -15,9 +17,25 @@ public AutoMoveTo(String[] args) {
         //requires(Drivetrain);
     	double rotation;
     	double[] point;
+    	String parentheseless;
+    	String[] pointparts;
     	for (String arg : args) {
     		if (AutoUtils.isDouble(arg)) {
     			rotation = Double.valueOf(arg);
+    			addSequential(new PIDTurn(rotation - AutoUtils.getRot(), Robot.dt, Robot.dt.getGyro()));
+    			AutoUtils.setRot(rotation);
+    		} else if (AutoUtils.isPoint(arg)) {
+    			parentheseless = arg.substring(1, arg.length() - 1);
+    			pointparts = parentheseless.split(",");
+    			point[0] = Double.parseDouble(pointparts[0]);
+    			point[1] = Double.parseDouble(pointparts[1]);
+    			addSequential(new PIDTurn(Math.toDegrees(Math.atan((point[0] - AutoUtils.getX())/(point[1] - AutoUtils.getY())) - AutoUtils.getRot()), Robot.dt, Robot.dt.getGyro()));
+    			addSequential(new PIDMove(Math.sqrt(((point[0] - AutoUtils.getX()) * (point[0] - AutoUtils.getX()) + ((point[1] - AutoUtils.getY()) * (point[1] - AutoUtils.getY())))), Robot.dt, new PIDSourceAverage(null, null)));
+    			AutoUtils.setX(point[0]);
+    			AutoUtils.setY(point[1]);
+    			AutoUtils.setRot(Math.toDegrees(Math.atan((point[0] - AutoUtils.getX())/(point[1] - AutoUtils.getY()))));
+    		} else {	
+    			throw new IllegalArgumentException();
     		}
     	}
 

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

@@ -3,6 +3,7 @@
 import java.util.ArrayList;
 
 import org.usfirst.frc.team199.Robot2018.Robot;
+import org.usfirst.frc.team199.Robot2018.autonomous.PIDSourceAverage;
 
 import edu.wpi.first.wpilibj.command.CommandGroup;
 import edu.wpi.first.wpilibj.command.WaitCommand;
@@ -25,10 +26,10 @@ public RunScript(String scriptName) {
 	    				addSequential(new AutoMoveTo(cmdArgs.split(" ")));
 	    				break;
 	    			case "turn":
-	    				addSequential(new AutoTurn(Double.parseDouble(cmdArgs)));
+	    				addSequential(new PIDTurn(Double.parseDouble(cmdArgs), Robot.dt, Robot.dt.getGyro()));
 	    				break;
 	    			case "move":
-	    				addSequential(new AutoMove(Double.parseDouble(cmdArgs)));
+	    				addSequential(new PIDMove(Double.parseDouble(cmdArgs), Robot.dt, new PIDSourceAverage(null, null))));
 	    				break;
 	    			case "switch":
 	    				addSequential(new EjectToSwitch());