add DriveDistance command

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

package org.usfirst.frc3501.RiceCatRobot.commands;

import org.usfirst.frc3501.RiceCatRobot.Robot;
import org.usfirst.frc3501.RiceCatRobot.RobotMap.Direction;
import org.usfirst.frc3501.RiceCatRobot.subsystems.DriveTrain;

import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.command.Command;

/**
 * This command will drive the specified distance at specified speed.
 *
 */
public class DriveDistance extends Command {
  private double distance; // in units of cm
  private double speed;

  public DriveDistance(double distance, double speed) {
    // Since negative values in setMotorSpeeds(...) are forwards, we reverse
    // speed here so positive input values will move the robot forwards.
    this.speed = -speed;
    this.distance = distance;
  }

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

  @Override
  protected void execute() {
    // nothing to do since motor speeds already set
  }

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