work on auto

1 files changed, 43 insertions, 7 deletions

diff --git a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
index 35a2e1d..7285108 100644
--- a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
+++ b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
@@ -27,8 +27,9 @@
  */
 package org.usfirst.frc.team4272.robot2017;
 
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 public class Autonomous {
 	private static interface Command {
@@ -44,7 +45,7 @@ public class Autonomous {
 
 	private static class DriveDistance implements Command {
 		private static final double pctTaper = 1.0/6;
-		private static final double pctTolerance = 0.005;
+		private static final double pctTolerance = 0.002;
 
 		private final HwRobot robot;
 		private final double pctThrottle;
@@ -127,6 +128,35 @@ public class Autonomous {
 			return true;
 		}
 	}
+	private static class Init implements Command {
+		public boolean execute(Control c) {
+			c.lDrive = c.rDrive = 0;
+			c.highGear = c.gedOut = false;
+			return true;
+		}
+	}
+	private static class TimedCommand implements Command {
+		private final Command inner;
+		private final double secs;
+		private final Timer t = new Timer();
+		private boolean initialized = false;
+		public TimedCommand(Command inner, double secs) {
+			this.inner = inner;
+			this.secs = secs;
+		}
+		public boolean execute(Control c) {
+			if (!initialized) {
+				t.reset();
+				t.start();
+				initialized = true;
+			}
+			if (t.get() < secs) {
+				return inner.execute(c);
+			} else {
+				return true;
+			}
+		}
+	}
 
 	private static final double axleWidth = 2; // in feet
 	private static Command driveDistance(HwRobot robot, double speed, double dist) {
@@ -162,16 +192,25 @@ public class Autonomous {
 		this.robot = robot;
 
 		String mode = (String) modeChooser.getSelected();
+		SmartDashboard.putString("read mode", mode);
 
 		switch (mode) {
 		case "Center Peg":
 			commands = new Command[]{
-				driveDistance(robot, 0.4, 8.75),
+				new Init(),
+				new TimedCommand(driveDistance(robot, 0.4, 8.75), 6),
+				stop(robot),
+				new Command() {public boolean execute(Control c) {
+					c.gedOut = true;
+					return true;
+				}},
+				driveDistance(robot, 0.4, -3),
 				stop(robot),
 			};
 			break;
 		case "Left Peg":
 			commands = new Command[]{
+				new Init(),
 				driveDistance(robot, 0.4, 3.5),
 				turnDegrees(robot, 0.4, 60),
 				driveDistance(robot, 0.4, 8.3),
@@ -180,6 +219,7 @@ public class Autonomous {
 			break;
 		case "Right Peg":
 			commands = new Command[]{
+				new Init(),
 				driveDistance(robot, 0.4, 3.5),
 				turnDegrees(robot, 0.4, -60),
 				driveDistance(robot, 0.4, 8.3),
@@ -192,14 +232,10 @@ public class Autonomous {
 	}
 
 	public Control run(Control c) {
-		c.gedOut = false;
-		c.highGear = false;
-
 		if (step < commands.length) {
 			if (commands[step].execute(c))
 				step++;
 		}
-
 		SmartDashboard.putNumber("step", step);
 		return c;
 	}