1 files changed, 47 insertions, 0 deletions
diff --git a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
new file mode 100644
index 0000000..f3ce43f
--- /dev/null
+++ b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
@@ -0,0 +1,47 @@
+package org.usfirst.frc.team4272.robot2017;
+
+public class Autonomous {
+	private final HwRobot robot;
+
+	public Autonomous(HwRobot robot) {
+		this.robot = robot;
+		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 (rDist < thresh) {
+			c.rDrive = speed;
+		} else if (rDist < max) {
+			c.rDrive = speed*Math.sqrt((target-rDist)/(target-thresh));
+		} else {
+			c.rDrive = 0;
+		}
+
+		if (Math.abs(lDist - rDist) > 0.25) {
+			if (lDist > rDist) {
+				c.lDrive *= 0.2;
+			} else {
+				c.rDrive *= 0.2;
+			}
+		}
+
+		return c;
+	}
+}