diff options
| -rw-r--r-- | src/org/usfirst/frc/team4272/robot2017/Autonomous.java | 23 |
1 files changed, 18 insertions, 5 deletions
diff --git a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java index 5fa7257..5a62be4 100644 --- a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java +++ b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java @@ -72,7 +72,7 @@ public class Autonomous { double lPct = lDistCurr/lDistTarget; double rPct = rDistCurr/rDistTarget; - c.lDrive = lSpeed * Math.signum(lDistTarget) * Math.signum(1-lPct); + c.lDrive = lSpeed; if (Math.abs(1-lPct) < pctTaper) { c.lDrive *= Math.abs((1-lPct)/pctTaper); if (Math.abs(robot.lRate.pidGet()) < 0.5) { @@ -82,8 +82,9 @@ public class Autonomous { c.lDrive = lSpeed; } } + c.lDrive *= Math.signum(lDistTarget) * Math.signum(1-lPct); - c.rDrive = rSpeed * Math.signum(rDistTarget) * Math.signum(1-rPct); + c.rDrive = rSpeed; if (Math.abs(1-rPct) < pctTaper) { c.rDrive *= Math.abs((1-rPct)/pctTaper); if (Math.abs(robot.rRate.pidGet()) < 0.5) { @@ -93,6 +94,7 @@ public class Autonomous { c.rDrive = rSpeed; } } + c.rDrive *= Math.signum(rDistTarget) * Math.signum(1-rPct); if (Math.abs(lPct - rPct) > pctTolerance) { if (lPct > rPct) { @@ -110,6 +112,12 @@ 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 final double axleWidth = 2; // in feet private static Command driveDistance(HwRobot robot, double speed, double dist) { @@ -125,6 +133,9 @@ 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; @@ -133,19 +144,21 @@ public class Autonomous { public Autonomous(HwRobot robot) { this.robot = robot; - // Center peg - /* + // Cross line / center peg commands = new Command[]{ driveDistance(robot, 0.4, 8.75), + stop(robot), }; - */ // Left peg + /* commands = new Command[]{ driveDistance(robot, 0.4, 3.5), turnDegrees(robot, 0.4, 60), driveDistance(robot, 0.4, 8.3), + stop(robot), }; + */ } public Control run(Control c) { |