From f06d9ac82cc3f02fa6be83a56a9588cdff3c48f4 Mon Sep 17 00:00:00 2001 From: Luke Shumaker Date: Sun, 12 Mar 2017 13:24:58 -0400 Subject: Autonomous: tidy a bit --- src/org/usfirst/frc/team4272/robot2017/Autonomous.java | 17 +++++++++++------ 1 file 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); + } } -- cgit v1.2.3