Write Autonomous.

1 files changed, 29 insertions, 9 deletions

diff --git a/src/org/usfirst/frc/team4272/robot2016/Autonomous.java b/src/org/usfirst/frc/team4272/robot2016/Autonomous.java
index 7b2194c..39b792d 100644
--- a/src/org/usfirst/frc/team4272/robot2016/Autonomous.java
+++ b/src/org/usfirst/frc/team4272/robot2016/Autonomous.java
@@ -1,26 +1,46 @@
 package org.usfirst.frc.team4272.robot2016;
 
-import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-
 public class Autonomous {
-	private final Timer time = new Timer();
 	private final HwRobot robot;
 
 	public Autonomous(HwRobot robot) {
 		this.robot = robot;
-		time.reset();
-		time.start();
+		robot.lDriveE.reset();
+		robot.rDriveE.reset();
 	}
 
 	public Control run(Control c) {
+		double lDist = robot.lDriveE.getDistance();
+		double rDist = robot.rDriveE.getDistance();
+
+		double speed = 0.6;
+		double thresh = 10;
+		double target = 15;
+		double max = 16;
 		
+		if (lDist < thresh) {
+			c.lDrive = speed;
+		} else if (lDist < max) {
+			c.lDrive = speed*Math.sqrt((target-lDist)/(target-thresh));
+		} else {
+			c.lDrive = 0;
+		}
 
-		if (time.get() < 15) {
+		if (rDist < thresh) {
+			c.rDrive = speed;
+		} else if (rDist < max) {
+			c.rDrive = speed*Math.sqrt((target-rDist)/(target-thresh));
+		} else {
+			c.rDrive = 0;
 		}
-		else {
+
+		if (Math.abs(lDist - rDist) > 0.25) {
+			if (lDist > rDist) {
+				c.lDrive *= 0.2;
+			} else {
+				c.rDrive *= 0.2;
+			}
 		}
-		SmartDashboard.putNumber("autoTime", time.get());
 
 		return c;
 	}