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

package org.usfirst.frc3501.RiceCatRobot.commands;

import org.usfirst.frc3501.RiceCatRobot.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();
        }
}