// Path: test/src/org/usfirst/frc/team340/robot/Robot.java
// public class Robot extends IterativeRobot {
// 
// 	public static final ExampleSubsystem exampleSubsystem = new ExampleSubsystem();
// 	public static OI oi;
// 
//     Command autonomousCommand;
//     SendableChooser chooser;
//     
//     //Init subsystems
//     public static LeftDrive lDrive;
//     public static RightDrive rDrive;
// 
//     public Robot() {
//     	lDrive = new LeftDrive();
//         rDrive = new RightDrive();
//     }
//     
//     /**
//      * This function is run when the robot is first started up and should be
//      * used for any initialization code.
//      */
//     public void robotInit() {
// 		oi = new OI();
//         chooser = new SendableChooser();
//         chooser.addDefault("Default Auto", new ExampleCommand());
// //        chooser.addObject("My Auto", new MyAutoCommand());
//         SmartDashboard.putData("Auto mode", chooser);
//         SmartDashboard.putData(lDrive);
//         SmartDashboard.putData(rDrive);
//     }
// 	
// 	/**
//      * This function is called once each time the robot enters Disabled mode.
//      * You can use it to reset any subsystem information you want to clear when
// 	 * the robot is disabled.
//      */
//     public void disabledInit(){
// 
//     }
// 	
// 	public void disabledPeriodic() {
// 		Scheduler.getInstance().run();
// 	}
// 
// 	/**
// 	 * This autonomous (along with the chooser code above) shows how to select between different autonomous modes
// 	 * using the dashboard. The sendable chooser code works with the Java SmartDashboard. If you prefer the LabVIEW
// 	 * Dashboard, remove all of the chooser code and uncomment the getString code to get the auto name from the text box
// 	 * below the Gyro
// 	 *
// 	 * You can add additional auto modes by adding additional commands to the chooser code above (like the commented example)
// 	 * or additional comparisons to the switch structure below with additional strings & commands.
// 	 */
//     public void autonomousInit() {
//         autonomousCommand = (Command) chooser.getSelected();
//         
// 		/* String autoSelected = SmartDashboard.getString("Auto Selector", "Default");
// 		switch(autoSelected) {
// 		case "My Auto":
// 			autonomousCommand = new MyAutoCommand();
// 			break;
// 		case "Default Auto":
// 		default:
// 			autonomousCommand = new ExampleCommand();
// 			break;
// 		} */
//     	
//     	// schedule the autonomous command (example)
//         if (autonomousCommand != null) autonomousCommand.start();
//     }
// 
//     /**
//      * This function is called periodically during autonomous
//      */
//     public void autonomousPeriodic() {
//         Scheduler.getInstance().run();
//     }
// 
//     public void teleopInit() {
// 		// This makes sure that the autonomous stops running when
//         // teleop starts running. If you want the autonomous to 
//         // continue until interrupted by another command, remove
//         // this line or comment it out.
//         if (autonomousCommand != null) autonomousCommand.cancel();
//     }
// 
//     /**
//      * This function is called periodically during operator control
//      */
//     public void teleopPeriodic() {
//         Scheduler.getInstance().run();
//     }
//     
//     /**
//      * This function is called periodically during test mode
//      */
//     public void testPeriodic() {
//         LiveWindow.run();
//     }
// }


import java.util.logging.Logger;
import org.usfirst.frc.team340.robot.Robot;
import edu.wpi.first.wpilibj.command.Command;
package org.usfirst.frc.team340.robot.commands;




/**
 *
 */
public class DriveTime extends Command {
	
	private Logger logger = Robot.getLogger(DriveTime.class);
	
	private double lSpeed;
	private double rSpeed;

	/**
	 * Code to drive robot based on time
	 * @param timeout time before stop
	 * @param speed speed to move (both rails)
	 */
    public DriveTime(double timeout, double speed) {
        this(timeout, speed, speed);
    }
    
    /**
     * Drive based on time
     * @param timeout time before stop
     * @param leftSpeed speed of left rail
     * @param rightSpeed speed of right rail
     */
    public DriveTime(double timeout, double leftSpeed, double rightSpeed) {
    	requires(Robot.drive);
    	setTimeout(timeout);
    	lSpeed = leftSpeed;
    	rSpeed = rightSpeed;
    }

    // Called just before this Command runs the first time
    protected void initialize() {
    	logger.info("[Initializing]");
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
// Path: test/src/org/usfirst/frc/team340/robot/Robot.java
// public class Robot extends IterativeRobot {
// 
// 	public static final ExampleSubsystem exampleSubsystem = new ExampleSubsystem();
// 	public static OI oi;
// 
//     Command autonomousCommand;
//     SendableChooser chooser;
//     
//     //Init subsystems
//     public static LeftDrive lDrive;
//     public static RightDrive rDrive;
// 
//     public Robot() {
//     	lDrive = new LeftDrive();
//         rDrive = new RightDrive();
//     }
//     
//     /**
//      * This function is run when the robot is first started up and should be
//      * used for any initialization code.
//      */
//     public void robotInit() {
// 		oi = new OI();
//         chooser = new SendableChooser();
//         chooser.addDefault("Default Auto", new ExampleCommand());
// //        chooser.addObject("My Auto", new MyAutoCommand());
//         SmartDashboard.putData("Auto mode", chooser);
//         SmartDashboard.putData(lDrive);
//         SmartDashboard.putData(rDrive);
//     }
// 	
// 	/**
//      * This function is called once each time the robot enters Disabled mode.
//      * You can use it to reset any subsystem information you want to clear when
// 	 * the robot is disabled.
//      */
//     public void disabledInit(){
// 
//     }
// 	
// 	public void disabledPeriodic() {
// 		Scheduler.getInstance().run();
// 	}
// 
// 	/**
// 	 * This autonomous (along with the chooser code above) shows how to select between different autonomous modes
// 	 * using the dashboard. The sendable chooser code works with the Java SmartDashboard. If you prefer the LabVIEW
// 	 * Dashboard, remove all of the chooser code and uncomment the getString code to get the auto name from the text box
// 	 * below the Gyro
// 	 *
// 	 * You can add additional auto modes by adding additional commands to the chooser code above (like the commented example)
// 	 * or additional comparisons to the switch structure below with additional strings & commands.
// 	 */
//     public void autonomousInit() {
//         autonomousCommand = (Command) chooser.getSelected();
//         
// 		/* String autoSelected = SmartDashboard.getString("Auto Selector", "Default");
// 		switch(autoSelected) {
// 		case "My Auto":
// 			autonomousCommand = new MyAutoCommand();
// 			break;
// 		case "Default Auto":
// 		default:
// 			autonomousCommand = new ExampleCommand();
// 			break;
// 		} */
//     	
//     	// schedule the autonomous command (example)
//         if (autonomousCommand != null) autonomousCommand.start();
//     }
// 
//     /**
//      * This function is called periodically during autonomous
//      */
//     public void autonomousPeriodic() {
//         Scheduler.getInstance().run();
//     }
// 
//     public void teleopInit() {
// 		// This makes sure that the autonomous stops running when
//         // teleop starts running. If you want the autonomous to 
//         // continue until interrupted by another command, remove
//         // this line or comment it out.
//         if (autonomousCommand != null) autonomousCommand.cancel();
//     }
// 
//     /**
//      * This function is called periodically during operator control
//      */
//     public void teleopPeriodic() {
//         Scheduler.getInstance().run();
//     }
//     
//     /**
//      * This function is called periodically during test mode
//      */
//     public void testPeriodic() {
//         LiveWindow.run();
//     }
// }


// Path: Stronghold2016/src/org/usfirst/frc/team340/robot/commands/DriveTime.java
import java.util.logging.Logger;
import org.usfirst.frc.team340.robot.Robot;
import edu.wpi.first.wpilibj.command.Command;

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




/**
 *
 */
public class DriveTime extends Command {
	
	private Logger logger = Robot.getLogger(DriveTime.class);
	
	private double lSpeed;
	private double rSpeed;

	/**
	 * Code to drive robot based on time
	 * @param timeout time before stop
	 * @param speed speed to move (both rails)
	 */
    public DriveTime(double timeout, double speed) {
        this(timeout, speed, speed);
    }
    
    /**
     * Drive based on time
     * @param timeout time before stop
     * @param leftSpeed speed of left rail
     * @param rightSpeed speed of right rail
     */
    public DriveTime(double timeout, double leftSpeed, double rightSpeed) {
    	requires(Robot.drive);
    	setTimeout(timeout);
    	lSpeed = leftSpeed;
    	rSpeed = rightSpeed;
    }

    // Called just before this Command runs the first time
    protected void initialize() {
    	logger.info("[Initializing]");
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
    	Robot.drive.setBothDrive(lSpeed, rSpeed);