C++ Gyro类代码示例

/**
 * Return the actual angle in degrees that the robot is currently facing.
 *
 * The angle is based on the current accumulator value corrected by the oversampling rate, the
 * gyro type and the A/D calibration values.
 * The angle is continuous, that is can go beyond 360 degrees. This make algorithms that wouldn't
 * want to see a discontinuity in the gyro output as it sweeps past 0 on the second time around.
 *
 * @param slot The slot the analog module is connected to
 * @param channel The analog channel the gyro is plugged into
 * @return the current heading of the robot in degrees. This heading is based on integration
 * of the returned rate from the gyro.
 */
float GetGyroAngle(UINT32 slot, UINT32 channel)
{
    Gyro *gyro = AllocateGyro(slot, channel);
    if (gyro)
        return gyro->GetAngle();
    return 0.0;
}

void DemoSceneManager::draw(double deltaT)
{
    _time += deltaT;
    float angle = _time * .1;   // .1 radians per second
    
    glClearColor(0.25f, 0.25f, 0.25f, 1.0f);
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    glEnable(GL_DEPTH_TEST);
    glDepthFunc(GL_LEQUAL);
    
    glCullFace(GL_BACK);
    glEnable(GL_CULL_FACE);
    
    Gyro *gyro = Gyro::getInstance();
    gyro->read();
    
    vmml::mat4f translation = vmml::create_translation(vmml::vec3f(_scrolling.x(), -_scrolling.y(), 0));
    vmml::mat4f scaling = vmml::create_scaling(vmml::vec3f(.2f));
    
    vmml::mat3f rotation = vmml::create_rotation(gyro->getRoll() * -M_PI_F - .3f, vmml::vec3f::UNIT_Y) *
    vmml::create_rotation(gyro->getPitch() * -M_PI_F + .3f, vmml::vec3f::UNIT_X);
    _eyePos = rotation * vmml::vec3f(0.0f, 0.0f, 0.0f);
    vmml::vec3f eyeUp = vmml::vec3f::DOWN;
    _viewMatrix = lookAt(_eyePos, vmml::vec3f::UP, rotation * eyeUp);
    
    _modelMatrix = translation * scaling;

//    drawModel2("quad");
    drawModel("tunnel3");
//    drawModel2("quad");
    _eyePos = rotation * vmml::vec3f(0.0f, 2.0f, 0.0f);
    _viewMatrix = lookAt(_eyePos, vmml::vec3f::UP, rotation * eyeUp);

}

/**
 * Set the gyro type based on the sensitivity.
 * This takes the number of volts/degree/second sensitivity of the gyro and uses it in subsequent
 * calculations to allow the code to work with multiple gyros.
 *
 * @param slot The slot the analog module is connected to
 * @param channel The analog channel the gyro is plugged into
 * @param voltsPerDegreePerSecond The type of gyro specified as the voltage that represents one degree/second.
 */
void SetGyroSensitivity(UINT32 slot, UINT32 channel,
                        float voltsPerDegreePerSecond)
{
    Gyro *gyro = AllocateGyro(slot, channel);
    if (gyro)
        gyro->SetSensitivity(voltsPerDegreePerSecond);
}

	/**
	 * Initialization code for autonomous mode should go here.
	 * 
	 * Use this method for initialization code which will be called each time
	 * the robot enters autonomous mode.
	 */
	void RA14Robot::AutonomousInit() {
		Config::LoadFromFile("config.txt");
		auto_case = (int) Config::GetSetting("auto_case", 1);
		alreadyInitialized = true;
		auto_timer->Reset();
		auto_timer->Start();
		missionTimer->Start();
		myDrive->ResetOdometer();
		myCamera->Set(Relay::kForward);
		myCollection->ExtendArm();
		cout<<"Reseting Gyro"<<endl;
		gyro->Reset();
		//myOdometer->Reset();
		//myDrive->ShiftUp();
		myDrive->ShiftDown();
		//shift to high gear
		if (!fout.is_open()) {
			cout << "Opening logging.csv..." << endl;
			fout.open("logging.csv");
			logheaders();
		}
		auto_state = 0;
#ifndef DISABLE_SHOOTER
		myCam->Reset();
		myCam->Enable();
#endif //Ends DISABLE_SHOOTER
	}

	RobotDemo()
	{
		//Initialize the objects for the drive train
		myRobot = new RobotDrive(1, 2);
		leftEnco = new Encoder(LEFT_ENCO_PORT_1, LEFT_ENCO_PORT_2);
		rightEnco = new Encoder(RIGHT_ENCO_PORT_1, RIGHT_ENCO_PORT_2);
		leftEnco->SetDistancePerPulse((4 * 3.14159)/ENCO_PULSES_PER_REV);
		leftEnco->Start();
		rightEnco->SetDistancePerPulse((4 * 3.14159)/ENCO_PULSES_PER_REV);
		rightEnco->Start();
		leftStick = new Joystick(1);
		rightStick = new Joystick(2);

		//Initialize the gyro
		gyro = new Gyro(GYRO_PORT);
		gyro->Reset();

		//Initialize the manipulators
		intake = new Intake(INTAKE_ROLLER_PORT, BALL_SENSOR_PORT, LEFT_SERVO_PORT, RIGHT_SERVO_PORT);
		catapult = new Catapult(LOADING_MOTOR_PORT, HOLDING_MOTOR_PORT, LOADED_LIMIT_1_PORT,
				LOADED_LIMIT_2_PORT, HOLDING_LIMIT_PORT);

		//Initialize the objects needed for camera tracking
		rpi = new RaspberryPi("17140");
		LEDLight = new Relay(1);
		LEDLight->Set(Relay::kForward);

		//Set the autonomous modes
		autonMode = ONE_BALL_AUTON;
		autonStep = AUTON_ONE_SHOOT;

		//Initialize the lcd
		lcd = DriverStationLCD::GetInstance();
	}

	/**
	 * This autonomous (along with the chooser code above) shows how to select between different autonomous modes
	 * using the dashboard. The sendable chooser code works with the Java SmartDashboard. If you prefer the LabVIEW
	 * Dashboard, remove all of the chooser code and uncomment the GetString line to get the auto name from the text box
	 * below the Gyro
	 *
	 * You can add additional auto modes by adding additional comparisons to the if-else structure below with additional strings.
	 * If using the SendableChooser make sure to add them to the chooser code above as well.
	 */
	void AutonomousInit()
	{
		autoSelected = *((std::string*)chooser->GetSelected());
		//std::string autoSelected = SmartDashboard::GetString("Auto Selector", autoNameDefault);
		std::cout << "Auto selected: " << autoSelected << std::endl;

		rotation = 0.0;
				//*((double*)posChooser->GetSelected());

		//goal = *((std::string*)goalChooser->GetSelected());
		shoot = "No";
		//*((std::string*)shootChooser->GetSelected());

		defenseCrossed = false;
		done = false;


		std::cout << "Here" << std::endl;
		drive->SetMaxOutput(1.0);
		std::cout << "there" << std::endl;
		//Make sure to reset the encoder!
		leftEnc->Reset();
		rightEnc->Reset();
		gyro->Reset();
		autoCounter = 0;
		timer->Reset();
	}

	//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;
	}

	bool GyroDrive(float desiredDriveAngle, float speed, int desiredDistance)
	{
		bool driving = true;
		double encoderInchesTraveled = fabs(leftEnco->GetDistance());//absolute value distance
		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;
		}

		float my_speed = 0.0;
		float turn = 0.0;

		if(speed > 0)
			//30.0 is the number you have to change to adjust properly
			turn = -((myAngle + desiredDriveAngle) / 30.0); //proportionally adjust turn. As the robot gets more off 0, the greater the turn will be
		else
			turn = (myAngle + desiredDriveAngle) / 30.0; //proportionally adjust turn. As the robot gets more off 0, the greater the turn will be

		if (encoderInchesTraveled < desiredDistance)
			my_speed = speed;
		else
		{
			my_speed = 0.0;
			driving = false;
		}

		myRobot->Drive(my_speed, turn);

		return driving;
	}

int main() {
  //Handle Ctrl-C quit
  signal(SIGINT, sig_handler);

  Shield *shield = new Shield();

  //two motor setup
  Motor left_motor(15, 0);
  Motor right_motor(4, 2);

  Gyro gyro;

  IR medA = IR(2, 6149.816568, 4.468768853);
  IR medB = IR(1, 2391.189039, -0.079559138);

  //was 0.015, 0, 0.4
  //also 0.05, 0, 0.2
  //for medA:
  PIDDrive driveA(&left_motor, &right_motor, shield, 0.00001, 0.0001, 0.2);
  PIDDrive driveB(&left_motor, &right_motor, shield, 0.00001, 0.0001, 0.1);

  int straight = 0;
  int turning = 0;
  double curAngle = gyro.get_angle();
  /*double startDist = medA.getDistance();
  double dist[5] = {startDist, startDist, startDist, startDist, startDist};
  double avg = startDist;
  double sum = 0;
*/
  while (running) {
    /*sum = 0;
    for (int i = 1; i <= 4; i++) {
      dist[i-1] = dist[i];
    }
    dist[4] = medA.getDistance();
    for (int i = 0; i < 5; i++) {
       sum +=  dist[i];
    }
    avg = sum / 5.;
    */
    std::cout << "sensor A: " << medA.getDistance() << "\t";
    std::cout << "sensor B: " << medB.getDistance() << std::endl;    
//std::cout << "avg: " << avg << std::endl;

      /*if (medA.getDistance() < 20 && medB.getDistance() > 30) {
      turning = 0;
      if (straight == 0) {
        curAngle = gyro.get_angle();
      }
      straight++;
      driv.drive(curAngle, gyro.get_angle(), 0.25); //keep driving straight
      std::cout << "straight\t" << "angle: " << curAngle <<  std::endl;
    } else if ((medA.getDistance() < 20 && medB.getDistance() < 30) || (medA.getDistance() > 30)) {
      straight = 0;
      if (turning == 0) {
        curAngle = gyro.get_angle();
      }
      turning++;
      //drive.drive(curAngle + 10, gyro.get_angle(), .2); //turn away from wall
      drive.stop();
      straight = 0;
      sleep(.2);
      std::cout << "turning " << "angle: " << curAngle << std::endl;
    } else {
      drive.drive(gyro.get_angle(), gyro.get_angle(), 0.2);
    }*/
    
    if (medB.getDistance() < 15) {
      driveA.stop();
      driveB.stop();
      while (medB.getDistance() < 30) {
        left_motor.setSpeed(shield, 0.2);
        right_motor.setSpeed(shield, -0.2);
        std::cout << "B" << std::endl;
      }
    } else if (medA.getDistance() > 80) {
      left_motor.setSpeed(shield, 0.2);
      right_motor.setSpeed(shield, -0.2);
      usleep(300000);
    } else {
      turning = 0;
      driveA.drive(15, medA.getDistance(), 0.2);
      std::cout << "A" << std::endl;
      usleep(100000);
    }

    /*
    if (medA.getDistance() < 14) {
       left_motor.setSpeed(shield, 0.2);
       right_motor.setSpeed(shield, -0.15);
    } else if (medA.getDistance() > 16) {
       left_motor.setSpeed(shield, -0.15);
       right_motor.setSpeed(shield, 0.2);
    } else {
       left_motor.setSpeed(shield, 0.2);
       right_motor.setSpeed(shield, 0.2);
    }
    */
    /*if (avg < 30) {
      drive.drive(gyro.get_angle(), gyro.get_angle(), -.25);
//.........这里部分代码省略.........

	/**
	 * Runs the motors with Mecanum drive.
	 */
	void OperatorControl()//teleop code
	{
		robotDrive.SetSafetyEnabled(false);
		gyro.Reset();
		grabEncoder.Reset();
		timer.Start();
		timer.Reset();
		double liftHeight = 0; //variable for lifting thread
		int liftHeightBoxes = 0; //another variable for lifting thread
		int liftStep = 0; //height of step in inches
		int liftRamp = 0; //height of ramp in inches
		double grabPower;
		bool backOut;
		uint8_t toSend[10];//array of bytes to send over I2C
		uint8_t toReceive[10];//array of bytes to receive over I2C
		uint8_t numToSend = 1;//number of bytes to send
		uint8_t numToReceive = 0;//number of bytes to receive
		toSend[0] = 1;//set the byte to send to 1
		i2c.Transaction(toSend, 1, toReceive, 0);//send over I2C
		bool isGrabbing = false;//whether or not grabbing thread is running
		bool isLifting = false;//whether or not lifting thread is running
		bool isBraking = false;//whether or not braking thread is running
		float driveX = 0;
		float driveY = 0;
		float driveZ = 0;
		float driveGyro = 0;
		bool liftLastState = false;
		bool liftState = false; //button pressed
		double liftLastTime = 0;
		double liftTime = 0;
		bool liftRan = true;
		Timer switchTimer;
		Timer grabTimer;
		switchTimer.Start();
		grabTimer.Start();


		while (IsOperatorControl() && IsEnabled())
		{
			// Use the joystick X axis for lateral movement, Y axis for forward movement, and Z axis for rotation.
			// This sample does not use field-oriented drive, so the gyro input is set to zero.

			toSend[0] = 1;
			numToSend = 1;


			driveX = driveStick.GetRawAxis(Constants::driveXAxis);//starts driving code
			driveY = driveStick.GetRawAxis(Constants::driveYAxis);
			driveZ = driveStick.GetRawAxis(Constants::driveZAxis);
			driveGyro = gyro.GetAngle() + Constants::driveGyroTeleopOffset;


			if (driveStick.GetRawButton(Constants::driveOneAxisButton)) {//if X is greater than Y and Z, then it will only go in the direction of X
				toSend[0] = 6;
				numToSend = 1;

				if (fabs(driveX) > fabs(driveY) && fabs(driveX) > fabs(driveZ)) {
					driveY = 0;
					driveZ = 0;
				}
				else if (fabs(driveY) > fabs(driveX) && fabs(driveY) > fabs(driveZ)) {//if Y is greater than X and Z, then it will only go in the direction of Y
					driveX = 0;
					driveZ = 0;
				}
				else {//if Z is greater than X and Y, then it will only go in the direction of Z
					driveX = 0;
					driveY = 0;
				}
			}

			if (driveStick.GetRawButton(Constants::driveXYButton)) {//Z lock; only lets X an Y function
				toSend[0] = 7;
				driveZ = 0;//Stops Z while Z lock is pressed
			}

			if (!driveStick.GetRawButton(Constants::driveFieldLockButton)) {//robot moves based on the orientation of the field
				driveGyro = 0;//gyro stops while field lock is enabled
			}

			driveX = Constants::scaleJoysticks(driveX, Constants::driveXDeadZone, Constants::driveXMax * (.5 - (driveStick.GetRawAxis(Constants::driveThrottleAxis) / 2)), Constants::driveXDegree);
			driveY = Constants::scaleJoysticks(driveY, Constants::driveYDeadZone, Constants::driveYMax * (.5 - (driveStick.GetRawAxis(Constants::driveThrottleAxis) / 2)), Constants::driveYDegree);
			driveZ = Constants::scaleJoysticks(driveZ, Constants::driveZDeadZone, Constants::driveZMax * (.5 - (driveStick.GetRawAxis(Constants::driveThrottleAxis) / 2)), Constants::driveZDegree);
			robotDrive.MecanumDrive_Cartesian(driveX, driveY, driveZ, driveGyro);//makes the robot drive




			if (pdp.GetCurrent(Constants::grabPdpChannel) < Constants::grabManualCurrent) {
				pickup.setGrabber(Constants::scaleJoysticks(grabStick.GetX(), Constants::grabDeadZone, Constants::grabMax, Constants::grabDegree)); //defines the grabber
				if(grabTimer.Get() < 1) {
					toSend[0] = 6;
				}
			}
			else {
				pickup.setGrabber(0);
				grabTimer.Reset();
				toSend[0] = 6;
//.........这里部分代码省略.........

	/**
	 * Periodic code for autonomous mode should go here.
	 *
	 * Use this method for code which will be called periodically at a regular
	 * rate while the robot is in autonomous mode.
	 */
	void RA14Robot::AutonomousPeriodic() {
		StartOfCycleMaintenance();

		target->Parse(server->GetLatestPacket());
		float speed = Config::GetSetting("auto_speed", .5);
		cout<<"Auto Speed: "<<Config::GetSetting("auto_speed", 0)<<endl; // original .1
		float angle = gyro->GetAngle();
		float error = targetHeading - angle;
		float corrected = error * Config::GetSetting("auto_heading_p", .01);
		//float corrected = error * Config::GetSetting("auto_heading_p", .01);
		cout <<"Gyro angle: "<<angle<<endl;
		cout <<"Error: " << error << endl;
		//float lDrive = Config::GetSetting("auto_speed", -0.3) + (error * Config::GetSetting("auto_heading_p", .01));
		//float rDrive = Config::GetSetting("auto_speed", -0.3) - (error * Config::GetSetting("auto_heading_p", .01));
		// Reading p value from the config file does not appear to be working. When we get p from config, the math is not correct.
		float lDrive = Config::GetSetting("auto_speed", -0.3) + (error*0.01);
		float rDrive = Config::GetSetting("auto_speed", -0.3) - (error*0.01);
		cout << "Left: " << lDrive << endl;
		cout << "Right: " << rDrive << endl;
									
		
#ifndef DISABLE_AUTONOMOUS
		switch(auto_case)
		{
			case 0:
				// start master autonomous mode
				switch (auto_state) {
				case 0: // start
					auto_timer->Reset();
					auto_timer->Start();
					myCam->Process(false, false, false);
					break;
				case 1:
					myCam->Process(false, false, false);
					if (target->IsValid()) {
						auto_state = 2;
					} else if (auto_timer->Get() >= Config::GetSetting("auto_target_timeout", 1)) {
						auto_state = 10;
					}
					break;
				case 2:
					myCam->Process(false, false, false);
					if (target->IsHot()) {
						auto_state = 10;
					} else {
						if (auto_timer->Get() >= Config::GetSetting("auto_target_hot_timeout", 5)) {
							auto_state = 10;
						}
					}
					break;
				case 10:
					myCam->Process(true, false, false);
					if (myCam->IsReadyToRearm()) {
						auto_state = 11;
					}
					break;
				case 11:
					myCam->Process(false, false, false);
					myDrive->DriveArcade(corrected, speed);
					if (myDrive->GetOdometer() >= Config::GetSetting("auto_drive_distance", 100))
					{
						myDrive->Drive(0,0);
					}
					break;
				case 12:
					myDrive->Drive(0,0);
					break;
				default:
					cout << "Unknown state #" << auto_state << endl; 
					break;
				}
				// end master autonomous mode
				break;
			case 1:
				if( target->IsHot() && target->IsValid() )
				{
					cout << "Target is HOTTT taking the shot" << endl;
					//Drive forward and shoot right away
					//if( target->IsLeft() || target->IsRight() )
					//{
					if(myDrive->GetOdometer() <= 216 - Config::GetSetting("auto_firing_distance", 96)) //216 is distance from robot to goal
					{
						myDrive->Drive(corrected,speed);
					}
					else
					{
						myDrive->Drive(0,0);
						cout << "FIRING" << endl;
#ifndef DISABLE_SHOOTER
						myCam->Process(1,0,0);
#endif //Ends DISABLE_SHOOTER
					}
				/*}
				else
//.........这里部分代码省略.........

	void Autonomous()
	{
		Timer timer;
		float power = 0;
		bool isLifting = false;
		bool isGrabbing = false;
		double liftHeight = Constants::liftBoxHeight-Constants::liftBoxLip;
		double grabPower = Constants::grabAutoCurrent;
		bool backOut;

		uint8_t toSend[1];//array of bytes to send over I2C
		uint8_t toReceive[0];//array of bytes to receive over I2C
		uint8_t numToSend = 1;//number of bytes to send
		uint8_t numToReceive = 0;//number of bytes to receive
		toSend[0] = 2;//set the byte to send to 1
		i2c.Transaction(toSend, numToSend, toReceive, numToReceive);//send over I2C

		bool isSettingUp = true;

		//pickup.setGrabber(-1); //open grabber all the way
		pickup.setLifter(0.8);

		while (isSettingUp && IsEnabled() && IsAutonomous()) {
			isSettingUp = false;
			/*if (grabOuterLimit.Get() == false) {
				pickup.setGrabber(0); //open until limit
			}
			else {
				isSettingUp = true;
			}*/

			if (liftLowerLimit.Get()) {
				pickup.setLifter(0); //down till bottom
			}
			else {
				isSettingUp = true;
			}
		}

		gyro.Reset();
		liftEncoder.Reset();
		grabEncoder.Reset();

		if (grabStick.GetZ() > .8) {
			timer.Reset();
			timer.Start();
			while (timer.Get() < 1) {
				robotDrive.MecanumDrive_Cartesian(0, power, 0, gyro.GetAngle());	// drive back
				if(power>-.4){
					power-=0.005;
					Wait(.005);
				}
			}
			robotDrive.MecanumDrive_Cartesian(0, 0, 0, gyro.GetAngle());	// STOP!!!
			timer.Stop();
			timer.Reset();
			Wait(1);
		}
		power = 0;

		while (isLifting && IsEnabled() && IsAutonomous()) {
			Wait(.005);
		}

		backOut = Constants::autoBackOut;
		pickup.grabberGrab(isGrabbing, grabPower, backOut, grabStick);
		Wait(.005);

		while (isGrabbing && IsEnabled() && IsAutonomous()) {
			Wait(.005);
		}

		liftHeight = 3*Constants::liftBoxHeight;
		Wait(.005);
		pickup.lifterPosition(liftHeight, isLifting, grabStick);
		Wait(.005);
		while (isLifting && IsEnabled() && IsAutonomous()) {
			Wait(.005);
		}

		while(prox.GetVoltage() * Constants::ultrasonicVoltageToInches / 12 < 2 && IsEnabled() && IsAutonomous());	// while the nearest object is closer than 2 feet

		timer.Start();

		while(prox.GetVoltage() * Constants::ultrasonicVoltageToInches  < Constants::autoBackupDistance && timer.Get() < Constants::autoMaxDriveTime && IsEnabled() && IsAutonomous()) {	// while the nearest object is further than 12 feet
			if (power < .45) { //ramp up the power slowly
				power += .00375;
			}
			robotDrive.MecanumDrive_Cartesian(0, power, 0, gyro.GetAngle());	// drive back
			float distance = prox.GetVoltage() * Constants::ultrasonicVoltageToInches / 12;	// distance from ultrasonic sensor
			SmartDashboard::PutNumber("Distance", distance);	// write stuff to smart dash
			SmartDashboard::PutNumber("Drive Front Left Current", pdp.GetCurrent(Constants::driveFrontLeftPin));
			SmartDashboard::PutNumber("Drive Front Right Current", pdp.GetCurrent(Constants::driveFrontRightPin));
			SmartDashboard::PutNumber("Drive Rear Left Current", pdp.GetCurrent(Constants::driveRearLeftPin));
			SmartDashboard::PutNumber("Drive Rear Right Current", pdp.GetCurrent(Constants::driveRearRightPin));
			SmartDashboard::PutNumber("Gyro Angle", gyro.GetAngle());
			SmartDashboard::PutNumber("Distance (in)", prox.GetVoltage() * Constants:: ultrasonicVoltageToInches);

			Wait(.005);
		}
//.........这里部分代码省略.........

	void TeleopPeriodic()
	{
		//camera->GetImage(frame);
		//imaqDrawShapeOnImage(frame, frame, { 10, 10, 100, 100 }, DrawMode::IMAQ_DRAW_VALUE, ShapeMode::IMAQ_SHAPE_OVAL, 0.0f);
		//CameraServer::GetInstance()->SetImage(frame);


		printf("Left Encoder: %i, Right Encoder: %i, Gyro: %f\n", leftEnc->Get(), rightEnc->Get(), gyro->GetAngle());

		drive->ArcadeDrive(driveStick);
		drive->SetMaxOutput((1-driveStick->GetThrottle())/2);
		//printf("%f\n", (1-stick->GetThrottle())/2);
		//leftMotor->Set(0.1);
		//rightMotor->Set(0.1);

		if (shootStick->GetRawAxis(3) > 0.5) {
			launch1->Set(1.0);
			launch2->Set(1.0);
		} else if (shootStick->GetRawAxis(2) > 0.5) {
			printf("Power Counter: %i\n", powerCounter);
			if (powerCounter < POWER_MAX) {
				powerCounter++;
				launch1->Set(-0.8);
				launch2->Set(-0.8);
			} else {
				launch1->Set(-0.6);
				launch2->Set(-0.6);
			}
		} else {
			launch1->Set(0.0);
			launch2->Set(0.0);
			powerCounter = 0.0;
		}

		//use this button to spin only one winch, to lift up.
		 if (shootStick->GetRawButton(7)) {
		 	otherWinch->Set(0.5);
		 } else if (shootStick->GetRawButton(8)) {
			 otherWinch->Set(-0.5);
		 } else {
		 	otherWinch->Set(0.0);
		 }

		if (shootStick->GetRawButton(5)) {
			winch->Set(-0.7);

			if (!shootStick->GetRawButton(7) && !shootStick->GetRawButton(8)) {
//				otherWinch->Set(-0.5);
			}
		} else if (shootStick->GetRawButton(6)) {
			winch->Set(0.7);
			if (!shootStick->GetRawButton(7) && !shootStick->GetRawButton(8)) {
//				otherWinch->Set(0.5);
			}
		} else {
			winch->Set(0.0);
			if (!shootStick->GetRawButton(7) && !shootStick->GetRawButton(8)) {
//,				otherWinch->Set(0.0);
			}
		}
		

		if (shootStick->GetRawButton(1)) {
			launchPiston->Set(1);
		} else {
			launchPiston->Set(0);
		}

		if (shootStick->GetRawButton(3)) {
			Autonomous::alignWithGoal(drive, launch1, launch2, winch, otherWinch, table, timer);
		}

		if (shootStick->GetRawButton(3) && debounce == false) {
			debounce = true;
			if (defenseUp) {
				defensePiston->Set(DoubleSolenoid::Value::kReverse);
				defenseUp = false;
			} else {
				defenseUp =true;
				defensePiston->Set(DoubleSolenoid::Value::kForward);
			}
		} else if (!shootStick->GetRawButton(3)){
			debounce = false;
		}
	}

	/****************************************
	 * Runs the motors with arcade steering.*
	 ****************************************/
	void OperatorControl(void)
	{
//TODO put in servo for lower camera--look in WPI to set	
//		Watchdog baddog;
		
	//	baddog.Feed();
		myRobot.SetSafetyEnabled(true);
		//SL Earth.Start(); // turns on Earth
//		SmartDashboard *smarty = SmartDashboard::GetInstance();
		//DriverStationLCD *dslcd = DriverStationLCD::GetInstance(); // don't press SHIFT 5 times; this line starts up driver station messages (in theory)
		//char debugout [100];
		compressor.Start();
		gyro.Reset(); // resets gyro angle
		int rpmForShooter;

		
		while (IsOperatorControl()) // while is the while loop for stuff; this while loop is for "while it is in Teleop"
		{ 
//			baddog.Feed();
			//myRobot.SetSafetyEnabled(true);
			//myRobot.SetExpiration(0.1);
			float leftYaxis = driver.GetY();
			float rightYaxis = driver.GetTwist();//RawAxis(5);
			myRobot.TankDrive(leftYaxis,rightYaxis); // drive with arcade style (use right stick)for joystick 1
			float random = gamecomponent.GetY();
			float lazysusan = gamecomponent.GetZ();
			//bool elevator = Frodo.Get();
			float angle = gyro.GetAngle();
			bool balance = Smeagol.Get();
			SmartDashboard::PutNumber("Gyro Value",angle);
			int NumFail = -1;
			//bool light = Pippin.Get();
			//SL float speed = Earth.GetRate();
			//float number = shooter.Get();
			//bool highspeed = button1.Get()
			//bool mediumspeed = button2.Get();
			//bool slowspeed = button3.Get();
			bool finder = autotarget.Get();
			//bool targetandspin = autodistanceandspin.Get();
			SmartDashboard::PutString("Targeting Activation","");
			//dslcd->Clear();
			//sprintf(debugout,"Number=%f",angle); 
			//dslcd->Printf(DriverStationLCD::kUser_Line2,2,debugout);
			//SL sprintf(debugout,"Number=%f",speed);
			//SL dslcd->Printf(DriverStationLCD::kUser_Line4,4,debugout);
			//sprintf(debugout,"Number=%f",number);
			//dslcd->Printf(DriverStationLCD::kUser_Line1,1,debugout);
			//sprintf(debugout,"Finder=%u",finder);
			//dslcd->Printf(DriverStationLCD::kUser_Line5,5,debugout);
			//dslcd->UpdateLCD(); // update the Driver Station with the information in the code
		    // sprintf(debugout,"Number=%u",maxi);
			// dslcd->Printf(DriverstationLCD::kUser_Line6,5,debugout)
						bool basketballpusher = julesverne.Get();
						bool bridgetipper = joystickbutton.Get();
						if (bridgetipper) // if joystick button 7 is pressed (is true)
						{	
							solenoid.Set(true); // then the first solenoid is on
						}
						
							else
							{
							//Wait(0.5); // and then the first solenoid waits for 0.5 seconds
							solenoid.Set(false); //and then the first solenoid turns off
						}
						if (basketballpusher) // if joystick button 6 is pressed (is true)
						{
							shepard.Set(true); // then shepard is on the run
							//Wait(0.5); // and shepard waits for 0.5 seconds
						}
							else
							{		
							shepard.Set(false); // and then shepard turns off
						} //10.19.67.9 IP address of computer;255.0.0.0 subnet mask ALL FOR WIRELESS CONNECTION #2			

			//}	
			//cheetah.Set(0.3*lazysusan);
//			smarty->PutDouble("pre-elevator",lynx.Get());
			lynx.Set(random);
//			smarty->PutDouble("elevator",lynx.Get());
			
//			smarty->PutDouble("joystick elevator",random);
			
			
			if (balance)						// this is the start of the balancing code
			{
				angle = gyro.GetAngle();
				myRobot.Drive(-0.03*angle, 0.0);
				Wait(0.005);
			}
			/*if (light)							//button 5 turns light on oand off on game controller
			     flashring.Set(Relay::kForward);
			     else
			            flashring.Set(Relay::kOff);
			 */           
			if (finder)
			{
				flashring.Set(Relay::kForward);
//.........这里部分代码省略.........

	void OperatorControl()
	{
		float xJoy, yJoy, gyroVal, angle = 0, turn = 0, angleDiff, turnPower;
		gyro.Reset();
		gyro.SetSensitivity(9.7);
		while (IsEnabled() && IsOperatorControl()) // loop as long as the robot is running
		{
			yJoy = xbox.getAxis(Xbox::L_STICK_V);
			xJoy = xbox.getAxis(Xbox::R_STICK_H);			
			gyroVal = gyro.GetAngle()/0.242*360;
			turn = 0.15;
			angle = angle - xJoy * xJoy * xJoy * turn;
			angleDiff = mod(angle - gyroVal, 360);
			turnPower = - mod(angleDiff / 180.0 + 1.0, 2) + 1.0;
			SmartDashboard::PutString("Joy1", vectorToString(xJoy, yJoy));
			SmartDashboard::PutNumber("Heading", mod(gyroVal, 360));
			SmartDashboard::PutNumber("Turn Power", turnPower);
			SmartDashboard::PutBoolean("Switch is ON:", dumperSwitch.Get());
			
			if (!xbox.isButtonPressed(Xbox::R))
			{
				drive.ArcadeDrive(yJoy * yJoy * yJoy, turnPower * fabs(turnPower), false);				
			}
		}
	}