re-order the things in Autonomous.java, add some explanatory comments.

1 files changed, 108 insertions, 58 deletions

diff --git a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
index ea8319e..a713f47 100644
--- a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
+++ b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
@@ -32,8 +32,82 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 public class Autonomous {
+	/*=[ Overview ]=======================================================*\
+	||                                                                    ||
+	|| Most of the robot's code is very simple.  Autonomous is less       ||
+	|| simple (it accounts for almost half of the total code!).           ||
+	||                                                                    ||
+	|| But we've organized things to make working on it fairly simple.    ||
+	||                                                                    ||
+	|| The exterior interface is simple:                                  ||
+	||  1. call `Autonomous.robotInit()` once when the robot turns on;    ||
+	||  2. call `a = new Autonomous(robot)` whenever autonomous mode      ||
+	||     starts;                                                        ||
+	||  3. call `control = a.run(control)` every 20ms to ask it what it   ||
+	||     wants the hardware to do.                                      ||
+	||                                                                    ||
+	|| But it's hard to write moderately complex autonomous modes         ||
+	|| directly with that.  It's hard to keep track of multiple           ||
+	|| steps.  So, internally, we invent "Commands", and say that an      ||
+	|| autonomous "Mode" is a function that returns a list of commands.   ||
+	||                                                                    ||
+	|| This file is divides in to 3 sections:                             ||
+	||  1. Modes: Define the autonomous modes here.                       ||
+	||  2. Abstraction: Implement the abstraction described above; define ||
+	||     what a "Command" is and what a "Mode" is, and simple code to   ||
+	||     step through the mode's commands as they run.		      ||
+	||  3. Commands: Definitions of complex commands (and functions to    ||
+	||     generate commands).                                            ||
+	||                                                                    ||
+	\*====================================================================*/
+
+	/*====================================================================*\
+	|| Modes: Define the high-level autonomous modes                      ||
+	\*====================================================================*/
+
+	public static void robotInit() {
+		Command init = c->{
+			c.lDrive = c.rDrive = 0;
+			c.highGear = c.gedOut = false;
+			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;},
+				c->{c.gedOut = true; return true;},
+				driveDistance(robot, 0.4, -3),
+				c->{c.lDrive = c.rDrive = 0; return true;},
+			});
+		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;},
+			});
+		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;},
+			});
+		modeChooser.addObject("Stand Still", robot->new Command[]{
+				c->{c.lDrive = c.rDrive = 0; return true;},
+			});
+
+		SmartDashboard.putData("Autonomous Mode", modeChooser);
+	}
+
+	/*====================================================================*\
+	|| Abstraction: Run whichever mode is selected.                       ||
+	\*====================================================================*/
+
 	private static interface Command {
-		/** Mutate the @{code Control} (possible because it is
+		/**
+		 * Mutate the @{code Control} (possible because it is
 		 * passed by reference), and return whether or not
 		 * this command has finished yet.
 		 *
@@ -42,6 +116,38 @@ public class Autonomous {
 		 */
 		public boolean execute(Control c);
 	}
+	private static interface Mode {
+		/**
+		 * Given a some robot hardware, instantiate a sequence
+		 * of @{code Command}s to run during Autonomous.
+		 *
+		 * @param robot A handle on the robot hardware.
+		 * @return The commands to run during Autonomous.
+		 */
+		public Command[] getCommands(HwRobot robot);
+	}
+
+	private static final SendableChooser<Mode> modeChooser = new SendableChooser<Mode>();
+
+	private final Command[] commands;
+	private int step = 0;
+
+	public Autonomous(HwRobot robot) {
+		commands = modeChooser.getSelected().getCommands(robot);
+	}
+
+	public Control run(Control c) {
+		if (step < commands.length) {
+			if (commands[step].execute(c))
+				step++;
+		}
+		SmartDashboard.putNumber("step", step);
+		return c;
+	}
+
+	/*====================================================================*\
+	|| Commands: For use in modes (above)                                 ||
+	\*====================================================================*/
 
 	private static class DriveDistance implements Command {
 		private static final double pctTaper = 1.0/6;
@@ -119,7 +225,7 @@ public class Autonomous {
 			SmartDashboard.putNumber("rMin", rMin);
 			SmartDashboard.putNumber("lDrive", c.lDrive);
 			SmartDashboard.putNumber("rDrive", c.rDrive);
-			return lPct >= 1 && rPct >= 1; 
+			return lPct >= 1 && rPct >= 1;
 		}
 	}
 	private static class TimedCommand implements Command {
@@ -158,60 +264,4 @@ public class Autonomous {
 	private static Command turnDegrees(HwRobot robot, double speed, double deg) {
 		return turnRadians(robot, speed, deg * Math.PI/180);
 	}
-
-	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", 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;},
-			});
-		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;},
-			});
-		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;},
-			});
-		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) {
-		if (step < commands.length) {
-			if (commands[step].execute(c))
-				step++;
-		}
-		SmartDashboard.putNumber("step", step);
-		return c;
-	}
 }