fix conflicts

[3501/2015-FRC-Spark] / src / org / usfirst / frc3501 / RiceCatRobot / commands / MoveDistance.java

diff --git a/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveDistance.java b/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveDistance.java
new file mode 100644 (file)
index 0000000..6608c33
--- /dev/null
+++ b/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveDistance.java
@@ -0,0 +1,48 @@
+package org.usfirst.frc3501.RiceCatRobot.commands;
+
+import org.usfirst.frc3501.RiceCatRobot.robot.Robot;
+import org.usfirst.frc3501.RiceCatRobot.subsystems.DriveTrain;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class MoveDistance extends Command {
+
+       double distance, minSpeed, maxSpeed;
+
+       public MoveDistance(double distance, double minSpeed, double maxSpeed) {
+               requires(Robot.driveTrain);
+               this.distance = distance;
+               this.minSpeed = minSpeed;
+               this.maxSpeed = maxSpeed;
+       }
+
+       protected void initialize() {
+               Robot.driveTrain.resetEncoders();
+       }
+
+       @Override
+       protected void execute() {
+               double speed = 4 * (minSpeed - maxSpeed) * 
+                               Math.pow((Robot.driveTrain.getAverageSpeed() / distance - 0.5), 2) 
+                               + maxSpeed;
+               Robot.driveTrain.setMotorSpeeds(speed, speed);
+       }
+
+       @Override
+       protected boolean isFinished() {
+               if (Robot.driveTrain.getLeftDistance() > distance
+                               && Robot.driveTrain.getRightDistance() > distance)
+                       return true;
+               return false;
+       }
+
+       @Override
+       protected void end() {
+               Robot.driveTrain.stop();
+       }
+
+       @Override
+       protected void interrupted() {
+               end();
+       }
+}