Autonomous: tidy a bit

1 files changed, 11 insertions, 6 deletions

diff --git a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
index a713f47..901e22a 100644
--- a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
+++ b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
@@ -72,30 +72,32 @@ public class Autonomous {
 			return true;
 		};
 
+		Command stop = c->{c.lDrive = c.rDrive = 0; return true;};
+
 		modeChooser.addObject("Center Peg", robot->new Command[]{
 				init,
-				new TimedCommand(driveDistance(robot, 0.4, 8.75), 6),
-				c->{c.lDrive = c.rDrive = 0; return true;},
+				timeout(6.0, driveDistance(robot, 0.4, 8.75)),
+				stop,
 				c->{c.gedOut = true; return true;},
 				driveDistance(robot, 0.4, -3),
-				c->{c.lDrive = c.rDrive = 0; return true;},
+				stop,
 			});
 		modeChooser.addObject("Left Peg", robot->new Command[]{
 				init,
 				driveDistance(robot, 0.4, 3.5),
 				turnDegrees(robot, 0.4, 60),
 				driveDistance(robot, 0.4, 8.3),
-				c->{c.lDrive = c.rDrive = 0; return true;},
+				stop,
 			});
 		modeChooser.addObject("Right Peg", robot->new Command[]{
 				init,
 				driveDistance(robot, 0.4, 3.5),
 				turnDegrees(robot, 0.4, -60),
 				driveDistance(robot, 0.4, 8.3),
-				c->{c.lDrive = c.rDrive = 0; return true;},
+				stop,
 			});
 		modeChooser.addObject("Stand Still", robot->new Command[]{
-				c->{c.lDrive = c.rDrive = 0; return true;},
+				stop,
 			});
 
 		SmartDashboard.putData("Autonomous Mode", modeChooser);
@@ -264,4 +266,7 @@ public class Autonomous {
 	private static Command turnDegrees(HwRobot robot, double speed, double deg) {
 		return turnRadians(robot, speed, deg * Math.PI/180);
 	}
+	private static Command timeout(double secs, Command inner) {
+		return new TimedCommand(inner, secs);
+	}
 }