C++ ArRobot::setDeltaHeading方法代码示例

void SensorDetectPopup::popupClosed(ArTypes::Byte4 popupID, int button)
{
  // A client closed the popup
  ArLog::log(ArLog::Normal, "popupExample: a client closed popup dialog window with id=%d. Button=%d...", popupID, button);
  myPopupDisplayed = false;

  if(button < 0)
  {
    ArLog::log(ArLog::Normal, "\t...popup timed out or closed due to an error.");
    return;
  }

  if (button == 0)
  {
    ArLog::log(ArLog::Normal, "\t...OK pressed.");
    return;
  }

  if(button == 1)
  {
    ArLog::log(ArLog::Normal, "\t...180 degree rotate requested.");
    myRobot->lock();
    myRobot->setDeltaHeading(180);
    myRobot->unlock();
    return;
  }

  if(button == 2)
  {
    ArLog::log(ArLog::Normal, "\t...exit requested.");
    myRobot->stopRunning();
    Aria::shutdown();
    Aria::exit(0);
  }
}

ArActionDesired* ActionReadSonar::fire(ArActionDesired currentDesired)
{


	ArRobot *robot = this->getRobot();
	
	int total = robot->getNumSonar(); // get the total number of sonar on the robot
	ArSensorReading* value; // This class abstracts range and angle read from sonar

	//cout << " 0 : " << robot->getSonarReading(0)->getSensorTh() << " 1 : " << robot->getSonarReading(1)->getSensorTh() 
	//	 << " 2 : " << robot->getSonarReading(2)->getSensorTh() << " 3 : " << robot->getSonarReading(3)->getSensorTh()
	//	 << " 4 : " << robot->getSonarReading(4)->getSensorTh() << " 5 : " << robot->getSonarReading(5)->getSensorTh()
	//	 << " 6 : " << robot->getSonarReading(6)->getSensorTh() << " 7 : " << robot->getSonarReading(7)->getSensorTh()
		 
	//	 << "r :" << robot->getTh() << endl;


	double limit = 800;
	double distance;

	// reset the actionDesired (must be done), to clear
	// its previous values.
	myDesired.reset();

	// if the sonar is null we can't do anything, so deactivate
	if (mySonar == NULL)
	{
		deactivate();
		return NULL;
	}

	// gets value of object between -20 degrees and 20 degrees of foward
	double angle = 0;
	distance = mySonar->currentReadingPolar(-20, 20, &angle);
	//cout << "distance from nearest object =" << distance << endl;
	

	if (distance <= limit) {
		int heading = 15;

		//cout << "angle :" << angle << endl;
		if (angle > 10) {
			heading = -heading;
		}
		else if (angle < -10) {
			heading = heading;
		}
		
		//cout << "x" << robot->getX() << " ," << robot->getY() << endl;
		cout << "distance from nearest object =" << distance << endl;

		
		robot->lock();
		robot->setVel(0);
		robot->unlock();

		robot->lock();
		robot->setDeltaHeading(heading);
		robot->unlock();

		ArUtil::sleep(50);
	}

	return &myDesired;
}

//.........这里部分代码省略.........
					{
						int l_edge = target.l_edge;
						int r_edge = target.r_edge;
						float difference = 9999999.9;

// Do another scan to make sure the object is actually there.
						new_vector = get_moving_objects(&sick, DT, 10, 0, 5, 175);

						num_objects = new_vector.size();
						to_ind = -1;
						if(num_objects)
						{
							for(i = 0; i < num_objects;i++)
							{
								if(new_vector[i].vmag > 0.1)
								{	
									if(r_diff(target, new_vector[i])<difference)
									{
										difference = r_diff(target, new_vector[i]);
										if(difference < 500.0)
											to_ind = i;
									}
								}
							}
						}

						if(to_ind > -1)
						{
							new_heading = (-90 + new_vector[to_ind].degree);

							if(test_flag != TEST)
							{
								robot.lock();
								robot.setDeltaHeading(new_heading);
								robot.unlock();
							}

							robot_state = FOLLOWING;
							target = new_vector[to_ind];

							target.degree = target.degree - new_heading;


							print_object_to_stream(target, ftarget);

							print_object_to_stream(target, flog);
							flog << new_heading;
						}

					}
				}
			}
			else		      
			{
				robot_state = TOO_CLOSE;
				printf("I'm too close to an obstacle, and I'm getting claustrophobic! I'm going to slowly back up now.\r\n");
			}



			break;

		case FOLLOWING:
		{
			flog << "FOLLOWING\r\n";

//.........这里部分代码省略.........
  {
    robot.lock();
    if (robot.isHeadingDone(5))
    {
      printf("directMotionExample: Finished turn\n");
      robot.unlock();
      break;
    }
    if (start.mSecSince() > 5000)
    {
      printf("directMotionExample: Turn timed out\n");
      robot.unlock();
      break;
    }
    robot.unlock();
    ArUtil::sleep(100);
  }
  printf("directMotionExample: Telling the robot to turn to 90, then sleeping 2 seconds\n");
  robot.lock();
  robot.setHeading(90);
  robot.unlock();
  start.setToNow();
  while (1)
  {
    robot.lock();
    if (robot.isHeadingDone(5))
    {
      printf("directMotionExample: Finished turn\n");
      robot.unlock();
      break;
    }
    if (start.mSecSince() > 5000)
    {
      printf("directMotionExample: turn timed out\n");
      robot.unlock();
      break;
    }
    robot.unlock();
    ArUtil::sleep(100);
  }
  printf("directMotionExample: Setting vel2 to 200 mm/sec on both wheels, then sleeping 3 seconds\n");
  robot.lock();
  robot.setVel2(200, 200);
  robot.unlock();
  ArUtil::sleep(3000);
  printf("directMotionExample: Stopping the robot, then sleeping for 2 seconds\n");
  robot.lock();
  robot.stop();
  robot.unlock();
  ArUtil::sleep(2000);
  printf("directMotionExample: Setting velocity to 200 mm/sec then sleeping 3 seconds\n");
  robot.lock();
  robot.setVel(200);
  robot.unlock();
  ArUtil::sleep(3000);
  printf("directMotionExample: Stopping the robot, then sleeping for 2 seconds\n");
  robot.lock();
  robot.stop();
  robot.unlock();
  ArUtil::sleep(2000);
  printf("directMotionExample: Setting vel2 with 0 on left wheel, 200 mm/sec on right, then sleeping 5 seconds\n");
  robot.lock();
  robot.setVel2(0, 200);
  robot.unlock();
  ArUtil::sleep(5000);
  printf("directMotionExample: Telling the robot to rotate at 50 deg/sec then sleeping 5 seconds\n");
  robot.lock();
  robot.setRotVel(50);
  robot.unlock();
  ArUtil::sleep(5000);
  printf("directMotionExample: Telling the robot to rotate at -50 deg/sec then sleeping 5 seconds\n");
  robot.lock();
  robot.setRotVel(-50);
  robot.unlock();
  ArUtil::sleep(5000);
  printf("directMotionExample: Setting vel2 with 0 on both wheels, then sleeping 3 seconds\n");
  robot.lock();
  robot.setVel2(0, 0);
  robot.unlock();
  ArUtil::sleep(3000);
  printf("directMotionExample: Now having the robot change heading by -125 degrees, then sleeping for 6 seconds\n");
  robot.lock();
  robot.setDeltaHeading(-125);
  robot.unlock();
  ArUtil::sleep(6000);
  printf("directMotionExample: Now having the robot change heading by 45 degrees, then sleeping for 6 seconds\n");
  robot.lock();
  robot.setDeltaHeading(45);
  robot.unlock();
  ArUtil::sleep(6000);
  printf("directMotionExample: Setting vel2 with 200 on left wheel, 0 on right wheel, then sleeping 5 seconds\n");
  robot.lock();
  robot.setVel2(200, 0);
  robot.unlock();
  ArUtil::sleep(5000);

  printf("directMotionExample: Done, shutting down Aria and exiting.\n");
  Aria::shutdown();
  return 0;
}