/**
	 * Runs the motor from the output of a Joystick.
	 */
	void OperatorControl() {
		while (IsOperatorControl() && IsEnabled()) {
			// Set the motor controller's output.
			// This takes a number from -1 (100% speed in reverse) to +1 (100% speed forwards).
			m_motor.Set(m_stick.GetY());

			Wait(kUpdatePeriod); // Wait 5ms for the next update.
		}
	}

	//
	// Main Tele Operator Mode function.  This function is called once, therefore a while loop that checks IsOperatorControl and IsEnabled is used 
	// to maintain control until the end of tele operator mode
	//
	void OperatorControl()
	{
		//myRobot.SetSafetyEnabled(true);
		
		timer->Start();
		Relay* reddlight = new Relay(4);
		//Timer* lighttimer = new Timer();
		//lighttimer->Start();
		while (IsOperatorControl() && IsEnabled())
		{
			reddlight->Set(reddlight->kForward);
			/*
			if (lighttimer->Get()<=0.5) {
				reddlight->Set(reddlight->kForward);
			}
			else if(lighttimer->Get()<1){
				reddlight->Set(reddlight->kOff);
			}
			else {
				lighttimer->Reset();
			}
			*/
			//
			// Get inputs
			//
			driverInput->GetInputs();
			drive->GetInputs();
			catapult->GetInputs();
			feeder->GetInputs();
			
			//
			// Pass values between components as necessary
			//
			//catapult->SetSafeToFire(feeder->GetAngle()<95); 
			
			//
			// Execute one step on each component
			//
			drive->ExecStep(); 
			catapult->ExecStep();
			feeder->ExecStep(); 
			
		    //
			// Set Outputs on all components
			//
			catapult->SetOutputs();
			feeder->SetOutputs();
			
			//
			// Wait for step timer to expire.  This allows us to control the amount of time each step takes. Afterwards, restart the 
			// timer for the next loop
			//
			while (timer->Get()<(PERIOD_IN_SECONDS));
			timer->Reset();

		}
	}

void WorkshopRobot::OperatorControl(void){
	printf("2250 start\n"); 					//Check in with the console.
	while (IsOperatorControl()){ 				//Called once at the beggining of the teleop mode.
		//printf("Feed!\n");
		driveSys->RunDriveSystem();
		LauncherMech->RunLauncher();
		Wait(0.005); 							//Minimum motor controller wait time. (also handy for ticks)
	}
}

	void OperatorControl(void)
	{
		while (IsOperatorControl())
		{
			driveTrain->Drive();
			pickupBall->Pickup();
			ballShooter->ShootBall();
		}
	}

	/**
	 * Runs the motors with arcade steering. 
	 */
	void OperatorControl(void)
	{
		myRobot.SetSafetyEnabled(true);
		while (IsOperatorControl())
		{
			myRobot.ArcadeDrive(stick); // drive with arcade style (use right stick)
			Wait(0.005);				// wait for a motor update time
		}
	}

	/**
	 * 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)
		}
	}

void TestBed::OperatorControl(void)
{
	while (IsOperatorControl() && !IsDisabled())
	{
		DriveSys->RemoteDrive();
		SmartDashboard::PutNumber("LeftStick: ", DriveSys->LeftStickY);
		SmartDashboard::PutNumber("RightStick:", DriveSys->RightStickY);
		Wait(0.005);// wait for a motor update time
	}
}

	/**
	 * Runs the motors with arcade steering. 
	 */
	void OperatorControl(void)
	{
		float speed = 0;
		while (IsOperatorControl())
		{
			myRobot.TankDrive(leftstick, rightstick);
			//driveleft.Set(1);  //Why are we setting driveleft and driveright to 1 and -1?
			//driveright.Set(-1);  
		}
	}

	void TeleopInit()
	{
		while(IsOperatorControl())
		{
			intake();
			shoot();
		
		}
		
	}

	/**
	 * Runs the motors with arcade steering.
	 */
	void OperatorControl()
	{
		while (IsOperatorControl() && IsEnabled())
		{

			myRobot.ArcadeDrive(stick); // drive with arcade style (use right stick)
			//camera.GetImage(Image); 	//get the most recent image from the camera
			Wait(0.005);				// wait for a motor update time
		}
	}

void OperatorControl(void)
{
	SetWatchdogEnabled(true);
	while (IsOperatorControl())
	{
		WatchdogFeed();
		ArcadeDrive(JOYSTICK_PORT);
		Wait(0.005);
	}
}

		/**
		 * 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()
		{
			control->initialize();
			while (IsOperatorControl())
			{
				control->run();
				dsLCD->UpdateLCD();
				Wait(0.005);
			}
		}

	// Code to be run during the remaining 2:20 of the match (after Autonomous())
	//
	// OperatorControl
	//	* Calls all the above methods
	void OperatorControl()
	{
		// SAFETY AND SANITY - SET ALL TO ZERO
		intake.Set(0.0);
		rightWinch.Set(0.0);
		leftWinch.Set(0.0);

		arm.Set(DoubleSolenoid::kReverse);

		/* TODO: Investigate. At least year's (GTR East) competition, we reached the conclusion that disabling this was 
		 * the only way we could get out robot code to work (reliably). Should this be set to false?
		 */ 
		robotDrive.SetSafetyEnabled(false);

		Timer clock;
		int sanity = 0;
		int bigSanity = 0;

		loading = false;
		loaded = winchSwitch.Get();

		RegisterButtons();
		gamepad.Update();
		leftStick.Update();

		compressor.Start();

		while (IsOperatorControl() && IsEnabled())
		{
			clock.Start();

			HandleDriverInputs();
			HandleShooter();
			HandleArm();
			//			HandleEject();

			while (!clock.HasPeriodPassed(LOOP_PERIOD)); // add an IsEnabled???
			clock.Reset();
			sanity++;
			if (sanity >= 100)
			{
				bigSanity++;
				sanity = 0;
				dsLCD->PrintfLine(DriverStationLCD::kUser_Line4, "%d", bigSanity);
			}
			gamepad.Update();
			leftStick.Update();
			dsLCD->UpdateLCD();
		}

		// SAFETY AND SANITY - SET ALL TO ZERO
		intake.Set(0.0);
		rightWinch.Set(0.0);
		leftWinch.Set(0.0);
	}

    /**
     * Drive based upon joystick inputs, and automatically control
     * motors if the robot begins tipping.
     */
    void OperatorControl()
    {
        robotDrive.SetSafetyEnabled(false);
        while (IsOperatorControl() && IsEnabled()) {

            double xAxisRate = stick.GetX();
            double yAxisRate = stick.GetY();
            double pitchAngleDegrees = ahrs->GetPitch();
            double rollAngleDegrees = ahrs->GetRoll();

            if ( !autoBalanceXMode &&
                 (fabs(pitchAngleDegrees) >=
                  fabs(kOffBalanceThresholdDegrees))) {
                autoBalanceXMode = true;
            }
            else if ( autoBalanceXMode &&
                      (fabs(pitchAngleDegrees) <=
                       fabs(kOnBalanceThresholdDegrees))) {
                autoBalanceXMode = false;
            }
            if ( !autoBalanceYMode &&
                 (fabs(pitchAngleDegrees) >=
                  fabs(kOffBalanceThresholdDegrees))) {
                autoBalanceYMode = true;
            }
            else if ( autoBalanceYMode &&
                      (fabs(pitchAngleDegrees) <=
                       fabs(kOnBalanceThresholdDegrees))) {
                autoBalanceYMode = false;
            }

            // Control drive system automatically,
            // driving in reverse direction of pitch/roll angle,
            // with a magnitude based upon the angle

            if ( autoBalanceXMode ) {
                double pitchAngleRadians = pitchAngleDegrees * (M_PI / 180.0);
                xAxisRate = sin(pitchAngleRadians) * -1;
            }
            if ( autoBalanceYMode ) {
                double rollAngleRadians = rollAngleDegrees * (M_PI / 180.0);
                yAxisRate = sin(rollAngleRadians) * -1;
            }

            try {
                // Use the joystick X axis for lateral movement, Y axis for forward movement, and Z axis for rotation.
                robotDrive.MecanumDrive_Cartesian(xAxisRate, yAxisRate,stick.GetZ());
            } catch (std::exception ex ) {
                std::string err_string = "Drive system error:  ";
                err_string += ex.what();
                DriverStation::ReportError(err_string.c_str());
            }
            Wait(0.005); // wait 5ms to avoid hogging CPU cycles
        }
    }

	/**
	 *  Tells the robot to drive to a set distance (in inches) from an object using
	 *  proportional control.
	 */
	void OperatorControl() {

		double currentDistance; //distance measured from the ultrasonic sensor values
		double currentSpeed; //speed to set the drive train motors

		while (IsOperatorControl() && IsEnabled()) {
			currentDistance = ultrasonic->GetValue() * valueToInches; //sensor returns a value from 0-4095 that is scaled to inches
			currentSpeed = (holdDistance - currentDistance) * pGain; //convert distance error to a motor speed
			myRobot->Drive(currentSpeed, 0); //drive robot
		}
	}

	/**
	 * Runs the motors with arcade steering. 
	 */
	void OperatorControl(void) {

		while (IsOperatorControl()) {
			dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, "Voltage: %f",
					signal.GetVoltage());
			dsLCD->PrintfLine(DriverStationLCD::kUser_Line3, "CVoltage: %f",
					signalControlVoltage.GetVoltage());
			dsLCD->UpdateLCD();
			Wait(0.005); // wait for a motor update time
		}
	}

void MainRobot::OperatorControl() {
    mWatchdog->SetEnabled(true);

    while (IsOperatorControl()) {
        mJoystickController->Run();
        GetWatchdog().Feed();
        Wait(cMotorWait);
    }

    return;
}

void Michael1::OperatorControl(void)
{
	printf("\n\n\tStart Teleop:\n\n");
	GetWatchdog().SetEnabled(false);
	ariels_light->Set(0);
	
	while (IsOperatorControl())
	{
		dt->TankDrive(left_stick, right_stick);
	}
}

	/**
	 * Runs the motors with arcade steering. 
	 */
	void OperatorControl(void)
	{
		myRobot.SetSafetyEnabled(true);
		while (IsOperatorControl())
		{
			myRobot.ArcadeDrive(stick.getAxisLeftY(), stick.getAxisLeftX()); // drive with arcade style (use left stick)
			dsLCD->Printf(DriverStationLCD::kUser_Line2, 1, "Move: %f4", stick.getAxisLeftY());
			dsLCD->Printf(DriverStationLCD::kUser_Line3, 1, "Rotate: %f4", stick.getAxisLeftX());
			dsLCD->UpdateLCD();
			Wait(0.005);				// wait for a motor update time
		}
	}

	void OperatorControl()
	{
		tank.StartDrive();
		kick.StartKicker();
		while (IsOperatorControl())
		{
			comp.checkCompressor();
			tank.Drive(PrimaryController.GetRawAxis(LEFT_JOYSTICK), PrimaryController.GetRawAxis(RIGHT_JOYSTICK), (int)PrimaryController.GetRawButton(BUTTON_HIGH_DRIVE_SHIFT), (int)PrimaryController.GetRawButton(BUTTON_LOW_DRIVE_SHIFT));
			kick.StateMachine(PrimaryController.GetRawButton(BUTTON_KICK));
			collect.runCollector(PrimaryController.GetRawButton(BUTTON_ROLLER_ON));
		}
	}