autonomous: use a simple commands system

1 files changed, 89 insertions, 33 deletions

diff --git a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
index 51c2061..d012a98 100644
--- a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
+++ b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
@@ -28,51 +28,107 @@
 package org.usfirst.frc.team4272.robot2017;
 
 public class Autonomous {
-	private final HwRobot robot;
-
-	public Autonomous(HwRobot robot) {
-		this.robot = robot;
-		robot.lDriveE.reset();
-		robot.rDriveE.reset();
+	private static interface Command {
+		public boolean execute(Control c);
 	}
 
-	private boolean driveDistance(Control c, double speed, double dist) {
-		double lDist = robot.lDriveE.getDistance();
-		double rDist = robot.rDriveE.getDistance();
+	private static class DriveDistance implements Command {
+		private static final double pctTaper = 1.0/3;
+		private static final double pctTolerance = 0.005;
+		private static final double pctThrottle = 0.2;
+		private static final double distFinished = 0.1;
 
-		double thresh = dist * 2.0/3.0;
-		double max = dist * 1.05;
-
-		if (lDist < thresh) {
-			c.lDrive = speed;
-		} else if (lDist < max) {
-			c.lDrive = speed*Math.sqrt((dist-lDist)/(dist-thresh));
-		} else {
-			c.lDrive = 0;
+		private final HwRobot robot;
+		private final double lSpeed, lDistTarget;
+		private final double rSpeed, rDistTarget;
+		public DriveDistance(HwRobot robot,
+		                     double lSpeed, double lDistTarget,
+		                     double rSpeed, double rDistTarget) {
+			this.robot = robot;
+			this.lSpeed = lSpeed;
+			this.lDistTarget = lDistTarget;
+			this.rSpeed = rSpeed;
+			this.rDistTarget = rDistTarget;
 		}
-
-		if (rDist < thresh) {
-			c.rDrive = speed;
-		} else if (rDist < max) {
-			c.rDrive = speed*Math.sqrt((dist-rDist)/(dist-thresh));
-		} else {
-			c.rDrive = 0;
-			return true;
+		public DriveDistance(HwRobot robot, double speed, double target) {
+			this(robot, speed, target, speed, target);
 		}
+		private boolean initialized = false;
+		public boolean execute(Control c) {
+			if (!initialized) {
+				robot.lDriveE.reset();
+				robot.rDriveE.reset();
+				initialized = true;
+			}
+			double lDistCurr = robot.lDriveE.getDistance();
+			double rDistCurr = robot.rDriveE.getDistance();
+			double lPct = lDistCurr/lDistTarget;
+			double rPct = rDistCurr/rDistTarget;
 
-		if (Math.abs(lDist - rDist) > 0.1) {
-			if (lDist > rDist) {
-				c.lDrive *= 0.2;
-			} else {
-				c.rDrive *= 0.2;
+			c.lDrive = Math.copySign(lSpeed, 1-lPct);
+			if (Math.abs(1-lPct) < pctTaper) {
+				c.lDrive *= Math.sqrt(Math.abs((1-lPct)/pctTaper));
 			}
+
+			c.rDrive = Math.copySign(rSpeed, 1-rPct);
+			if (Math.abs(1-rPct) < pctTaper) {
+				c.rDrive *= Math.sqrt(Math.abs((1-lPct)/pctTaper));
+			}
+
+			if (Math.abs(lPct - rPct) > pctTolerance) {
+				if (lPct > rPct) {
+					c.lDrive *= pctThrottle;
+				} else {
+					c.rDrive *= pctThrottle;
+				}
+			}
+			return Math.abs(lDistCurr - lDistTarget) < distFinished && Math.abs(rDistCurr - rDistTarget) < distFinished;
 		}
-		return false;
+	}
+
+	private static final double axleWidth = 2; // in feet
+	private static Command driveDistance(HwRobot robot, double speed, double dist) {
+		return new DriveDistance(robot, speed, dist);
+	}
+	private static Command turnRadians(HwRobot robot, double speed, double rad) {
+		return new DriveDistance(robot,
+		                         speed, Math.copySign(rad*axleWidth, rad),
+		                         speed, Math.copySign(rad*axleWidth, -rad));
+	}
+	private static Command turnDegrees(HwRobot robot, double speed, double deg) {
+		return turnRadians(robot, speed, deg * Math.PI/180);
+	}
+
+	private final HwRobot robot;
+	private final Command[] commands;
+	private int step = 0;
+
+	public Autonomous(HwRobot robot) {
+		this.robot = robot;
+
+		// Center peg
+		/*
+		commands = new Command[]{
+			driveDistance(robot, 0.3, 8.75),
+		};
+		*/
+
+		// Left peg
+		commands = new Command[]{
+			driveDistance(robot, 0.3, 3.5),
+			turnDegrees(robot, 0.3, 60),
+			driveDistance(robot, 0.3, 8.3),
+		};
 	}
 
 	public Control run(Control c) {
 		c.gedOut = false;
-		driveDistance(c, 0.3, 8.75);
+
+		if (step < commands.length) {
+			if (commands[step].execute(c))
+				step++;
+		}
+
 		return c;
 	}
 }