add DriveForTime command

[3501/2015-FRC-Spark] / src / org / usfirst / frc3501 / RiceCatRobot / commands / DriveForTime.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 takes a time in seconds which is how long it should run
 *
 */
public class DriveForTime extends Command {
  private double seconds;
  private double speed;
  private Timer timer;
  private Direction direction;

  public DriveForTime(double seconds, double speed, Direction direction) {
    // limit speed to the range [0, MOTOR_MAX_VAL]
    this.speed = Math.max(speed, -speed);
    this.speed = Math.min(speed, DriveTrain.MOTOR_MAX_VAL);

    this.seconds = seconds;
    this.direction = direction;
  }

  @Override
  protected void initialize() {
    timer = new Timer();
    timer.reset();
    timer.start();
  }

  @Override
  protected void execute() {
    if (direction == Direction.FORWARD) {
      Robot.driveTrain.setMotorSpeeds(-speed, -speed);
    } else if (direction == Direction.BACKWARD) {
      Robot.driveTrain.setMotorSpeeds(speed, speed);
    }
  }

  @Override
  protected boolean isFinished() {
    return timer.get() > seconds;
  }

  @Override
  protected void end() {
    Robot.driveTrain.setMotorSpeeds(0, 0);
  }

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