diff options
| -rw-r--r-- | src/org/usfirst/frc/team4272/robot2017/Autonomous.java | 166 |
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; - } } |