diff options
| -rw-r--r-- | src/org/usfirst/frc/team4272/robot2017/Autonomous.java | 17 |
1 files changed, 11 insertions, 6 deletions
diff --git a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java index a713f47..901e22a 100644 --- a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java +++ b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java @@ -72,30 +72,32 @@ public class Autonomous { return true; }; + Command stop = c->{c.lDrive = c.rDrive = 0; 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;}, + timeout(6.0, driveDistance(robot, 0.4, 8.75)), + stop, c->{c.gedOut = true; return true;}, driveDistance(robot, 0.4, -3), - c->{c.lDrive = c.rDrive = 0; return true;}, + stop, }); 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;}, + stop, }); 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;}, + stop, }); modeChooser.addObject("Stand Still", robot->new Command[]{ - c->{c.lDrive = c.rDrive = 0; return true;}, + stop, }); SmartDashboard.putData("Autonomous Mode", modeChooser); @@ -264,4 +266,7 @@ public class Autonomous { private static Command turnDegrees(HwRobot robot, double speed, double deg) { return turnRadians(robot, speed, deg * Math.PI/180); } + private static Command timeout(double secs, Command inner) { + return new TimedCommand(inner, secs); + } } |