public enum Direction {
	dPadRight(90),
	dPadUpRight(45),
	dPadUp(0),
	dPadUpLeft(315),
	dPadLeft(270),
	dPadDownLeft(225),
	dPadDown(180),
	dPadDownRight(135);
		
	public final int direction;
		
	private Direction(int direction) {
		this.direction = direction;
	}
}
 public class ArmMove extends Command {

	Logger logger = Robot.getLogger(ArmMove.class);
	private double speed = 0.0;
	
	private double leftSpeed = 0;
	private double rightSpeed = 0;
	
    public ArmMove(double speed) {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
    	this.speed = speed;
    	requires(Robot.harvester);
    }

    // Called just before this Command runs the first time
    protected void initialize() {    	
    }
    
	// Called repeatedly when this Command is scheduled to run
	@SuppressWarnings("static-access")
	protected void execute() {

		double leftPot = Robot.harvester.getLeftAimPot();
		double rightPot = Robot.harvester.getRightAimPot();

		this.leftSpeed = this.speed;
		this.rightSpeed = this.speed;

		if (!Robot.harvester.hasReset()) {
			leftSpeed *= 0.75;
			rightSpeed *= 0.75;
		}/* else if(Robot.harvester.hasReset() && Math.abs(Robot.harvester.getLeftAimPot()-Robot.harvester.getRightAimPot()) < 1) {
			// do nothing we have arms in sync and speeds are already set
		}else {
			if ((leftPot < rightPot && speed > 0) || (leftPot > rightPot && speed < 0)) {
				// Robot.harvester.setLeftTilt(speed);
				leftSpeed = speed;
				rightSpeed = speed / 2.5;
				// Robot.harvester.setRightTilt(speed/2.5);

			} else if ((rightPot < leftPot && speed > 0) || (rightPot > leftPot && speed < 0)) {
				// Robot.harvester.setLeftTilt(speed/2.5);
				leftSpeed = speed / 2.5;
				rightSpeed = speed;
				// Robot.harvester.setRightTilt(speed);
			}
		}*/
		// leftSpeed = speed;
		// rightSpeed = speed;

    	// slow down as we get higher
    	// will not slow down below 20% of start speed
//    	if(speed > 0 && Robot.harvester.hasReset()) {
//    		leftSpeed *= (-(leftPot+rightPot)/2.0/225.0+1);
//    		rightSpeed *= (-(leftPot+rightPot)/2.0/225.0+1);
//    		logger.info("Formula: " + (-(leftPot+rightPot)/2.0/225.0+1));
//    	}
    	
    	//Slow down as we get lower
    	//logger.info("reset:" + Robot.harvester.hasReset());
//    	if(speed < 0 && Robot.harvester.hasReset()) {
//    		//what happens when we get negative values passed in here?
//    		//  need to figure out how to deal with this...
//    		//  Maybe special case for negativ pot values
//    		leftSpeed = leftSpeed * ((1/150.0)*((leftPot+rightPot)/2.0-Robot.harvester.HARVESTER_MAX_ANGLE)+1);
//    		rightSpeed = rightSpeed * ((1/150.0)*((leftPot+rightPot)/2.0-Robot.harvester.HARVESTER_MAX_ANGLE)+1);
//    	}
    	
    	// stop left if bump slow right
		if(Robot.harvester.getLeftLimit() && speed < 0) {
			leftSpeed = 0;
			rightSpeed /= 2;
			Robot.harvester.resetLeftPot();
		}
		
		if(Robot.harvester.getTopLeftLimit() && speed > 0) {
			leftSpeed = 0;
			rightSpeed /= 2;
		}
		// stop left slow right if pot is too high and we have reset
		if(leftPot > Robot.harvester.HARVESTER_MAX_ANGLE && speed > 0 && Robot.harvester.hasReset()) {
			leftSpeed = 0;
			rightSpeed /= 2;
		}
		// stop right if bump slow left
		if(Robot.harvester.getRightLimit() && speed < 0) {
			rightSpeed = 0;
			leftSpeed /= 2;
			Robot.harvester.resetRightPot();
		}
		
		if(Robot.harvester.getTopRightLimit() && speed > 0) {
			rightSpeed = 0;
			leftSpeed /= 2;
		}
		// stop right slow left if pot is too high and we have reset
		if(rightPot > Robot.harvester.HARVESTER_MAX_ANGLE && speed > 0 && Robot.harvester.hasReset()) {
			rightSpeed = 0;
			leftSpeed /= 2;
		}
		
		if(Robot.harvester.getLeftCurrent() > 15 /*average of regular & stoppers*/) {
			leftSpeed = 0;
			rightSpeed /= 2;
		}
		
		if(Robot.harvester.getLeftCurrent() > 15 /*same*/) {
			rightSpeed = 0;
			leftSpeed /= 2;
		}
		
		System.out.println("Left current: " + Robot.harvester.getLeftCurrent());
		System.out.println("Right current: " + Robot.harvester.getRightCurrent());
		
		Robot.harvester.setRightTilt(rightSpeed);
		Robot.harvester.setLeftTilt(leftSpeed);
		
//		System.out.println(leftSpeed);
		
//		logger.info("left pot: " + leftPot + " right pot: " + rightPot + " hasReset" + Robot.harvester.hasReset());//+ 
				//" left limit: " + Robot.harvester.getLeftLimit()  + " right limit: " + Robot.harvester.getRightLimit());
//		logger.info("getTopLeftLimit: " + Robot.harvester.getTopLeftLimit() + " getTopRightLimit: " + Robot.harvester.getTopRightLimit());
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return leftSpeed == 0 && rightSpeed == 0;
    }

    // Called once after isFinished returns true
    protected void end() {
    	Robot.harvester.setTilt(0);
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    	end();
    }
} public class ArmMoveVariable extends Command {

    public ArmMoveVariable() {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
    	requires(Robot.harvester);
    }

    // Called just before this Command runs the first time
    protected void initialize() {
    }

    private double speed = 0;
    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
    	
    	speed = Robot.oi.getCoDriverLeftY();
    	double leftSpeed = speed;
    	double rightSpeed = speed;
    	
//    	Robot.harvester.setLeftTilt(Robot.oi.getCoDriverLeftY());
//    	Robot.harvester.setRightTilt(Robot.oi.getCoDriverLeftY());
    	
    	if(Robot.harvester.getLeftLimit() && speed < 0) {
			leftSpeed = 0;
			rightSpeed /= 2;
			Robot.harvester.resetLeftPot();
		}
		
		if(Robot.harvester.getTopLeftLimit() && speed > 0) {
			leftSpeed = 0;
			rightSpeed /= 2;
		}
		// stop left slow right if pot is too high and we have reset
		/*if(leftPot > Robot.harvester.HARVESTER_MAX_ANGLE && speed > 0 && Robot.harvester.hasReset()) {
			leftSpeed = 0;
			rightSpeed /= 2;
		}*/
		// stop right if bump slow left
		if(Robot.harvester.getRightLimit() && speed < 0) {
			rightSpeed = 0;
			leftSpeed /= 2;
			Robot.harvester.resetRightPot();
		}
		
		if(Robot.harvester.getTopRightLimit() && speed > 0) {
			rightSpeed = 0;
			leftSpeed /= 2;
		}
		// stop right slow left if pot is too high and we have reset
		/*if(rightPot > Robot.harvester.HARVESTER_MAX_ANGLE && speed > 0 && Robot.harvester.hasReset()) {
			rightSpeed = 0;
			leftSpeed /= 2;
		}*/
		
		Robot.harvester.setRightTilt(rightSpeed);
		Robot.harvester.setLeftTilt(leftSpeed);
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return (Robot.harvester.getLeftLimit() && Robot.harvester.getRightLimit() && speed < 0) || 
        		(Robot.harvester.getTopLeftLimit() && Robot.harvester.getTopRightLimit() && speed > 0);
    }

    // Called once after isFinished returns true
    protected void end() {
    	Robot.harvester.setLeftTilt(0);
    	Robot.harvester.setRightTilt(0);
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    	end();
    }
} public class ArmStop extends Command {

	Logger logger = Robot.getLogger(ArmStop.class);
	
	/**
	 * Set requirements for arm stop command
	 * Requires harvester subsystem
	 * Stops the arm's movement
	 */
    public ArmStop() {
    	
    	requires(Robot.harvester);
    	
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
    }

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

    // Called repeatedly when this Command is scheduled to run
    /**
     * Stops the arm
     */
    protected void execute() {
    	Robot.harvester.setTilt(0);
    }

    // Make this return true when this Command no longer needs to run execute()
    /**
     * Sets command to completed
     * @return boolean true
     */
    protected boolean isFinished() {
        return true;
    }

    // Called once after isFinished returns true
    protected void end() {
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
//    	logger.info("Interrupted");
    }
} public class ArmToMax extends Command {
	
	Logger logger = Robot.getLogger(ArmToMax.class);
	
//	private static final double differentiate = .025;
//	private static final double minSpd = .1;
//	private static double lSpd = .5, rSpd = .5;

	/**
	 * Automagically puts the harvester at its farthest back angle
	 */
    public ArmToMax() {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
    	requires(Robot.harvester);
//    	setTimeout(3);
    }

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

    // Called repeatedly when this Command is scheduled to run
    /*protected void execute() {
    	if(Robot.harvester.getTopLeftLimit()) {
    		lSpd = 0;
    		rSpd -= differentiate;
    	}
    	
    	if(Robot.harvester.getTopRightLimit()) {
    		lSpd -= differentiate;
    		rSpd = 0;
    	}
    	
    	if((lSpd < minSpd) && (lSpd != 0)) {
    		lSpd = minSpd;
    	}
    	
    	if((rSpd < minSpd) && (rSpd != 0)) {
    		rSpd = minSpd;
    	}
    	
    	if(isTimedOut()) {
    		lSpd = 0;
    		rSpd = 0;
    	}
    	
    	Robot.harvester.setLeftTilt(lSpd);
    	Robot.harvester.setRightTilt(rSpd);
    	logger.info("lftSw: " + Robot.harvester.getTopLeftLimit() + " rgtSw:" + Robot.harvester.getTopRightLimit() + "lft:" + lSpd + " rgt:" + rSpd);
    }*/
    
    protected void execute() {
    	double leftSpeed = 0.85;
    	double rightSpeed = 0.85;
    	
    	System.out.println(Robot.harvester.getTopLeftLimit() + " " + Robot.harvester.getTopRightLimit());
    	
    	
    	if(Robot.harvester.getTopLeftLimit()) {
    		rightSpeed *= 0.7;
    	} else if(Robot.harvester.getTopRightLimit()) {
    		leftSpeed *= 0.7;
    	}
    	
    	System.out.println(leftSpeed + " " + rightSpeed);
    	
    	Robot.harvester.setLeftTilt(leftSpeed);
    	Robot.harvester.setRightTilt(rightSpeed);
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return (Robot.harvester.getTopLeftLimit() && Robot.harvester.getTopRightLimit());
    }

    // Called once after isFinished returns true
    protected void end() {
    	logger.info("[Ending]");
    	Robot.harvester.setTilt(0);
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    	logger.info("[Interrupted]");
    	end();
    }
} public class ArmToZero extends Command {
	 BuiltInAccelerometer accel = new BuiltInAccelerometer();

	//This is bound to 1/3 the max speed for armToPosition.
	/*private double maxV = vBound/2;
	//May need to be changed later to be bound to ArmToPosition maxAcc
	private double maxA = 0.05;
	private double cmdV = 0;
	private double deltaV;
	
	private double leftSpeed = 0,
			       rightSpeed = 0;*/
	 
	@SuppressWarnings("unused")
	private static Logger log = Robot.getLogger("ArmToZero");
	
    public ArmToZero() {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
    	requires(Robot.harvester);
    }

    // Called just before this Command runs the first time
    protected void initialize() {
//    	cmdV = 0;
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
    	
    	double leftSpeed = -0.85;
    	double rightSpeed = -0.85;
    	
    	if(Robot.harvester.getLeftLimit()) {
    		rightSpeed *= 0.7;
    	} else if(Robot.harvester.getRightLimit()) {
    		leftSpeed *= 0.7;
    	}
    	
    	Robot.harvester.setLeftTilt(leftSpeed);
    	Robot.harvester.setRightTilt(rightSpeed);
    	/*
    	//this ramps up cmdV to the max v which is half of the max speed we use in the arm to pos command
    	deltaV = maxV - cmdV;
    	if (cmdV < maxV) {
    		if (deltaV < maxA) {
    			cmdV = maxV;
    		} else {
    			cmdV += maxA;
    		}
    	}
    	leftSpeed = cmdV;
    	rightSpeed = cmdV;
    	//stop the left side if we hit the left limit
    	if (Robot.harvester.getLeftLimit()) {
    		leftSpeed = 0;
    		rightSpeed = rightSpeed/2;
    	}
    	
    	//stop the right side if the right limit is hit
    	if (Robot.harvester.getRightLimit()) {
    		leftSpeed = leftSpeed/2;
    		rightSpeed = 0;
    	}
    	log.info("lft speed " + leftSpeed + " rgt speed " + rightSpeed);
    	Robot.harvester.setRightTilt(-rightSpeed);
    	Robot.harvester.setLeftTilt(-leftSpeed);*/
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return (Robot.harvester.getRightLimit() && Robot.harvester.getLeftLimit());
    }

    // Called once after isFinished returns true
    protected void end() {
    	//Just in case it's still moving
    	Robot.harvester.setTilt(0);
    	Robot.harvester.resetBothPots();
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    	end();
    }
} public class Climb extends Command {
	
	Logger logger = Robot.getLogger(Climb.class);
	
//	private boolean dartStage = false;
//	private boolean ptoStage = false;
	
	private double speed = 0;
	
	/**
	 * Set requirements for climb command.
	 * Requires climber and drive.
	 * Pulls down the arm to raise the robot
	 */
    public Climb(double speed) {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
    	this.speed = speed;
    	requires(Robot.climber);
    	requires(Robot.drive);
    }

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

    // Called repeatedly when this Command is scheduled to run
    /**
     * Engages clutch
     * If the arm is not at the bottom AND the clutch is engaged AND the latch is open, set driverail motors to full speed
     */
    
//    private double dartExtendSpeed = 0.85;
//    private double dartInSpeed = -1;
    
    @SuppressWarnings("deprecation")
	protected void execute() {
    	if(Robot.climber.getDartLimit() && this.speed > 0) {
    		Robot.climber.runDart(0);
    	} else {
    		Robot.climber.runDart(this.speed);
    	}
    	
    	System.out.println("Limit: " + Robot.climber.getDartLimit()  + " and Banner: " + Robot.climber.getWinchBanner());
//    	if(!dartStage) {
//    		Robot.climber.runDart(dartExtendSpeed);
//    		if(Robot.climber.getDartLimit()) {
//    			Robot.climber.runDart(0);
//    			dartStage = true;
//    		}
//    	} else if(!ptoStage) {
//    		Robot.drive.engagePTO();
//    		Robot.drive.setBothDrive(-1,-1);
//    		if(Robot.climber.getWinchBanner()) {
//    			Robot.drive.setBothDrive(0, 0);
//    			ptoStage = true;
//    		}
//    	} else {
//    		Robot.climber.runDart(dartInSpeed);
//    	}
//    	Robot.drive.engagePTO();
//    	if(!Robot.climber.isAtBottom() && Robot.drive.clutchState) {
//    		Robot.drive.setBothDrive(Robot.drive.CLIMB_DRIVE_SPEED, Robot.drive.CLIMB_DRIVE_SPEED);
//    	}
    }

    // Make this return true when this Command no longer needs to run execute()
    /**
     * Checks to see if command is completed 
     * @return boolean is climber arm at bottom
     */
    protected boolean isFinished() {
    	return false;
//        return Robot.climber.isAtBottom();
    }

    // Called once after isFinished returns true
    /**
     * Stops driverail motors when completed
     * @return void
     */
    @SuppressWarnings("deprecation")
	protected void end() {
    	logger.info("[Ending]");
    	Robot.drive.disengagePTO();
    	Robot.climber.runDart(0);
    	Robot.drive.setBothDrive(0, 0);
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    /**
     * When command interrupted, call end
     * @return void
     */
    protected void interrupted() {
    	logger.info("[Interrupted]");
    	end();
    }
} public class DischargeBall extends Command {

	//Logger
	Logger logger = Robot.getLogger(DischargeBall.class);
	
    public DischargeBall() {
    	requires(Robot.harvesterRollers);
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
    }

    // 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.harvesterRollers.setBallControl(Robot.harvesterRollers.HARVESTER_DISCHARGE_BALL_V_BUS);
    	Robot.harvesterRollers.setShooter(Robot.harvesterRollers.SHOOTER_DISCHARGE_BALL_V_BUS);
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return false;
    }

    // Called once after isFinished returns true
    protected void end() {
    	logger.info("[Ending]");
    	Robot.harvesterRollers.setBallControl(0);
    	Robot.harvesterRollers.setShooter(0);
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    	logger.info("[Interrupted]");
    	end();
    }
} public class DriveTurn90 extends Command {

    public DriveTurn90() {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
    	requires(Robot.drive);
    }
    
    private double initialAngle;

    // Called just before this Command runs the first time
    protected void initialize() {
    	initialAngle = Robot.drive.getGyroAngle();
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
    	if(Robot.drive.getGyroAngle()-initialAngle < 90) {
    		Robot.drive.setLeftDrive(0.5);
    		Robot.drive.setRightDrive(-0.5);
    	} else {
    		Robot.drive.setLeftDrive(0);
    		Robot.drive.setRightDrive(0);
    	}
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return Robot.drive.getGyroAngle()-initialAngle >= 90;
    }

    // Called once after isFinished returns true
    protected void end() {
    	Robot.drive.setLeftDrive(0);
		Robot.drive.setRightDrive(0);
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    	end();
    }
} public class DriveWithXbox extends Command {
	
	Logger logger = Robot.getLogger(DriveWithXbox.class);
	
    public DriveWithXbox() {
        // Use requires() here to declare subsystem dependencies
        requires(Robot.drive);
    }
	
    @SuppressWarnings("unused")
	private boolean locked = false;
//    private double lockedAngle = 0;
    
    // Called just before this Command runs the first time
    protected void initialize() {
    	locked = false;
    	logger.info("[Initializing]Drive?");
    }
	
    // Called repeatedly when this Command is scheduled to run
    private double moveSlowScale = 0.6;
    private double rotateSlowScale = 0.7;
    
//    public double throttleThrottling(double input) {
//    	if(input > 0.1) {
//    		return 	-Math.sqrt(-Math.pow((input-1),2)+1);
//    	} else if (input < -0.1){
//    		return Math.sqrt(-Math.pow((-input-1),2)+1);
//    	}
//    	return 0;
//    }
    
    protected void execute() { 
//    	System.out.println("gyro: " + Robot.drive.getGyroAngle());
    	//Clutch should never be engaged while we are under human control
//    	Robot.drive.disengagePTO();
    	
    	/*if(Robot.oi.getXYButtonState() == 2) {
    		Robot.drive.engagePTO();
    	} else if(Robot.oi.getXYButtonState() == 0) {
    		Robot.climber.releaseLatch();
    	} else {
    		Robot.climber.closeLatch();
    		Robot.drive.disengagePTO();
    	}*/
    	
    	if(Robot.drive.PTOState) {
    		Robot.drive.engagePTO();
    	} else {
    		Robot.drive.disengagePTO();
    	}
    	
//    	double off = 0;
    	
    	//Allow driver to drive in any of three modes.
    	// 1. Use triggers for speed, joystick for turning
    	// 2. Use right joystick for slow motion
    	// 3. Use left joystick for normal arcadeDrive
//    	if(Math.abs(Robot.oi.getDriverSummedTriggers()) > .1) {
//    		if(Math.abs(Robot.oi.getDriverLeftX()) < .05) {
//    			if(!locked) {
//    				lockedAngle = Robot.drive.getGyroAngle();
//    				locked = true;
//    			} else {
//    				off = (Robot.drive.getGyroAngle() - lockedAngle)/20;
//    				System.out.println(lockedAngle);
//    				Robot.drive.arcadeDrive((Robot.oi.getDriverSummedTriggers()), (off));
//    			}
//    		} else {
//    			locked = false;
//    			lockedAngle = Robot.drive.getGyroAngle();
//	    		Robot.drive.arcadeDrive((Robot.oi.getDriverSummedTriggers()/**.85*/), (Robot.oi.getDriverLeftX())/**.85*/);
//	    	}
//    	} else if(Math.abs(Robot.oi.getDriverRightY()) > 0.1 
//    	if(Math.abs(Robot.oi.getDriverSummedTriggers()) > .1) {
//    		Robot.drive.arcadeDrive((Robot.oi.getDriverSummedTriggers()), (Robot.oi.getDriverLeftX()));
//    	} else
    	if(Math.abs(Robot.oi.getDriverRightY()) > 0.1 
    			|| Math.abs(Robot.oi.getDriverRightX()) > 0.1) {
    		Robot.drive.arcadeDrive(-(Robot.oi.getDriverRightY())*moveSlowScale, -(Robot.oi.getDriverRightX())*rotateSlowScale);
    		locked = false;
    	} else {
    		double leftX;
    		double leftY;
    		if(Robot.oi.getDriverLeftY()<0){
    			leftY = (Robot.oi.getDriverLeftY())*(Robot.oi.getDriverLeftY());
    		}else{
    			leftY = -(Robot.oi.getDriverLeftY())*(Robot.oi.getDriverLeftY());
    		}
    		if(Robot.oi.getDriverLeftX()<0){
    			leftX = (Robot.oi.getDriverLeftX())*(Robot.oi.getDriverLeftX());
    		}else{
    			leftX = -(Robot.oi.getDriverLeftX())*(Robot.oi.getDriverLeftX());
    		}
    		Robot.drive.arcadeDrive(leftY,leftX);
    		locked = false;
    	}
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return false;
    }

    // Called once after isFinished returns true
    protected void end() {
    	logger.info("[Ending]Drive?");
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    	logger.info("[Interrupted]Drive?");
    	end();
    }
} public class HarvestBall extends Command {


	//Logger
	Logger logger = Robot.getLogger(HarvestBall.class);
	
    public HarvestBall() {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
    	requires(Robot.harvesterRollers);
    	
    }

    // Called just before this Command runs the first time
    protected void initialize() {
    	logger.info("[Initializing]");
    	Robot.oi.driverRumbleRight(1);
    	Robot.oi.driverRumbleLeft(1);
    	Robot.oi.coDriverRumbleLeft(1);
    	Robot.oi.coDriverRumbleRight(1);
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
    	double desiredShooterWheelHarvestSpeed = Robot.harvesterRollers.SHOOTER_HARVEST_V_BUS;
    	
    	if(Robot.harvesterRollers.hasBallLeft()) {
//    		Robot.oi.driverRumbleLeft(0);
//    		Robot.oi.coDriverRumbleLeft(0);
//    		desiredShooterWheelHarvestSpeed /= 2;
    	} else {
    		Robot.oi.driverRumbleLeft(1);
    		Robot.oi.coDriverRumbleLeft(1);
    	}
    	
    	if(Robot.harvesterRollers.hasBallRight()) {
//    		Robot.oi.driverRumbleRight(0);
//    		Robot.oi.coDriverRumbleRight(0);
//    		desiredShooterWheelHarvestSpeed /= 2;
    	} else {
    		Robot.oi.driverRumbleRight(1);
    		Robot.oi.coDriverRumbleRight(1);
    	}
    	
    	Robot.harvesterRollers.setBallControl(Robot.harvesterRollers.HARVESTER_HARVEST_V_BUS);
    	Robot.harvesterRollers.setShooter(desiredShooterWheelHarvestSpeed);
    	
    	if(Robot.harvesterRollers.hasBall()) {
    		logger.info("Both sensors tripped; ball harvested");
    	} else {
    		logger.info("Left sensor: " + Robot.harvesterRollers.hasBallLeft() + "; right sensor: " + Robot.harvesterRollers.hasBallRight());
    	}
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
    	return false;
    }

    // Called once after isFinished returns true
    protected void end() {
    	logger.info("[Ending]");
    	Robot.harvesterRollers.setBallControl(0);
    	Robot.harvesterRollers.setShooter(0);
    	
    	Robot.oi.driverRumbleLeft(0);
    	Robot.oi.driverRumbleRight(0);
    	
    	Robot.oi.coDriverRumbleLeft(0);
    	Robot.oi.coDriverRumbleRight(0);
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    	logger.info("[Interrupted]");
    	end();
    }
} public class RollersShootFire extends Command {

    public RollersShootFire() {
        // Use requires() here to declare subsystem dependencies
        requires(Robot.harvesterRollers);
    }

    // Called just before this Command runs the first time
    protected void initialize() {
//    	Robot.harvesterRollers.getShooterSpeed();
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
    	Robot.harvesterRollers.setShooter(-10/ControllerPower.getInputVoltage());
    	Robot.harvesterRollers.setBallControl(Robot.harvesterRollers.HARVESTER_DISCHARGE_BALL_V_BUS);
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return false;
    }

    // Called once after isFinished returns true
    protected void end() {
    }

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

    public RollersSpeedOut() {
        // Use requires() here to declare subsystem dependencies
        requires(Robot.harvesterRollers);
    }

    // Called just before this Command runs the first time
    protected void initialize() {
    	
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
    	if(this.timeSinceInitialized()<Robot.harvesterRollers.SHOOTER_SHOOT_SPINUP_TIME){
    		Robot.harvesterRollers.setBallControl(Robot.harvesterRollers.HARVESTER_HARVEST_V_BUS*12/ControllerPower.getInputVoltage());
    	}else{
    		Robot.harvesterRollers.setBallControl(0);
    	}
    	Robot.harvesterRollers.setShooter(-10/ControllerPower.getInputVoltage());
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return false;
    }

    // Called once after isFinished returns true
    protected void end() {
    	Robot.harvesterRollers.setBallControl(0);
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    	this.end();
    }
} public class RollersStopShooter extends Command {
	
	Logger logger = Robot.getLogger(RollersStopShooter.class);

	/**
	 * Set requirements for stop shooter command
	 * Requires harvester subsystem
	 * Stops the shooter
	 */
    public RollersStopShooter() {    	
    	requires(Robot.harvesterRollers);    	
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
    }

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

    // Called repeatedly when this Command is scheduled to run
    /**
     * Stops the shooter
     */
    protected void execute() {
    	Robot.harvesterRollers.setBallControl(0);
    	Robot.harvesterRollers.setShooter(0);
    }

    // Make this return true when this Command no longer needs to run execute()
    /**
     * Sets command to completed
     * @return boolean true
     */
    protected boolean isFinished() {
        return true;
    }

    // Called once after isFinished returns true
    protected void end() {
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    	logger.info("Interrupted");
    }
} public class StopBallControl extends Command {

    public StopBallControl() {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
    	requires(Robot.harvesterRollers);
    }

    // Called just before this Command runs the first time
    protected void initialize() {
    	Robot.harvesterRollers.setBallControl(0);
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
    	Robot.harvesterRollers.setBallControl(0);
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return true;
    }

    // Called once after isFinished returns true
    protected void end() {
    	Robot.harvesterRollers.setBallControl(0);
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    	Robot.harvesterRollers.setBallControl(0);
    }
} public class StopDrive extends Command {

    public StopDrive() {
        
    	requires(Robot.drive);
    	
    	// Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
    }

    // Called just before this Command runs the first time
    protected void initialize() {
    }

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

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return true;
    }

    // Called once after isFinished returns true
    protected void end() {
    	
    }

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

    public StopShooterWheels() {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
    	requires(Robot.harvesterRollers);
    }

    // Called just before this Command runs the first time
    protected void initialize() {
    	Robot.harvesterRollers.setShooter(0);
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
    	Robot.harvesterRollers.setShooter(0);
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return true;
    }

    // Called once after isFinished returns true
    protected void end() {
    	Robot.harvesterRollers.setShooter(0);
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    	Robot.harvesterRollers.setShooter(0);
    }
} public class ToggleFlashlight extends Command {

    public ToggleFlashlight() {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
    	requires(Robot.flashlight);
    }

    // Called just before this Command runs the first time
    protected void initialize() {
    	if(Robot.flashlight.isOn) {
    		Robot.flashlight.disableFlashLight();
    	} else {
    		Robot.flashlight.enableFlashLight();
    	}
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return true;
    }

    // Called once after isFinished returns true
    protected void end() {
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    }
} public class MO_ArmDown extends Command {
	private static final Logger logger = Robot.getLogger(MO_ArmDown.class); 
	
	/**
	 * Set requirements for arm down operation
	 * Requires harvester subsystem
	 * Will cause the harvester arm to move downwards
	 */
    public MO_ArmDown() {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
    	requires(Robot.harvester);
    }

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

    // Called repeatedly when this Command is scheduled to run
    /**
     * Moves the arm down
     */
    protected void execute() {
    	logger.info("left bump: " + Robot.harvester.getLeftLimit() + 
    			" right bump: " + Robot.harvester.getRightLimit() +
    			" left pot: " + Robot.harvester.getLeftAimPot() +  
    			" right pot: " + Robot.harvester.getRightAimPot());
    	Robot.harvester.setLeftTilt(-0.75);
    	Robot.harvester.setRightTilt(-0.75);
    }

    // Make this return true when this Command no longer needs to run execute()
    /**
     * Sets command to completed
     * @return boolean false
     */
    protected boolean isFinished() {
        return false;
    }

    // Called once after isFinished returns true
    /**
     * Stops the movement of the arm
     */
    protected void end() {
    	logger.info("[ending]");
    	Robot.harvester.setTilt(0);
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    /**
     * Ends
     */
    protected void interrupted() {
    	logger.info("[interrupted]");
    	end();
    }
} public class MO_ArmStop extends Command {

    public MO_ArmStop() {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
    	requires(Robot.harvester);
    }

    // Called just before this Command runs the first time
    protected void initialize() {
    	Robot.harvester.setTilt(0);
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
    	Robot.harvester.setTilt(0);
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return true;
    }

    // Called once after isFinished returns true
    protected void end() {
    	Robot.harvester.setTilt(0);
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    	Robot.harvester.setTilt(0);
    }
} public class MO_ArmUp extends Command {
	private static final Logger logger = Robot.getLogger(MO_ArmUp.class); 

	/**
	 * Set requirements for arm up operation
	 * Requires harvester subsystem
	 * Will cause the harvester arm to move upwards
	 */
    public MO_ArmUp() {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
    	requires(Robot.harvester);
    }

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

    // Called repeatedly when this Command is scheduled to run
    /**
     * Moves the arm up
     */
    protected void execute() {
    	logger.info("left bump: " + Robot.harvester.getLeftLimit() + 
    			" right bump: " + Robot.harvester.getRightLimit() +
    			" left pot: " + Robot.harvester.getLeftAimPot() +  
    			" right pot: " + Robot.harvester.getRightAimPot());
    	Robot.harvester.setLeftTilt(1);
    	Robot.harvester.setRightTilt(1);
    }

    // Make this return true when this Command no longer needs to run execute()
    /**
     * Sets command to completed
     * @return boolean false
     */
    protected boolean isFinished() {
        return false;
    }

    // Called once after isFinished returns true
    /**
     * Stops the movement of the arm
     */
    protected void end() {
    	logger.info("[ending]");
    	Robot.harvester.setLeftTilt(0);
    	Robot.harvester.setRightTilt(0);
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    /**
     * Ends
     */
    protected void interrupted() {
    	logger.info("[interrupted]");
    	end();
    }
} public class MO_BallControlIn extends Command {
	private static final Logger logger = Robot.getLogger(MO_BallControlIn.class); 
	
	/**
	 * Set requirements for ball control in operation
	 * Requires harvester subsystem
	 * Will cause the harvester roller to rotate inwards
	 */
    public MO_BallControlIn() {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
    	requires(Robot.harvesterRollers);
    }

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

    // Called repeatedly when this Command is scheduled to run
    /**
     * Sets the rotation of the ball control in
     */
    protected void execute() {
    	Robot.harvesterRollers.setBallControl(-1);
    }

    // Make this return true when this Command no longer needs to run execute()
    /**
     * Sets command to completed
     * @return boolean false
     */
    protected boolean isFinished() {
        return false;
    }

    // Called once after isFinished returns true
    /**
     * Stops the ball control in
     */
    protected void end() {
    	logger.info("[ending]");
    	Robot.harvesterRollers.setBallControl(0);
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    /**
     * 
     */
    protected void interrupted() {
    	logger.info("[interrupted]");
    	end();
    }
} public class MO_BallControlOut extends Command {
	private static final Logger logger = Robot.getLogger(MO_BallControlOut.class); 
	
	/**
	 * Set requirements for ball control out operation
	 * Requires harvester subsystem
	 * Will cause the harvester roller to rotate outwards
	 */
    public MO_BallControlOut() {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
    	requires(Robot.harvesterRollers);	
    }

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

    // Called repeatedly when this Command is scheduled to run
    /**
     * Sets the rotation of the ball control out
     */
    protected void execute() {
    	Robot.harvesterRollers.setBallControl(1);
    }

    // Make this return true when this Command no longer needs to run execute()
    /**
     * Sets command to completed
     * @return boolean false
     */
    protected boolean isFinished() {
        return false;
    }

    // Called once after isFinished returns true
    /**
     * Stops the ball control out
     */
    protected void end() {
    	logger.info("[ending]");
    	Robot.harvesterRollers.setBallControl(0);
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    	logger.info("[interrupted]");
    	end();
    }
} public class MO_ClutchOff extends Command {

	Logger logger = Robot.getLogger(MO_ClutchOff.class);
	
	/**
	 * Set requirements for clutch operation
	 * Requires driver subsystem
	 * Disengages the clutch that drives the climbing arm mechanism
	 */
    public MO_ClutchOff() {
    	
    	requires(Robot.drive);
    	
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
    }

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

    // Called repeatedly when this Command is scheduled to run
    /**
     * Disengages the clutch
     */
    protected void execute() {
    	Robot.drive.disengagePTO();
    }

    // Make this return true when this Command no longer needs to run execute()
    /**
     * Sets command to completed
     * @return boolean true
     */
    protected boolean isFinished() {
        return false;
    }

    // Called once after isFinished returns true
    protected void end() {
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    }
} public class MO_ClutchOn extends Command {
	
	Logger logger = Robot.getLogger(MO_ClutchOn.class);
	
	/**
	 * Set requirements for clutch operation
	 * Requires driver subsystem
	 * Engages the clutch that drives the climbing arm mechanism
	 */
    public MO_ClutchOn() {
    	
    	requires(Robot.drive);
    	
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
    }

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

    // Called repeatedly when this Command is scheduled to run
    /**
     * Engages the clutch
     */
    protected void execute() {
    	Robot.drive.engagePTO();
    }

    // Make this return true when this Command no longer needs to run execute()
    /**
     * Sets command to completed
     * @return boolean true
     */
    protected boolean isFinished() {
        return false;
    }

    // Called once after isFinished returns true
    protected void end() {
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    }
} public class MO_ShooterIn extends Command {
	private static final Logger logger = Robot.getLogger(MO_ShooterIn.class); 

	/**
	 * Set requirements for shooter in operation
	 * Requires harvester subsystem
	 * Will cause the shooter roller to rotate inwards
	 */
    public MO_ShooterIn() {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
    	requires(Robot.harvesterRollers);
    }

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

    // Called repeatedly when this Command is scheduled to run
    /**
     * Sets the rotation for the shooter in
     */
    protected void execute() {
    	Robot.harvesterRollers.setShooter(1);
    }

    // Make this return true when this Command no longer needs to run execute()
    /**
     * Sets command to completed
     * @return boolean false
     */
    protected boolean isFinished() {
        return false;
    }

    // Called once after isFinished returns true
    /**
     * Stops the shooter in
     */
    protected void end() {
    	logger.info("[ending]");
    	Robot.harvesterRollers.setShooter(0);
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    /**
     * Ends
     */
    protected void interrupted() {
    	logger.info("[interrupted]");
    	end();
    }
} public class MO_ShooterOut extends Command {
	private static final Logger logger = Robot.getLogger(MO_ShooterOut.class); 
	
	/**
	 * Set requirements for shooter out operation
	 * Requires harvester subsystem
	 * Will cause the shooter roller to rotate outwards
	 */
    public MO_ShooterOut() {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
    	requires(Robot.harvesterRollers);
    }

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

    // Called repeatedly when this Command is scheduled to run
    /**
     * Sets the rotation for the shooter out
     */
    protected void execute() {
    	Robot.harvesterRollers.setShooter(-1);
    }

    // Make this return true when this Command no longer needs to run execute()
    /**
     * Sets command to completed
     * @return boolean false
     */
    protected boolean isFinished() {
        return false;
    }

    // Called once after isFinished returns true
    /**
     * Stops the shooter out
     */
    protected void end() {
    	logger.info("[ending]");
    	Robot.harvesterRollers.setShooter(0);
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    /**
     * Ends
     */
    protected void interrupted() {
    	logger.info("[interrupted]");
    	end();
    }
}
import org.usfirst.frc.team340.robot.DPad.Direction;
import org.usfirst.frc.team340.robot.commands.ArmMove;
import org.usfirst.frc.team340.robot.commands.ArmMoveVariable;
import org.usfirst.frc.team340.robot.commands.ArmStop;
import org.usfirst.frc.team340.robot.commands.ArmToMax;
import org.usfirst.frc.team340.robot.commands.ArmToZero;
import org.usfirst.frc.team340.robot.commands.Climb;
import org.usfirst.frc.team340.robot.commands.DischargeBall;
import org.usfirst.frc.team340.robot.commands.DriveTurn90;
import org.usfirst.frc.team340.robot.commands.DriveWithXbox;
import org.usfirst.frc.team340.robot.commands.HarvestBall;
import org.usfirst.frc.team340.robot.commands.RollersShootFire;
import org.usfirst.frc.team340.robot.commands.RollersSpeedOut;
import org.usfirst.frc.team340.robot.commands.RollersStopShooter;
import org.usfirst.frc.team340.robot.commands.StopBallControl;
import org.usfirst.frc.team340.robot.commands.StopDrive;
import org.usfirst.frc.team340.robot.commands.StopShooterWheels;
import org.usfirst.frc.team340.robot.commands.ToggleFlashlight;
import org.usfirst.frc.team340.robot.commands.overrides.MO_ArmDown;
import org.usfirst.frc.team340.robot.commands.overrides.MO_ArmStop;
import org.usfirst.frc.team340.robot.commands.overrides.MO_ArmUp;
import org.usfirst.frc.team340.robot.commands.overrides.MO_BallControlIn;
import org.usfirst.frc.team340.robot.commands.overrides.MO_BallControlOut;
import org.usfirst.frc.team340.robot.commands.overrides.MO_ClutchOff;
import org.usfirst.frc.team340.robot.commands.overrides.MO_ClutchOn;
import org.usfirst.frc.team340.robot.commands.overrides.MO_ShooterIn;
import org.usfirst.frc.team340.robot.commands.overrides.MO_ShooterOut;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Joystick.RumbleType;
import edu.wpi.first.wpilibj.buttons.Button;
import edu.wpi.first.wpilibj.buttons.JoystickButton;

package org.usfirst.frc.team340.robot;



/**
 * This class is the glue that binds the controls on the physical operator
 * interface to the commands and command groups that allow control of the robot.
 */
public class OI {

	//Driver - Construct Buttons
	Joystick xBoxDriver = new Joystick(0);
	Button A1 = new JoystickButton(xBoxDriver, 1);
	Button B1 = new JoystickButton(xBoxDriver, 2);
	Button X1 = new JoystickButton(xBoxDriver, 3);
	Button Y1 = new JoystickButton(xBoxDriver, 4);
	Button LB1 = new JoystickButton(xBoxDriver, 5);
	Button RB1 = new JoystickButton(xBoxDriver, 6);
	Button Back1 = new JoystickButton(xBoxDriver, 7);
	Button Start1 = new JoystickButton(xBoxDriver, 8);
	Button LeftStick1 = new JoystickButton(xBoxDriver, 9);
	JoyTrigger leftTrig1 = new JoyTrigger(xBoxDriver, 2);
	JoyTrigger rightTrig1 = new JoyTrigger(xBoxDriver, 3);
	Button dPadUp1 = new DPad(xBoxDriver, Direction.dPadUp);
	Button dPadDown1 = new DPad(xBoxDriver, Direction.dPadDown);
	
	//Co-driver - Construct Buttons
	Joystick xBoxCoDriver = new Joystick(1);
	Button A2 = new JoystickButton(xBoxCoDriver, 1);
	Button B2 = new JoystickButton(xBoxCoDriver, 2);
	Button X2 = new JoystickButton(xBoxCoDriver, 3);
	Button Y2 = new JoystickButton(xBoxCoDriver, 4);
	Button LB2 = new JoystickButton(xBoxCoDriver, 5);
	Button RB2 = new JoystickButton(xBoxCoDriver, 6);
	Button Back2 = new JoystickButton(xBoxCoDriver, 7);
	Button Start2 = new JoystickButton(xBoxCoDriver, 8);
	Button LeftStick2 = new JoystickButton(xBoxCoDriver, 9);
	JoyTrigger leftTrig2 = new JoyTrigger(xBoxCoDriver, 2);
	JoyTrigger rightTrig2 = new JoyTrigger(xBoxCoDriver, 3);
	Button dPadUp2 = new DPad(xBoxCoDriver, Direction.dPadUp);
	Button dPadRight2 = new DPad(xBoxCoDriver, Direction.dPadRight);
	Button dPadDown2 = new DPad(xBoxCoDriver, Direction.dPadDown);
	public class CoDriverLeftStick extends Button {
		public boolean get() {
			return Math.abs(getCoDriverLeftY()) > 0.1;
		}
	}
	CoDriverLeftStick coDriverLeftStick = new CoDriverLeftStick();
	
	//Manual board - Construct Buttons
	Joystick ManualBoard = new Joystick(2);
    Button HarvesterUp = new JoystickButton(ManualBoard, 2);
    Button HarvesterDown = new JoystickButton(ManualBoard, 1);
    Button RollerIn = new JoystickButton(ManualBoard, 10);
    Button RollerOut = new JoystickButton(ManualBoard, 13);
    Button ShooterIn = new JoystickButton(ManualBoard, 5);
    Button ShooterOut = new JoystickButton(ManualBoard, 6);
    Button ClimberUp = new JoystickButton(ManualBoard, 4);
    Button ClimberDown = new JoystickButton(ManualBoard, 3);
    Button ClimberEngage = new JoystickButton(ManualBoard, 9);
    public class climberDisengage extends Button {
		public boolean get() {
			return ManualBoard.getPOV() == 0;
		}
	}
    climberDisengage ClimberDisengage = new climberDisengage();
    
    public OI() {

    	//////////////////////////////////////////
    	//			     Driver 				//
    	//////////////////////////////////////////
    	X1.whenPressed(new DriveTurn90());
    	X1.whenReleased(new StopDrive());
    	
    	LB1.whenPressed(new HarvestBall());
    	LB1.whenReleased(new RollersStopShooter());
    	RB1.whenPressed(new RollersSpeedOut());
    	Start1.whenPressed(new Climb(0.7));
    	Start1.whenReleased(new DriveWithXbox());
    	Back1.whenPressed(new Climb(-1));
    	Back1.whenReleased(new DriveWithXbox());
    	
    	rightTrig1.whenPressed(new RollersShootFire());
    	rightTrig1.whenReleased(new RollersStopShooter());
    	
    	dPadUp1.whenPressed(new ArmMove(1.0));
    	dPadUp1.whenReleased(new ArmStop());
    	dPadDown1.whenPressed(new ArmMove(-1.0));
    	dPadDown1.whenReleased(new ArmStop());
    	
    	//////////////////////////////////////////
    	//			   Co-Driver 				//
    	//////////////////////////////////////////
      	A2.whenPressed(new ArmToZero());
    	A2.whenReleased(new ArmStop());
    	B2.whenPressed(new ArmToMax());
    	B2.whenReleased(new ArmStop());
    	Y2.whenPressed(new DischargeBall());
    	Y2.whenReleased(new RollersStopShooter());
    	
    	LB2.whenPressed(new HarvestBall());
    	LB2.whenReleased(new RollersStopShooter());
    	RB2.whenPressed(new RollersSpeedOut());
    	Back2.whenPressed(new MO_ArmDown());
    	Back2.whenReleased(new ArmStop());

    	rightTrig2.whenPressed(new RollersShootFire());
    	rightTrig2.whenReleased(new RollersStopShooter());
    	
    	dPadUp2.whenPressed(new ArmMove(0.90));
    	dPadUp2.whenReleased(new ArmStop());
    	dPadDown2.whenPressed(new ArmMove(-0.8));
    	dPadDown2.whenReleased(new ArmStop());
    	dPadRight2.whenPressed(new ToggleFlashlight());    	
    	
    	coDriverLeftStick.whenPressed(new ArmMoveVariable());
    	coDriverLeftStick.whenReleased(new StopDrive());
    	
    	//////////////////////////////////////////
    	//			  MANUAL BOARD				//
    	//////////////////////////////////////////
    	
    	HarvesterUp.whenPressed(new MO_ArmUp());
    	HarvesterUp.whenReleased(new MO_ArmStop());
    	HarvesterDown.whenPressed(new MO_ArmDown());
    	HarvesterDown.whenReleased(new MO_ArmStop());
    	
    	RollerIn.whenPressed(new MO_BallControlIn());
    	RollerIn.whenReleased(new StopBallControl());

    	RollerOut.whenPressed(new MO_BallControlOut());
