diff options
| -rw-r--r-- | src/org/usfirst/frc/team4272/robot2017/Autonomous.java | 45 |
1 files changed, 17 insertions, 28 deletions
diff --git a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java index b8aca0c..a64e5ad 100644 --- a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java +++ b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java @@ -122,19 +122,6 @@ public class Autonomous { return lPct >= 1 && rPct >= 1; } } - private static class Stop implements Command { - public boolean execute(Control c) { - c.lDrive = c.rDrive = 0; - return true; - } - } - private static class Init implements Command { - public boolean execute(Control c) { - c.lDrive = c.rDrive = 0; - c.highGear = c.gedOut = false; - return true; - } - } private static class TimedCommand implements Command { private final Command inner; private final double secs; @@ -172,9 +159,6 @@ public class Autonomous { private static Command turnDegrees(HwRobot robot, double speed, double deg) { return turnRadians(robot, speed, deg * Math.PI/180); } - private static Command stop(HwRobot robot) { - return new Stop(); - } private final HwRobot robot; private final Command[] commands; @@ -195,40 +179,45 @@ public class Autonomous { String mode = (String) modeChooser.getSelected(); SmartDashboard.putString("read mode", mode); + Command init = c->{ + c.lDrive = c.rDrive = 0; + c.highGear = c.gedOut = false; + return true; + }; + switch (mode) { case "Center Peg": commands = new Command[]{ - new Init(), + init, new TimedCommand(driveDistance(robot, 0.4, 8.75), 6), - stop(robot), - new Command() {public boolean execute(Control c) { - c.gedOut = true; - return true; - }}, + c->{c.lDrive = c.rDrive = 0; return true;}, + c->{c.gedOut = true; return true;}, driveDistance(robot, 0.4, -3), - stop(robot), + c->{c.lDrive = c.rDrive = 0; return true;}, }; break; case "Left Peg": commands = new Command[]{ - new Init(), + init, driveDistance(robot, 0.4, 3.5), turnDegrees(robot, 0.4, 60), driveDistance(robot, 0.4, 8.3), - stop(robot), + c->{c.lDrive = c.rDrive = 0; return true;}, }; break; case "Right Peg": commands = new Command[]{ - new Init(), + init, driveDistance(robot, 0.4, 3.5), turnDegrees(robot, 0.4, -60), driveDistance(robot, 0.4, 8.3), - stop(robot), + c->{c.lDrive = c.rDrive = 0; return true;}, }; break; default: - commands = new Command[]{stop(robot)}; + commands = new Command[]{ + c->{c.lDrive = c.rDrive = 0; return true;}, + }; } } |