summaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
authorLuke Shumaker <lukeshu@lukeshu.com>2017-03-04 17:12:55 -0500
committerLuke Shumaker <lukeshu@lukeshu.com>2017-03-04 17:32:30 -0500
commitbe551a17736ed4be2f9479a716fb315a7891dc1e (patch)
treef9d80aef74b373cfa27a7072db41698d21d1b6c9 /src
parent6aefca6524459dc59b13e2b26d7750f926e00895 (diff)
work on auto
Diffstat (limited to 'src')
-rw-r--r--src/org/usfirst/frc/team4272/robot2017/Autonomous.java50
1 files changed, 43 insertions, 7 deletions
diff --git a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
index 35a2e1d..7285108 100644
--- a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
+++ b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
@@ -27,8 +27,9 @@
*/
package org.usfirst.frc.team4272.robot2017;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
public class Autonomous {
private static interface Command {
@@ -44,7 +45,7 @@ public class Autonomous {
private static class DriveDistance implements Command {
private static final double pctTaper = 1.0/6;
- private static final double pctTolerance = 0.005;
+ private static final double pctTolerance = 0.002;
private final HwRobot robot;
private final double pctThrottle;
@@ -127,6 +128,35 @@ public class Autonomous {
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;
+ private final Timer t = new Timer();
+ private boolean initialized = false;
+ public TimedCommand(Command inner, double secs) {
+ this.inner = inner;
+ this.secs = secs;
+ }
+ public boolean execute(Control c) {
+ if (!initialized) {
+ t.reset();
+ t.start();
+ initialized = true;
+ }
+ if (t.get() < secs) {
+ return inner.execute(c);
+ } else {
+ return true;
+ }
+ }
+ }
private static final double axleWidth = 2; // in feet
private static Command driveDistance(HwRobot robot, double speed, double dist) {
@@ -162,16 +192,25 @@ public class Autonomous {
this.robot = robot;
String mode = (String) modeChooser.getSelected();
+ SmartDashboard.putString("read mode", mode);
switch (mode) {
case "Center Peg":
commands = new Command[]{
- driveDistance(robot, 0.4, 8.75),
+ new Init(),
+ new TimedCommand(driveDistance(robot, 0.4, 8.75), 6),
+ stop(robot),
+ new Command() {public boolean execute(Control c) {
+ c.gedOut = true;
+ return true;
+ }},
+ driveDistance(robot, 0.4, -3),
stop(robot),
};
break;
case "Left Peg":
commands = new Command[]{
+ new Init(),
driveDistance(robot, 0.4, 3.5),
turnDegrees(robot, 0.4, 60),
driveDistance(robot, 0.4, 8.3),
@@ -180,6 +219,7 @@ public class Autonomous {
break;
case "Right Peg":
commands = new Command[]{
+ new Init(),
driveDistance(robot, 0.4, 3.5),
turnDegrees(robot, 0.4, -60),
driveDistance(robot, 0.4, 8.3),
@@ -192,14 +232,10 @@ public class Autonomous {
}
public Control run(Control c) {
- c.gedOut = false;
- c.highGear = false;
-
if (step < commands.length) {
if (commands[step].execute(c))
step++;
}
-
SmartDashboard.putNumber("step", step);
return c;
}