public class DriveForward extends Command {
	private double distance = 4000;
	//private double initRightEnc = 0;
	//private double initLeftEnc = 0;
	private double startTime;
    public static float maxAccel = 0;
    public static float minAccel = 0;
	private double currRightEnc;
	private double currLeftEnc;
	private double speed;
	private double diffCounts;
	private double stopLevel = -1.0; // Acceleration backwards to stop
	private double dispCount = 0;
	private double timeout;
/*private double prevRightError;
	private double prevLeftError;
	private double currRightError;
	private double currLeftError;
	private double iTerm = 0;
	private double pTerm = 1;
	private double currTime;
	private double prevTime;
	private double currRightControl;
	private double prevLeftControl;
	private double currLeftControl;
	private double prevRightControl;
	private double rightDeltaT;
	private double leftDeltaT; */
	
	

     public DriveForward() {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
    	requires(Robot.driveTrain);
    	timeout = 4;
    	}
    
     public DriveForward(double distance, double speed) {
        // Use requires() here to declare subsystem dependencies
    	this();
        this.speed = speed;
    	setDistance(distance);
    	}
     public DriveForward(double distance, double speed, double time) {
         // Use requires() here to declare subsystem dependencies
     	this();
         this.speed = speed;
         timeout = time;
     	setDistance(distance);
     	}
     
    
	// Called just before this Command runs the first time
    protected void initialize() {
    	RobotMap.driveTrainRightFront.setPosition(0);
    	RobotMap.driveTrainLeftFront.setPosition(0);
		RobotMap.driveTrainLeftFront.setEncPosition(0);
		RobotMap.driveTrainRightFront.setEncPosition(0);
    	startTime = Timer.getFPGATimestamp();
    	maxAccel = 0;
    	minAccel = 0;
    	dispCount = 0;
    	//initRightEnc = DriveEncoders.getRawRightValue();
    	//initLeftEnc = DriveEncoders.getRawLeftValue();
    	//prevTime = System.currentTimeMillis();
    	//prevRightError = 0;
    	//prevLeftError = 0;
    	//SmartDashboard.putString("Auton Debugging", "DriveForwardInit");
    	}

    // Called repeatedly when this Command is scheduled to run
    protected void execute() { 
    	//currTime = System.currentTimeMillis();
    	currRightEnc = DriveEncoders.getRawRightValue();
    	currLeftEnc = DriveEncoders.getRawLeftValue();

    	/*currRightError = currRightEnc - initRightEnc;
    	currLeftError = currLeftEnc - initLeftEnc;
    	
    	rightDeltaT = iTerm * (currTime - prevTime) * currRightError + (pTerm * (currRightError - prevRightError))/(currTime - prevTime);
    	currRightControl = prevRightControl + rightDeltaT;
    	
    	leftDeltaT = iTerm * (currTime - prevTime) * currLeftError + (pTerm * (currLeftError - prevLeftError))/(currTime - prevTime);
    	currLeftControl = prevLeftControl + leftDeltaT;

       	prevTime = currTime;
    	prevRightError = currRightError;
    	prevLeftError = currLeftError;
    	*/
    	
    	diffCounts = DriveEncoders.getRawEncDifference();
    	if (Math.abs(diffCounts) < 20 ){
    		Robot.driveTrain.setTankDriveCommand(.6 * speed, .6 * speed);
    	}
    	else if (diffCounts > 20){	//THIS WAS 50!!!
    		//Robot.driveTrain.setTankDriveCommand(.7, .5);
    		Robot.driveTrain.setTankDriveCommand(.5 * speed, .6 * speed);  
    	}
    	else if (diffCounts < -20)
    	{
    		Robot.driveTrain.setTankDriveCommand(.6 * speed, .5 * speed);
    	}

    	if (dispCount == 10) {
    		System.out.println ("Right: " + currRightEnc + " Left :" + currLeftEnc + " diff: " + diffCounts + " Pixy: " + Robot.pixyValue);
    		dispCount = 0;

    	}
    	else {
    		dispCount++;

    	}
    	
    	if (minAccel > NavX.ahrs.getWorldLinearAccelY())
        	minAccel = NavX.ahrs.getWorldLinearAccelY();
        if (maxAccel < NavX.ahrs.getWorldLinearAccelY())
        	maxAccel = NavX.ahrs.getWorldLinearAccelY();
        SmartDashboard.putNumber("Max Accelerometer", maxAccel);
        SmartDashboard.putNumber("Min Accelerometer", minAccel);

    	//SmartDashboard.putNumber("distanceAuton", DriveEncoders.getAbsoluteValue());
    	//SmartDashboard.putNumber("auton stop", (getDistance()) * (DriveEncoders.getAbsoluteValue() - getInitEncoderVal());
    }
    
    // Make this return true when this Command no longer needs to run execute()
    // Require minimum distance before trigger on accelerometer 
    protected boolean isFinished() {
    	if (Timer.getFPGATimestamp() - startTime > timeout) {
    		System.out.println("End for time");
			return true;
    	}
			else if (NavX.ahrs.getWorldLinearAccelY() < stopLevel && Math.abs(RobotMap.driveTrainRightFront.getEncPosition()) > distance) {
			System.out.println("end for Accel and Dist: Cur:" + RobotMap.driveTrainRightFront.getEncPosition() + " Dist: " + distance + " Accel " + maxAccel);
			return true;
		
		}
			else 
				return false;
    }    		
    	/*			
       	return (Timer.getFPGATimestamp() - startTime) > 5 || 
       			(NavX.ahrs.getWorldLinearAccelY() < -0.8 && 
       					Math.abs(RobotMap.driveTrainRightFront.getEncPosition()) > distance);
    	 */ 
    

    // Called once after isFinished returns true2000
    protected void end() {
    	Robot.driveTrain.setTankDriveCommand(0.0, 0.0);
    	//SmartDashboard.putString("Auton Debugging", "DriveForwardEnd");

    }

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

    
    
    public double getDistance() {
		return distance;
	}


	public void setDistance(double straightDistance) {
		this.distance = straightDistance;
	}


/*	public double getInitEncoderVal() {
		return initEncoderVal;
	}


	public void setInitEncoderVal(double initEncoderVal) {
		this.initEncoderVal = initEncoderVal;
	}
*/

} public class RotateWithPIDTankDrive extends Command implements PIDOutput{
	
    private PIDController turnController2;
    private double targetAngleDegrees;
    private double rotateToAngleRate;    
    
    private static final double kP = 0.1; //0.1 original value  (was 0.8 on July 12)
    private static final double kI = 0.00;
    private static final double kD = 0.15;
    private static final double kF = 0.00;
    private double turnRate;
    int counter=0;
    
    static final double kToleranceDegrees = 4.0f;       
    
    public RotateWithPIDTankDrive(double targetAngleDegrees) {
      requires(Robot.driveTrain);
      
/*      try {
			*//***********************************************************************
			 * navX-MXP: - This is all defined in NavX Subsystem.
			 * - Communication via RoboRIO MXP (SPI, I2C, TTL UART) and USB.            
			 * - See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface.
			 * 
			 * navX-Micro:
			 * - Communication via I2C (RoboRIO MXP or Onboard) and USB.
			 * - See http://navx-micro.kauailabs.com/guidance/selecting-an-interface.
			 * 
			 * Multiple navX-model devices on a single robot are supported.
			 ************************************************************************//*
  		
      //ahrs = new AHRS(SerialPort.Port.kUSB); 
      } catch (RuntimeException ex ) {
          DriverStation.reportError("Error instantiating navX MXP:  " + ex.getMessage(), true);
      }*/
      
      this.targetAngleDegrees=targetAngleDegrees;
      turnController2 = new PIDController(kP, kI, kD, kF, NavX.ahrs, this);
      turnController2.setInputRange(-180.0f,  180.0f);
      //turnController2.setOutputRange(-1.0, 1.0);
      turnController2.setOutputRange(-0.7, 0.7);
      turnController2.setAbsoluteTolerance(kToleranceDegrees);
      turnController2.setContinuous(true);
      turnController2.disable();
    }

    // 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 (!turnController2.isEnabled()) {
    		SmartDashboard.putNumber("TargetAngle: ", targetAngleDegrees);
    		turnController2.setSetpoint(targetAngleDegrees);
			rotateToAngleRate = 0; // This value will be updated in the pidWrite() method.
			turnController2.enable();
		}
    	
		turnRate = rotateToAngleRate;
/*		if (rotateToAngleRate >= 0.05) {
			turnRate += 0.25;
		}
		else if (rotateToAngleRate < 0.05)
			turnRate -= 0.25;
		else
			turnRate = 0.0;
*/		SmartDashboard.putNumber("Rotate Rate: ", rotateToAngleRate);
		SmartDashboard.putNumber("Turn value: ", turnRate);
		SmartDashboard.putNumber("Heading (Yaw): ", NavX.ahrs.getYaw());
		SmartDashboard.putNumber("Orientation (Angle): ", NavX.ahrs.getAngle());
		//Robot.driveTrain.setTankDriveCommand(leftStickValue,  rightStickValue);
		Robot.driveTrain.setTankDriveCommand(turnRate,  -turnRate);
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        //return false;
    	final int PATIENCE = 50;
    	if((Math.abs(NavX.ahrs.getYaw() - targetAngleDegrees)) <= kToleranceDegrees)
    	{
    		counter++;
    	} else {
    		counter=0;
    	}
    	if(counter > PATIENCE){
    		return true;
    	}else {
    		return false;
    	}
/*    	if (targetAngleDegrees >= 0) {
    		return (NavX.ahrs.getYaw() >= targetAngleDegrees);
    	}
    	else {
        	return (NavX.ahrs.getYaw() < targetAngleDegrees);
    	}
*/    }

    // Called once after isFinished returns true
    protected void end() {
		Robot.driveTrain.setTankDriveCommand(0.0, 0.0);
		SmartDashboard.putNumber("End Yaw: ", NavX.ahrs.getYaw());


    }

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

	@Override
	public void pidWrite(double output) {
		 rotateToAngleRate = output;
		
	}
} public class TurnLeft extends Command {
	public double initEnc;
	public double finalEnc;

    public TurnLeft() {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
    }
    public TurnLeft(double dist)
    {
    	finalEnc = -dist;
    	//System.out.println("Constructor " + initEnc + ", " + DriveEncoders.getRightValue() + ", " + finalEnc);
    	
    	//SmartDashboard.putString("Turn Left Command", "Constructor"); //TODO: remove this,for testing only
    }

    
    // Called just before this Command runs the first time
    protected void initialize() {
    	initEnc = DriveEncoders.getRightValue();
    	//SmartDashboard.putString("Turn Left Command", "Initialize"); //TODO: remove this,for testing only
    	//System.out.println("Initialize: "+ initEnc + ", " + DriveEncoders.getRightValue() + ", " + finalEnc);
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
    	Robot.driveTrain.setTankDriveCommand(-.5, .5);
    	//SmartDashboard.putString("Turn Left Command", "Execute"); //TODO: remove this,for testing only
    	//System.out.println("Execute " + initEnc + ", " + DriveEncoders.getRightValue() + ", " + finalEnc);
    	//System.out.println("Execute: " + finalEnc + " < " + (DriveEncoders.getRightValue() - initEnc) + " is " + (finalEnc < (DriveEncoders.getRightValue() - initEnc)));
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return (finalEnc > (DriveEncoders.getRightValue() - initEnc));
        
    }

    // Called once after isFinished returns true
    protected void end() {
    	//System.out.println("End" + initEnc + ", " + DriveEncoders.getRightValue() + ", " + finalEnc);
    	Robot.driveTrain.setTankDriveCommand(0, 0);
    	//SmartDashboard.putString("Turn Left Command", "End"); //TODO: remove this,for testing only
    }

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

    public TurnRight() {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
    }
    public TurnRight(double dist)
    {
    	finalEnc = dist;
    	//System.out.println("Constructor " + initEnc + ", " + DriveEncoders.getLeftValue() + ", " + finalEnc);
    	
    	//SmartDashboard.putString("Turn Right Command", "Constructor"); //TODO: remove this,for testing only
    }

    
    // Called just before this Command runs the first time
    protected void initialize() {
    	initEnc = DriveEncoders.getLeftValue();
    	//SmartDashboard.putString("Turn Right Command", "Initialize"); //TODO: remove this,for testing only
    	//System.out.println("Initialize: "+ initEnc + ", " + DriveEncoders.getLeftValue() + ", " + finalEnc);
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
    	Robot.driveTrain.setTankDriveCommand(.75, -.75);
    	//SmartDashboard.putString("Turn Right Command", "Execute"); //TODO: remove this,for testing only
    	//System.out.println("Execute " + initEnc + ", " + DriveEncoders.getLeftValue() + ", " + finalEnc);
    	
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return (finalEnc < (DriveEncoders.getLeftValue() - initEnc));
        
    }

    // Called once after isFinished returns true
    protected void end() {
    	//System.out.println("End" + initEnc + ", " + DriveEncoders.getLeftValue() + ", " + finalEnc);
    	Robot.driveTrain.setTankDriveCommand(0, 0);
    	//SmartDashboard.putString("Turn Right Command", "End"); //TODO: remove this,for testing only
    }

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

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

    // 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.driveTrain.setArcadeDriveCommand(0.5, 0);
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
    	return !Robot.oi.bButton.get();
    }

    // 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 Climb extends Command {

	private boolean fixedSpeed = false;

    public Climb() {
        requires(Robot.climb);
    }

    // Called just before this Command runs the first time
    protected void initialize() {
    	RobotMap.compressor.stop();
    	Robot.isClimbing = true;
    	if(Robot.lTrigger > 0.1) {
    		fixedSpeed = false;
    	} else {
    		fixedSpeed = true;
    	}
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
    	if(Robot.lTrigger > 0.1) {
    		Robot.climb.setClimbMotorSpeed(1.0 * Robot.lTrigger);
    	} else {
    		Robot.climb.setClimbMotorSpeed(1.0);
    	}

    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
    	if(!fixedSpeed) {
    		return Robot.lTrigger < 0.1;
    	} else {
    		return !Robot.oi.getXBoxController().getRawButton(8);
    	}
    }

    // Called once after isFinished returns true
    protected void end() {
    	Robot.isClimbing = false;
    	RobotMap.compressor.start();
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    	Robot.isClimbing = false;
    	RobotMap.compressor.start();
    }
}
 public class InterfaceFlip extends Command {
	
	public static boolean isFlipped = true;

	@Override
	protected void initialize() {
		// TODO Auto-generated method stub
		isFlipped = !isFlipped;
		
	}

	@Override
	protected void execute() {
		// TODO Auto-generated method stub
	}

	@Override
	protected boolean isFinished() {
		// TODO Auto-generated method stub
		return true;
	}

	@Override
	protected void end() {
		// TODO Auto-generated method stub

	}

	@Override
	protected void interrupted() {
		// TODO Auto-generated method stub

	}

} public class SwitchCamera extends Command {

    public SwitchCamera() {
        requires(Robot.driveTrain);
        
    }

    // 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.camera = Math.abs(Robot.camera - 1);
    	
    }

    // 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 TankDriveLeft extends Command {

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

    // 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.driveTrain.setTankDriveCommand(0, .5);
    	
    }

    // 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 TankDriveRight extends Command {

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

    // 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.driveTrain.setTankDriveCommand(.5, 0);
    }

    // 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 CloseDoors extends Command {

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

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

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
    	GearScore.gearScoreDoor.set(DoubleSolenoid.Value.kForward);
    	OI.doorsOpen = false;
    	
    }

    // 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 ExpelGear extends CommandGroup {

    public ExpelGear() {
    	
    	addSequential(new OpenDoors());
    	addSequential(new WaitUntilTime(300, Robot.gearIntake));
    	addSequential(new RetractPusher());
    	addSequential(new WaitUntilTime(400, Robot.gearIntake));
    	addSequential(new ExtendPusher());
    	//addSequential(new WaitUntilTime(500, Robot.gearIntake));
    	//addSequential(new CloseDoors());
    	
    }
}
 public class IngestToggle extends Command {

    public IngestToggle() {
    	requires(Robot.ballIntake);
        // 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.isIngesting = !Robot.isIngesting;
    }

    // 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 AgitatorOff extends Command {

    public AgitatorOff() {
    	requires(Robot.shooter);
        // 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() {
    	Shooter.shooterAgitator.set(0);
    	//System.out.println("AgitatorOff");
    }

    // 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 AgitatorOn extends Command {

    public AgitatorOn() {
    	requires(Robot.shooter);
        // 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() {
    	if(!Robot.isClimbing)
    		Shooter.shooterAgitator.set(20);

    }

    // 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 FeederOff extends Command {

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

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

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
    	Shooter.shooterFeeder.set(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 FeederOn extends Command {

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

    // 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(!Robot.isClimbing)
    		Shooter.shooterFeeder.set(1);
    	
    }

    // 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 ShooterOff extends Command {

    public ShooterOff() {
    	requires(Robot.shooter);
        // 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() {
    	Shooter.shooterShootMotor.set(0);
    	//System.out.println("ShooterOff");

    }

    // 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 ShooterOn extends Command {

    public ShooterOn() {
    	requires(Robot.shooter);
        // 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() {
    	if(!Robot.isClimbing)
    		Shooter.shooterShootMotor.set((double)Math.max(0, Math.min(100, Robot.shootSpeeed)) / 100);

    }

    // 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 ShooterSequenceOff extends CommandGroup {

    public ShooterSequenceOff() {
    	addSequential(new AgitatorOff());
    	addSequential(new FeederOff());
    	addSequential(new ShooterOff());
        // Add Commands here:
        // e.g. addSequential(new Command1());
        //      addSequential(new Command2());
        // these will run in order.

        // To run multiple commands at the same time,
        // use addParallel()
        // e.g. addParallel(new Command1());
        //      addSequential(new Command2());
        // Command1 and Command2 will run in parallel.

        // A command group will require all of the subsystems that each member
        // would require.
        // e.g. if Command1 requires chassis, and Command2 requires arm,
        // a CommandGroup containing them would require both the chassis and the
        // arm.
    }
} public class ShooterSequenceOn extends CommandGroup {

    public ShooterSequenceOn() {
    	addSequential(new AgitatorOn());
    	addSequential(new ShooterOn());
    	addSequential(new WaitUntilSpeed(Shooter.shooterShootMotor, 50, Robot.shooter));
    	addSequential(new FeederOn());
        // Add Commands here:
        // e.g. addSequential(new Command1());
        //      addSequential(new Command2());
        // these will run in order.

        // To run multiple commands at the same time,
        // use addParallel()
        // e.g. addParallel(new Command1());
        //      addSequential(new Command2());
        // Command1 and Command2 will run in parallel.

        // A command group will require all of the subsystems that each member
        // would require.
        // e.g. if Command1 requires chassis, and Command2 requires arm,
        // a CommandGroup containing them would require both the chassis and the
        // arm.
    }
}
import org.usfirst.frc2832.Robot2017.autonCommands.DriveForward;
import org.usfirst.frc2832.Robot2017.autonCommands.RotateWithPIDTankDrive;
import org.usfirst.frc2832.Robot2017.autonCommands.TurnLeft;
import org.usfirst.frc2832.Robot2017.autonCommands.TurnRight;
import org.usfirst.frc2832.Robot2017.commands.AllForward;
import org.usfirst.frc2832.Robot2017.commands.Climb;
import org.usfirst.frc2832.Robot2017.commands.InterfaceFlip;
import org.usfirst.frc2832.Robot2017.commands.SwitchCamera;
import org.usfirst.frc2832.Robot2017.commands.TankDriveLeft;
import org.usfirst.frc2832.Robot2017.commands.TankDriveRight;
import org.usfirst.frc2832.Robot2017.commands.gearunload.CloseDoors;
import org.usfirst.frc2832.Robot2017.commands.gearunload.ExpelGear;
import org.usfirst.frc2832.Robot2017.commands.ingest.IngestToggle;
import org.usfirst.frc2832.Robot2017.commands.shooter.AgitatorOff;
import org.usfirst.frc2832.Robot2017.commands.shooter.AgitatorOn;
import org.usfirst.frc2832.Robot2017.commands.shooter.FeederOff;
import org.usfirst.frc2832.Robot2017.commands.shooter.FeederOn;
import org.usfirst.frc2832.Robot2017.commands.shooter.ShooterOff;
import org.usfirst.frc2832.Robot2017.commands.shooter.ShooterOn;
import org.usfirst.frc2832.Robot2017.commands.shooter.ShooterSequenceOff;
import org.usfirst.frc2832.Robot2017.commands.shooter.ShooterSequenceOn;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.buttons.JoystickButton;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

// RobotBuilder Version: 2.0
//
// This file was generated by RobotBuilder. It contains sections of
// code that are automatically generated and assigned by robotbuilder.
// These sections will be updated in the future when you export to
// Java from RobotBuilder. Do not put any code or make any change in
// the blocks indicating autogenerated code or it will be lost on an
// update. Deleting the comments indicating the section will prevent
// it from being updated in the future.


package org.usfirst.frc2832.Robot2017;




/**
 * 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 {
	
	//// CREATING BUTTONS
    // One type of button is a joystick button which is any button on a joystick.
    // You create one by telling it which joystick it's on and which button
    // number it is.
    // Joystick stick = new Joystick(port);
    // Button button = new JoystickButton(stick, buttonNumber);

    // There are a few additional built in buttons you can use. Additionally,
    // by subclassing Button you can create custom triggers and bind those to
    // commands the same as any other Button.

    //// TRIGGERING COMMANDS WITH BUTTONS
    // Once you have a button, it's trivial to bind it to a button in one of
    // three ways:

    // Start the command when the button is pressed and let it run the command
    // until it is finished as determined by it's isFinished method.
    // button.whenPressed(new ExampleCommand());

    // Run the command while the button is being held down and interrupt it once
    // the button is released.
    // button.whileHeld(new ExampleCommand());

    // Start the command when the button is released  and let it run the command
    // until it is finished as determined by it's isFinished method.
    // button.whenReleased(new ExampleCommand());


    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
	public Joystick xBoxController;
	public JoystickButton aButton, xButton, bButton, yButton, rightBumper, leftBumper, startButton, selectButton, leftJoystick, rightJoystick;
    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
	public static boolean doorsOpen = false;

    @SuppressWarnings("deprecation")
	public OI() {
        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS

        xBoxController = new Joystick(0);
        
        aButton = new JoystickButton(xBoxController, 1);
        bButton = new JoystickButton(xBoxController, 2);
        xButton = new JoystickButton(xBoxController, 3);
        yButton = new JoystickButton(xBoxController, 4);
        leftBumper = new JoystickButton(xBoxController, 5);
        rightBumper = new JoystickButton(xBoxController, 6);
        selectButton = new JoystickButton(xBoxController, 7);
        startButton = new JoystickButton(xBoxController, 8);
        leftJoystick = new JoystickButton(xBoxController, 9);
        rightJoystick = new JoystickButton(xBoxController, 10);
        
        //aButton.whenPressed(new ShooterSequenceOn());
        //aButton.whenReleased(new ShooterSequenceOff());
        startButton.whenPressed(new InterfaceFlip());
        yButton.whenPressed(new ExpelGear());
        bButton.whenPressed(new IngestToggle());
        xButton.whileHeld(new CloseDoors());
        aButton.whenPressed(new FeederOn());

        leftJoystick.whenPressed(new SwitchCamera());
