diff options
| -rw-r--r-- | src/org/usfirst/frc/team4272/robot2017/Autonomous.java | 22 |
1 files changed, 13 insertions, 9 deletions
diff --git a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java index 499dbf5..51c2061 100644 --- a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java +++ b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java @@ -36,19 +36,17 @@ public class Autonomous { robot.rDriveE.reset(); } - public Control run(Control c) { + private boolean driveDistance(Control c, double speed, double dist) { double lDist = robot.lDriveE.getDistance(); double rDist = robot.rDriveE.getDistance(); - double speed = 0.6; - double thresh = 10; - double target = 15; - double max = 16; - + double thresh = dist * 2.0/3.0; + double max = dist * 1.05; + if (lDist < thresh) { c.lDrive = speed; } else if (lDist < max) { - c.lDrive = speed*Math.sqrt((target-lDist)/(target-thresh)); + c.lDrive = speed*Math.sqrt((dist-lDist)/(dist-thresh)); } else { c.lDrive = 0; } @@ -56,19 +54,25 @@ public class Autonomous { if (rDist < thresh) { c.rDrive = speed; } else if (rDist < max) { - c.rDrive = speed*Math.sqrt((target-rDist)/(target-thresh)); + c.rDrive = speed*Math.sqrt((dist-rDist)/(dist-thresh)); } else { c.rDrive = 0; + return true; } - if (Math.abs(lDist - rDist) > 0.25) { + if (Math.abs(lDist - rDist) > 0.1) { if (lDist > rDist) { c.lDrive *= 0.2; } else { c.rDrive *= 0.2; } } + return false; + } + public Control run(Control c) { + c.gedOut = false; + driveDistance(c, 0.3, 8.75); return c; } } |