diff options
| -rw-r--r-- | src/org/usfirst/frc/team4272/robot2017/Autonomous.java | 66 |
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) { |