fix conflicts

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

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();
	}
}