From 178f9cf75f06969b9349dda8d4cb1c425b27ed6f Mon Sep 17 00:00:00 2001 From: Luke Shumaker Date: Tue, 21 Feb 2017 01:59:48 -0500 Subject: Rename from robot2016 to robot2017 --- .../usfirst/frc/team4272/robot2017/Autonomous.java | 47 ++++++++++++++++++++++ 1 file changed, 47 insertions(+) create mode 100644 src/org/usfirst/frc/team4272/robot2017/Autonomous.java (limited to 'src/org/usfirst/frc/team4272/robot2017/Autonomous.java') 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; + } +} -- cgit v1.2.3