work on autonomous

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;
 	}
 }