diff options
| -rw-r--r-- | src/org/usfirst/frc/team4272/robot2016/Autonomous.java | 38 |
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; } |