summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--src/org/usfirst/frc/team4272/robot2017/Autonomous.java17
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);
+ }
}