summaryrefslogtreecommitdiff
path: root/src/org/usfirst/frc/team4272
diff options
context:
space:
mode:
authorLuke Shumaker <lukeshu@lukeshu.com>2017-03-12 13:17:50 -0400
committerLuke Shumaker <lukeshu@lukeshu.com>2017-03-12 13:17:50 -0400
commit562e8780399da70f0cb22b32667cfa3c65c5de52 (patch)
tree75345d6c92c79f9fc423a5e24fc2a9f6adb2c67f /src/org/usfirst/frc/team4272
parent8492bb861d13b012cba695e1de0549c40a45a294 (diff)
re-order the things in Autonomous.java, add some explanatory comments.
Diffstat (limited to 'src/org/usfirst/frc/team4272')
-rw-r--r--src/org/usfirst/frc/team4272/robot2017/Autonomous.java166
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;
- }
}