forget enums, we have lambdas!

1 files changed, 24 insertions, 42 deletions

diff --git a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
index 3f6b7b4..ea8319e 100644
--- a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
+++ b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
@@ -159,69 +159,51 @@ public class Autonomous {
 		return turnRadians(robot, speed, deg * Math.PI/180);
 	}
 
-	private final HwRobot robot;
-	private final Command[] commands;
-	private int step = 0;
-
-	public static enum Mode {
-		CENTER_PEG, LEFT_PEG, RIGHT_PEG, NONE
+	private static final Command init = c->{
+		c.lDrive = c.rDrive = 0;
+		c.highGear = c.gedOut = false;
+		return true;
+	};
+
+	private static interface Mode {
+		public Command[] getCommands(HwRobot robot);
 	}
 	private static final SendableChooser<Mode> modeChooser = new SendableChooser<Mode>();
 
 	public static void robotInit() {
-		modeChooser.addObject("Center Peg", Mode.CENTER_PEG);
-		modeChooser.addObject("Left Peg", Mode.LEFT_PEG);
-		modeChooser.addObject("Right Peg", Mode.RIGHT_PEG);
-		modeChooser.addObject("Stand Still", Mode.NONE);
-		SmartDashboard.putData("Autonomous Mode", modeChooser);
-	}
-
-	public Autonomous(HwRobot robot) {
-		this.robot = robot;
-
-		Mode mode = modeChooser.getSelected();
-
-		Command init = c->{
-			c.lDrive = c.rDrive = 0;
-			c.highGear = c.gedOut = false;
-			return true;
-		};
-
-		switch (mode) {
-		case CENTER_PEG:
-			commands = new Command[]{
+		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;},
 				c->{c.gedOut = true; return true;},
 				driveDistance(robot, 0.4, -3),
 				c->{c.lDrive = c.rDrive = 0; return true;},
-			};
-			break;
-		case LEFT_PEG:
-			commands = new Command[]{
+			});
+		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;},
-			};
-			break;
-		case RIGHT_PEG:
-			commands = new Command[]{
+			});
+		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;},
-			};
-			break;
-		case NONE:
-		default:
-			commands = new Command[]{
+			});
+		modeChooser.addObject("Stand Still", robot->new Command[]{
 				c->{c.lDrive = c.rDrive = 0; return true;},
-			};
-		}
+			});
+		SmartDashboard.putData("Autonomous Mode", modeChooser);
+	}
+
+	private final Command[] commands;
+	private int step = 0;
+
+	public Autonomous(HwRobot robot) {
+		commands = modeChooser.getSelected().getCommands(robot);
 	}
 
 	public Control run(Control c) {