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