/**
	 * Runs the motors with arcade steering. 
	 */
	void OperatorControl(void)
	{
		
		GetWatchdog().SetEnabled(true);
		compressor->Start();
		
		GetWatchdog().SetExpiration(0.5);
		
		bool valve_state = false;
		
		while (IsOperatorControl())
		{
			motor->Set(stick->GetY());
			
			if (stick->GetRawButton(1) && !valve_state)
			{
				valve->Set(true);
				valve_state = true;
			}
			
			if (!stick->GetRawButton(1) && valve_state)
			{
				valve->Set(false);
				valve_state = false;
			}
			// Update driver station
			//dds->sendIOPortData(valve);

			GetWatchdog().Feed();
		}
	}

	/**
	 * Runs the motors with arcade steering. 
	 */
	void OperatorControl(void)
	{
		AnalogTrigger trig1(2);
		trig1.SetLimitsVoltage(1.5, 2.5);
			
		AnalogTrigger trig2(3);
		trig2.SetLimitsVoltage(1.5, 2.5);

		Encoder encoder(
			trig1.CreateOutput(AnalogTriggerOutput::kRisingPulse),
			trig2.CreateOutput(AnalogTriggerOutput::kRisingPulse),
			false, Encoder::k2X); 
		
		double tm = GetTime();
		
		GetWatchdog().SetEnabled(true);
		while (IsOperatorControl())
		{
			GetWatchdog().Feed();
			myRobot.ArcadeDrive(stick); // drive with arcade style (use right stick)
			
			if (GetTime() - tm > 0.25)
			{
				printf("Encoder: %d\r", encoder.Get());
				tm = GetTime();
			}
			
			Wait(0.005);				// wait for a motor update time
		}
	}

	//robot turns to desired position with a deadband of 2 degrees in each direction
	bool GyroTurn (float desiredTurnAngle, float turnSpeed)
	{
		bool turning = true;
		float myAngle = gyro->GetAngle();
		//normalizes angle from gyro to [-180,180) with zero as straight ahead
		while(myAngle >= 180)
		{
			GetWatchdog().Feed();
			myAngle = myAngle - 360;
		}
		while(myAngle < -180)
		{
			GetWatchdog().Feed();
			myAngle = myAngle + 360;
		}
		if(myAngle < (desiredTurnAngle - 2))// if robot is too far left, turn right a bit
		{
			myRobot->Drive(turnSpeed, -turnSpeed); //(right,left)
		}
		if(myAngle > (desiredTurnAngle + 2))// if robot is too far right, turn left a bit
		{
			myRobot->Drive(-turnSpeed, turnSpeed); //(right,left)
		}
		else
		{
			myRobot->Drive(0, 0);
			turning = false;
		}

		return turning;
	}

	/**
	 * Runs the motors with arcade steering. 
	 */
	void OperatorControl(void)
	{
		GetWatchdog().SetEnabled(true);
		while (IsOperatorControl())
		{
			GetWatchdog().Feed();
			myRobot.ArcadeDrive(stick); // drive with arcade style (use right stick)
		}
	}

	Spike::Spike(void)
	{ 
		GetWatchdog().SetExpiration(10.0);//set up watchdog
		GetWatchdog().SetEnabled(true);
		
		Drive = new RobotDrive(LEFT_DRV_PWM,RIGHT_DRV_PWM);
		rightjoy = new Joystick(1);
		leftjoy = new Joystick(2);

	}

	void DashBoardInput() {
		int i = 0;
		GetWatchdog().SetEnabled(true);
		while (IsAutonomous() && IsEnabled()) {
			i++;
			GetWatchdog().Feed();
			dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "%f, %i",
					driverStation->GetAnalogIn(1), i);
			dsLCD->UpdateLCD();
		}
	}

	/**
	 * Drive left & right motors for 2 seconds, enabled by a jumper (jumper
	 * must be in for autonomous to operate).
	 */
	void Autonomous(void)
	{
		GetWatchdog().SetEnabled(false);
		if (ds->GetDigitalIn(ENABLE_AUTONOMOUS) == 1)	// only run the autonomous program if jumper is in place
		{
			myRobot->Drive(0.5, 0.0);			// drive forwards half speed
			Wait(2000);							//    for 2 seconds
			myRobot->Drive(0.0, 0.0);			// stop robot
		}
		GetWatchdog().SetEnabled(true);
	}

        /**
     * Constructor for this robot subclass.
     * Create an instance of a RobotDrive with left and right motors plugged into PWM
     * ports 1 and 2 on the first digital module. The two servos are PWM ports 3 and 4.
     * Tested with camera settings White Balance: Flurorescent1, Brightness 40, Exposure Auto
     */
    TwoColorDemo(void)
    {
        ds = DriverStation::GetInstance();
        myRobot = new RobotDrive(1, 2, 0.5);    // robot will use PWM 1-2 for drive motors
        rightStick = new Joystick(1);   // create the joysticks
        leftStick = new Joystick(2);
        // remember to use jumpers on the sidecar for the Servo PWMs
        horizontalServo = new Servo(9); // create horizontal servo on PWM 9
        verticalServo = new Servo(10);  // create vertical servo on PWM 10
        servoDeadband = 0.01;   // move if > this amount 
        framesPerSecond = 15;   // number of camera frames to get per second
        sinStart = 0.0;         // control where to start the sine wave for pan
        memset(&par, 0, sizeof(par));   // initialize particle analysis report

        /* image data for tracking - override default parameters if needed */
        /* recommend making PINK the first color because GREEN is more 
         * subsceptible to hue variations due to lighting type so may
         * result in false positives */
        // PINK
        sprintf(td1.name, "PINK");
        td1.hue.minValue = 220;
        td1.hue.maxValue = 255;
        td1.saturation.minValue = 75;
        td1.saturation.maxValue = 255;
        td1.luminance.minValue = 85;
        td1.luminance.maxValue = 255;
        // GREEN
        sprintf(td2.name, "GREEN");
        td2.hue.minValue = 55;
        td2.hue.maxValue = 125;
        td2.saturation.minValue = 58;
        td2.saturation.maxValue = 255;
        td2.luminance.minValue = 92;
        td2.luminance.maxValue = 255;

        /* set up debug output: 
         * DEBUG_OFF, DEBUG_MOSTLY_OFF, DEBUG_SCREEN_ONLY, DEBUG_FILE_ONLY, DEBUG_SCREEN_AND_FILE 
         */
        SetDebugFlag(DEBUG_SCREEN_ONLY);

        /* start the CameraTask  */
        if (StartCameraTask(framesPerSecond, 0, k320x240, ROT_0) == -1)
        {
            DPRINTF(LOG_ERROR,
                    "Failed to spawn camera task; exiting. Error code %s",
                    GetVisionErrorText(GetLastVisionError()));
        }
        /* allow writing to vxWorks target */
        Priv_SetWriteFileAllowed(1);

        /* stop the watchdog if debugging  */
        GetWatchdog().SetExpiration(0.5);
        GetWatchdog().SetEnabled(false);
    }

	/**
	 * Runs the motors with arcade steering. 
	 */
	void RobotMain(void)
	{
		GetWatchdog().SetEnabled(false);
		
		while (true)
		{
			GetWatchdog().Feed();
			sendIOPortData();
			sendVisionData();
			Wait(1.0);
		}
	}

	/****************************************
	 * MainRobot: (The constructor)
	 * Mandatory method.
	 * TODO:
	 * - Tweak anything related to the scissor lift - verify values.
	 * - Find out how to configure Victor.
	 */
	MainRobot(void):
		// Below: The constructor needs to know which port connects to which
		// motor so it can control the Jaguars correctly.
		// See constants at top.
		robotDrive(LEFT_FRONT_MOTOR_PORT, LEFT_REAR_MOTOR_PORT, 
		RIGHT_FRONT_MOTOR_PORT, RIGHT_REAR_MOTOR_PORT)
		{
			SmartDashboard::init();
			GetWatchdog().SetExpiration(0.1);  				// In seconds.
			stick1 = new Joystick(MOVE_JOYSTICK_USB); 		// Joystick 1
			stick2 = new Joystick(LIFT_JOYSTICK_USB);		// Joystick 2
			
			minibot = new MinibotDeployment (
					MINIBOT_DEPLOY_PORT,
					MINIBOT_DEPLOYED_DIO,
					MINIBOT_RETRACTED_DIO);

			lineSensors = new LineSensors (
					LEFT_CAMERA_PORT,
					MIDDLE_CAMERA_PORT,
					RIGHT_CAMERA_PORT);
			
			lift = new LiftController (
					LIFT_MOTOR_PORT,
					HIGH_LIFT_DIO,
					LOW_LIFT_DIO);
			lift->initButtons(
					kJSButton_2,	// Botton top button
					kJSButton_4,	// Left top button
					kJSButton_3, 	// Center top button
					kJSButton_5); 	// Right top button

			
			// The wiring was inverted on the left motors, so the below
			// is necessary.
			robotDrive.SetInvertedMotor(RobotDrive::kFrontLeftMotor, true);
			robotDrive.SetInvertedMotor(RobotDrive::kRearLeftMotor, true);
			
			isFastSpeedOn = false;
			isSafetyModeOn = true;
			isLiftHigh = false;
			// isSafetyModeOn:  Controlled by the driver -- should only have to
			// 					choose once.
			// isLiftHigh: 		Controlled by the program -- turns true only 
			//					when height is too high, otherwise turns false.
			
			isDoingPreset = false;
			
			GetWatchdog().SetEnabled(true);
			UpdateDashboard("TestingTestingTestingTesting"
							"TestingTestingTestingTestingTesting");
			UpdateDashboard("Finished initializing.");
		}

	void OperatorControl(void)
	{		
		GetWatchdog().SetExpiration(.1);
		GetWatchdog().SetEnabled(true);
		GetWatchdog().Feed();

		while (IsOperatorControl())
		{
			GetWatchdog().Feed();	   				
			UpdateDash();
			Wait(.001f);
		}
	}

void RobotBeta1::resetGyro(void) {
	DBG("Enter\n");
	float angle;
	do {
		gyro->Reset();
		angle = gyro->GetAngle();
		DBG("calibrate angle %f\r", angle);
		GetWatchdog().Feed();
		Wait(0.1);
		GetWatchdog().Feed();
	} while (((int)angle) != 0);
	DBG("\nExit\n");
}

	/**
	 * Run the closed loop position controller on the Jag.
	 */
	void OperatorControl()
	{
		printf("In OperatorControl\n");
		GetWatchdog().SetEnabled(true);
		while (IsOperatorControl() && !IsDisabled())
		{
			GetWatchdog().Feed();
			// Set the desired setpoint
			speedJag.Set(stick.GetAxis(axis) * 150.0);
			UpdateDashboardStatus();
			Wait(0.05);
		}
	}

	void Test(void)
	{
		compressor->Start();
		myRobot.SetSafetyEnabled(true);
		GetWatchdog().SetEnabled(true);
		GetWatchdog().SetExpiration(1);
		GetWatchdog().Feed();

		while(IsTest())
		{
			GetWatchdog().Feed();
			Wait(0.1);
		}
	}

	Robot2014(void){
		GetWatchdog().SetExpiration(1);
		GetWatchdog().SetEnabled(false);
		
		drivePad = new Joystick(DRIVEPAD);
		gamePad = new Joystick(GAMEPAD);
		
		driveTrain = new MecanumDrive();
		ballShooter = new Shooter();
		pickupBall = new BallPickup();
		//vision = new VisionSystem();
		
		autonTimer = new Timer();
	}

	void OperatorControl(void) {
		XboxController *xbox = XboxController::getInstance();

		bool isEndGame = false;
		GetWatchdog().SetEnabled(true);
		_driveControl.initialize();
		//_poleVaultControl.initialize();
		shooterControl.InitializeOperator();
		while (IsEnabled() && IsOperatorControl()) {
			GetWatchdog().Feed();
			dsLCD->Clear();
			if (xbox->isEndGame()) {
				isEndGame = !isEndGame;
			}
			if (!isEndGame) {
				shooterControl.Run();
				//_poleVaultControl.act();
				_driveControl.act();
				dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "Normal");
				led0->Set((shooterControl.getLED1())?Relay::kOn: Relay::kOff);
				led1->Set((shooterControl.getLED2())?Relay::kOn: Relay::kOff);
			}

			else {
				shooterControl.RunEndGame();
				//_poleVaultControl.actEndGame();
				_driveControl.actEndGame();
				dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "End Game");
				
				flashCount--;
				
				if (flashCount<=0){
					isFlashing = !isFlashing;
					flashCount=FLASHTIME;
				
				}
				
				
				led0->Set((isFlashing)?Relay::kOn: Relay::kOff);
				led1->Set((isFlashing)?Relay::kOn: Relay::kOff);
			}
//			dsLCD->PrintfLine(DriverStationLCD::kUser_Line6, "Flash: %i", flashCount);
			dsLCD->UpdateLCD();
			Wait(WAIT_TIME); // wait for a motor update time

		}

		GetWatchdog().SetEnabled(false);
	}

	/**
	 * Runs the motors under driver control with either tank or arcade steering selected
	 * by a jumper in DS Digin 0. Also an arm will operate based on a joystick Y-axis. 
	 */
	void OperatorControl(void)
	{
		Victor armMotor(5);						// create arm motor instance
		while (IsOperatorControl())
		{
			GetWatchdog().Feed();

			// determine if tank or arcade mode; default with no jumper is for tank drive
			if (ds->GetDigitalIn(ARCADE_MODE) == 0) {	
				myRobot->TankDrive(leftStick, rightStick);	 // drive with tank style
			} else{
				myRobot->ArcadeDrive(rightStick);	         // drive with arcade style (use right stick)
			}

			// Control the movement of the arm using the joystick
			// Use the "Y" value of the arm joystick to control the movement of the arm
			float armStickDirection = armStick->GetY();

			// if at a limit and telling the arm to move past the limit, don't drive the motor
			if ((armUpperLimit->Get() == 0) && (armStickDirection > 0.0)) {
				armStickDirection = 0;
			} else if ((armLowerLimit->Get() == 0) && (armStickDirection < 0.0)) {
				armStickDirection = 0;
			}

			// Set the motor value 
			armMotor.Set(armStickDirection);
		}
	}

	void OperatorControl(void) {
		GetWatchdog().SetEnabled(true);
		dsLCD->Clear();
		dsLCD->UpdateLCD();
		driveControl.initialize();
		pneumaticsControl->initialize();
		shooterControl->initialize();
		while (IsOperatorControl() && IsEnabled()) {
			GetWatchdog().Feed();
			driveControl.run();
			shooterControl->run();
			dsLCD->UpdateLCD();
			Wait(0.005); // wait for a motor update time
		}
		pneumaticsControl->disable();
	}

	void tune_jog(void)
	{
		GetWatchdog().Feed();
		
		static volatile float time;
		static volatile float pos;
		
		if (t_axis > na) 
			return;
		
		if (t_jog_time[t_axis] < .1) 
			return;

		time += 0.01;
		
		if ((time >= 0) && ( time < t_jog_time[t_axis] )) 
			return;
		
		time = 0;
		
		if (pos == 12) pos = 24;
		else pos = 12;
			
		set_position(t_axis, pos, 1, -.35);
		
		/*
		
		if  (pot_pos[t_axis] < t_pos_dn[t_axis])
		       set_position  (t_axis, t_pos_up[t_axis], t_lim_up[t_axis],t_lim_dn[t_axis]);

		if  (pot_pos[t_axis] > t_pos_up[t_axis])
		       set_position  (t_axis, t_pos_dn[t_axis], t_lim_up[t_axis],t_lim_dn[t_axis]);
		
		*/
	}

	void AutonomousContinuous(void) {
		printf("Running in autonomous continuous...\n");

		GetWatchdog().Feed();
		
		if (kicker->HasBall())
		{
			//We have a ball, thus stop moving and kick the ball
			drivetrain->Drive(0.0, 0.0);
			kicker->SetKickerMode(KICKER_MODE_KICK);
		} else {
			//We do not have a ball
			if (kicker->IsKickerInPosition())
			{
				//Move forward!
				drivetrain->ArcadeDrive(autonomousForwardPower, 0.0);
			} else {
				//If not in position, wait for it to be there...
				drivetrain->ArcadeDrive(0.0, 0.0);
				kicker->SetKickerMode(KICKER_MODE_ARM);
			}
		}
		
		//Run the kicker
		kicker->Act();
	}