wh1ter0se / PowerUp-2018

package org.usfirst.frc.team3695.robot.commands;

import org.usfirst.frc.team3695.robot.Robot;

import edu.wpi.first.wpilibj.DriverStation;

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

public class CyborgCommandDriveDirect extends Command {

    public static final long TIME_WAIT = 1000;

    public final double percent;

    private long time;

    private boolean inRange;

    public CyborgCommandDriveDirect(double percent) {

        this.percent = percent;

        requires(Robot.SUB_DRIVE);

    }

    protected void initialize() {

    	DriverStation.reportWarning("DRIVING BY POWER", false);

        Robot.SUB_DRIVE.reset();

        time = System.currentTimeMillis() + TIME_WAIT;

    }

    protected void execute() {

        inRange = Robot.SUB_DRIVE.driveDistance(percent, percent);

    }

    protected void end() {

        Robot.SUB_DRIVE.driveDirect(0, 0);

    }

    protected void interrupted() {

        end();

    }