Clean Up Code for FRC 3501-Spark Code: Restructure codebase

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

package org.usfirst.frc3501.RiceCatRobot.commands;

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

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 DriveFor extends Command {
    private double seconds;
    private Timer timer;
    private Direction direction;

    public DriveFor(double seconds, Direction direction) {
        this.seconds = seconds;
        this.direction = direction;

    }

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

    @Override
    protected void execute() {
        System.out.println(timer.get());
        if (direction == Direction.FORWARD) {
            if (timer.get() < seconds * 0.2) { // for the first 20% of time, run
                                               // the robot at -.5 speed
                Robot.driveTrain.arcadeDrive(-0.3, 0);
            } else if (timer.get() >= seconds * 0.2
                    && timer.get() <= seconds * 0.8) { // for the +20% - 75%
                                                       // time, move the robot
                                                       // at -.3 speed.
                Robot.driveTrain.arcadeDrive(-0.5, 0);
            } else if (timer.get() < seconds) {
                Robot.driveTrain.arcadeDrive(-0.25, 0);
            } else {
                Robot.driveTrain.arcadeDrive(0, 0);
            }
        } else if (direction == Direction.BACKWARD) {
            if (timer.get() < seconds * 0.2) {
                Robot.driveTrain.arcadeDrive(0.3, 0);
            } else if (timer.get() >= seconds * 0.2
                    && timer.get() <= seconds * 0.8) {
                Robot.driveTrain.arcadeDrive(0.5, 0);
            } else if (timer.get() < seconds) {
                Robot.driveTrain.arcadeDrive(0.25, 0);
            } else {
                Robot.driveTrain.arcadeDrive(0, 0);
            }
        }
    }

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

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

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

}