Work on auto.

1 files changed, 18 insertions, 5 deletions

diff --git a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
index 5fa7257..5a62be4 100644
--- a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
+++ b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
@@ -72,7 +72,7 @@ public class Autonomous {
 			double lPct = lDistCurr/lDistTarget;
 			double rPct = rDistCurr/rDistTarget;
 
-			c.lDrive = lSpeed * Math.signum(lDistTarget) * Math.signum(1-lPct);
+			c.lDrive = lSpeed;
 			if (Math.abs(1-lPct) < pctTaper) {
 				c.lDrive *= Math.abs((1-lPct)/pctTaper);
 				if (Math.abs(robot.lRate.pidGet()) < 0.5) {
@@ -82,8 +82,9 @@ public class Autonomous {
 					c.lDrive = lSpeed;
 				}
 			}
+			c.lDrive *= Math.signum(lDistTarget) * Math.signum(1-lPct);
 
-			c.rDrive = rSpeed * Math.signum(rDistTarget) * Math.signum(1-rPct);
+			c.rDrive = rSpeed;
 			if (Math.abs(1-rPct) < pctTaper) {
 				c.rDrive *= Math.abs((1-rPct)/pctTaper);
 				if (Math.abs(robot.rRate.pidGet()) < 0.5) {
@@ -93,6 +94,7 @@ public class Autonomous {
 					c.rDrive = rSpeed;
 				}
 			}
+			c.rDrive *= Math.signum(rDistTarget) * Math.signum(1-rPct);
 
 			if (Math.abs(lPct - rPct) > pctTolerance) {
 				if (lPct > rPct) {
@@ -110,6 +112,12 @@ public class Autonomous {
 			return lPct >= 1 && rPct >= 1; 
 		}
 	}
+	private static class Stop implements Command {
+		public boolean execute(Control c) {
+			c.lDrive = c.rDrive = 0;
+			return true;
+		}
+	}
 
 	private static final double axleWidth = 2; // in feet
 	private static Command driveDistance(HwRobot robot, double speed, double dist) {
@@ -125,6 +133,9 @@ public class Autonomous {
 	private static Command turnDegrees(HwRobot robot, double speed, double deg) {
 		return turnRadians(robot, speed, deg * Math.PI/180);
 	}
+	private static Command stop(HwRobot robot) {
+		return new Stop();
+	}
 
 	private final HwRobot robot;
 	private final Command[] commands;
@@ -133,19 +144,21 @@ public class Autonomous {
 	public Autonomous(HwRobot robot) {
 		this.robot = robot;
 
-		// Center peg
-		/*
+		// Cross line / center peg
 		commands = new Command[]{
 			driveDistance(robot, 0.4, 8.75),
+			stop(robot),
 		};
-		*/
 
 		// Left peg
+		/*
 		commands = new Command[]{
 			driveDistance(robot, 0.4, 3.5),
 			turnDegrees(robot, 0.4, 60),
 			driveDistance(robot, 0.4, 8.3),
+			stop(robot),
 		};
+		*/
 	}
 
 	public Control run(Control c) {