/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.commands.highlevel;

import edu.wpi.first.wpilibj.templates.RobotMap;
import edu.wpi.first.wpilibj.templates.commands.CommandBase;

/**
 *
 * @author team1987
 */
public class H_ShooterPreset extends CommandBase {
    private double m_angle;
    private double m_speed;
    private double m_ratio;
    private double m_timeout;
    
    
    public H_ShooterPreset(double speed, double angle, double bottomTopRatio, int timeout) {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
        requires(shooter);
        this.m_angle = angle;
        this.m_speed = speed;
        this.m_ratio = bottomTopRatio;
        this.m_timeout = timeout;
    }

    // Called just before this Command runs the first time
    protected void initialize() {
        setTimeout(m_timeout);
        shooter.setPID(true);
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {   
        shooter.setTrajectory(m_angle, m_speed);
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        if (RobotMap.isAutonomous)
            return isTimedOut();
        else if (RobotMap.S2bttn11Pressed) //!oi.getJoystick2().getRawButton(11);
            return !RobotMap.S2bttn11Pressed;
        else if (RobotMap.S2bttn2Pressed)
            return !RobotMap.S2bttn2Pressed;
        else return false;
    }

    // Called once after isFinished returns true
    protected void end() {
        if (RobotMap.isAutonomous)
            shooter.setPID(true);
        else if (RobotMap.isTeleop)
            shooter.setPID(false);
                
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    }
}
