clean up MoveDistance

[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 {
  private double distance; // in units of TODO
  private double minSpeed; // in range (0, 1]
  private double maxSpeed; // in range (0, 1]

  public MoveDistance(double distance, double minSpeed, double maxSpeed) {
    requires(Robot.driveTrain);
    this.distance = distance;
    this.minSpeed = Math.min(minSpeed, maxSpeed);
    this.maxSpeed = Math.max(minSpeed, maxSpeed);
  }

  @Override
  protected void initialize() {
    Robot.driveTrain.resetEncoders();
  }

  @Override
  protected void execute() {
    // TODO: create a math helper library and move this function there
    double speed = 4 * (minSpeed - maxSpeed)
        * Math.pow((Robot.driveTrain.getAverageSpeed() / distance - 0.5), 2)
        + maxSpeed;
    Robot.driveTrain.setMotorSpeeds(speed, speed);
  }

  @Override
  protected boolean isFinished() {
    return Robot.driveTrain.getLeftDistance() > distance
        && Robot.driveTrain.getRightDistance() > distance;
  }

  @Override
  protected void end() {
    Robot.driveTrain.stop();
  }

  @Override
  protected void interrupted() {
    end();
  }
}