X-Git-Url: http://challenge-bot.com/repos/?a=blobdiff_plain;f=src%2Forg%2Fusfirst%2Ffrc3501%2FRiceCatRobot%2Fcommands%2FMoveDistance.java;fp=src%2Forg%2Fusfirst%2Ffrc3501%2FRiceCatRobot%2Fcommands%2FMoveDistance.java;h=6608c33904679f63abc05eb76ecb47b8861261d3;hb=eb9e6bd8da48f0833680f6e6705542bacacd4dd8;hp=0000000000000000000000000000000000000000;hpb=de4f822fbfd21dd7421f56b749240e7b11d9bc5c;p=3501%2F2015-FRC-Spark

diff --git a/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveDistance.java b/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveDistance.java
new file mode 100644
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();
+	}
+}