1 files changed, 24 insertions, 7 deletions

diff --git a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
index 021a00a..60dcd24 100644
--- a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
+++ b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
@@ -65,7 +65,7 @@ public class Autonomous {
 	|| Modes: Define the high-level autonomous modes                      ||
 	\*====================================================================*/
 
-	public static void robotInit() {
+	public static void networkInit() {
 		Command init = c->{
 			c.lDrive = c.rDrive = 0;
 			c.highGear = c.gedOut = false;
@@ -79,6 +79,12 @@ public class Autonomous {
 				driveDistance(robot, 0.4, 10),
 				stop,
 			});
+		
+		modeChooser.addObject("Drive 7 seconds", robot->new Command[]{
+				init,
+				timeout(7.0, c->{c.lDrive = c.rDrive = 0.4; return false;}),
+				stop,
+			});
 		modeChooser.addObject("Center Peg", robot->new Command[]{
 				init,
 				timeout(6.0, driveDistance(robot, 0.4, 8.75)),
@@ -89,9 +95,9 @@ public class Autonomous {
 			});
 		modeChooser.addObject("Left Peg", robot->new Command[]{
 				init,
-				driveDistance(robot, 0.4, 3.5),
+				driveDistance(robot, 0.5, 8.0),
 				turnDegrees(robot, 0.4, 60),
-				driveDistance(robot, 0.4, 8.3),
+				driveDistance(robot, 0.4, 2.16),
 				stop,
 			});
 		modeChooser.addObject("Right Peg", robot->new Command[]{
@@ -158,18 +164,24 @@ public class Autonomous {
 
 	private static class DriveDistance implements Command {
 		private static final double pctTaper = 1.0/6;
-		private static final double pctTolerance = 0.002;
+
+		private double
+			cntT = 0,
+			cntL = 0,
+			cntR = 0;
 
 		private final HwRobot robot;
 		private final double pctThrottle;
+		private final double pctTolerance;
 		private final double lSpeed, lDistTarget;
 		private final double rSpeed, rDistTarget;
 		private double lMin = 0, rMin = 0;
-		public DriveDistance(HwRobot robot, double pctThrottle,
+		public DriveDistance(HwRobot robot, double pctThrottle, double pctTolerance,
 		                     double lSpeed, double lDistTarget,
 		                     double rSpeed, double rDistTarget) {
 			this.robot = robot;
 			this.pctThrottle = pctThrottle;
+			this.pctTolerance = pctTolerance;
 			this.lSpeed = lSpeed;
 			this.lDistTarget = lDistTarget;
 			this.rSpeed = rSpeed;
@@ -218,10 +230,13 @@ public class Autonomous {
 			c.rDrive *= Math.signum(rDistTarget) * Math.signum(1-rPct);
 
 			/* Make sure the left and right sides are even */
+			cntT++;
 			if (Math.abs(lPct - rPct) > pctTolerance) {
 				if (lPct > rPct) {
+					cntL++;
 					c.lDrive *= pctThrottle;
 				} else {
+					cntR++;
 					c.rDrive *= pctThrottle;
 				}
 			}
@@ -232,6 +247,8 @@ public class Autonomous {
 			SmartDashboard.putNumber("rMin", rMin);
 			SmartDashboard.putNumber("lDrive", c.lDrive);
 			SmartDashboard.putNumber("rDrive", c.rDrive);
+			SmartDashboard.putNumber("lCnt", cntL/(double)cntT);
+			SmartDashboard.putNumber("rCnt", cntR/(double)cntT);
 			return lPct >= 1 && rPct >= 1;
 		}
 	}
@@ -259,12 +276,12 @@ public class Autonomous {
 	}
 
 	private static Command driveDistance(HwRobot robot, double speed, double dist) {
-		return new DriveDistance(robot, 0.2,
+		return new DriveDistance(robot, 0.05, 0.001,
 		                         speed, dist,
 		                         speed, dist);
 	}
 	private static Command turnRadians(HwRobot robot, double speed, double rad) {
-		return new DriveDistance(robot, 0.6,
+		return new DriveDistance(robot, 0.6, 0.002,
 		                         speed, Math.copySign(rad*robot.axleWidth/2, rad),
 		                         speed, Math.copySign(rad*robot.axleWidth/2, -rad));
 	}