2 files changed, 65 insertions, 14 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));
 	}
diff --git a/src/org/usfirst/frc/team4272/robot2017/Robot.java b/src/org/usfirst/frc/team4272/robot2017/Robot.java
index b65ce35..851f3f1 100644
--- a/src/org/usfirst/frc/team4272/robot2017/Robot.java
+++ b/src/org/usfirst/frc/team4272/robot2017/Robot.java
@@ -43,27 +43,43 @@ public class Robot extends IterativeRobot {
 	private Autonomous auto;
 	private Teleop teleop;
 
+	private boolean networkInitialized = false;
+	private void networkInit() {
+		Autonomous.networkInit();
+	}
+
 	/**
 	 * This function is run when the robot is first started up and should be
 	 * used for any initialization code.
 	 */
 	public void robotInit() {
-		Autonomous.robotInit();
 	}
 
 	public void autonomousInit() {
 		try {
+			if (!networkInitialized) {
+				networkInit();
+				networkInitialized = true;
+			}
 			auto = new Autonomous(robot);
-		} catch (Exception e) {}
+		} catch (Exception e) {
+			e.printStackTrace();
+		}
 	}
 	public void autonomousPeriodic() {
 		try {
 			robot.run(auto.run(control));
-		} catch (Exception e) {}
+		} catch (Exception e) {
+			e.printStackTrace();
+		}
 	}
 
 	public void teleopInit() {
 		try {
+			if (!networkInitialized) {
+				networkInit();
+				networkInitialized = true;
+			}
 			teleop = new Teleop(robot);
 		} catch (Exception e) {}
 	}
@@ -71,18 +87,36 @@ public class Robot extends IterativeRobot {
 	public void teleopPeriodic() {
 		try {
 			robot.run(teleop.run(control, oi));
-		} catch (Exception e) {}
+		} catch (Exception e) {
+			e.printStackTrace();
+		}
 	}
 
 	public void disabledInit() {
+		try {
+			if (!networkInitialized) {
+				networkInit();
+				networkInitialized = true;
+			}
+		} catch (Exception e) {
+			e.printStackTrace();
+		}
 	}
 
 	public void disabledPeriodic() {
 	}
 
-	/**
-	 * This function is called periodically during test mode
-	 */
+	public void testInit() {
+		try {
+			if (!networkInitialized) {
+				networkInit();
+				networkInitialized = true;
+			}
+		} catch (Exception e) {
+			e.printStackTrace();
+		}
+	}
+
 	public void testPeriodic() {
 	}
 }