Пример #1
0
void Joydrive::drive(void)
{
  int trans, rot;

  // print out some data about the robot
  printf("\rx %6.1f  y %6.1f  tth  %6.1f vel %7.1f mpacs %3d   ", 
	 myRobot->getX(), myRobot->getY(), myRobot->getTh(), 
	 myRobot->getVel(), myRobot->getMotorPacCount());
  fflush(stdout);

  // see if a joystick butotn is pushed, if so drive
  if (myJoyHandler.haveJoystick() && (myJoyHandler.getButton(1) ||
				    myJoyHandler.getButton(2)))
  {
    // get the values out of the joystick handler
    myJoyHandler.getAdjusted(&rot, &trans);
    // drive the robot
    myRobot->setVel(trans);
    myRobot->setRotVel(-rot);
  }
  // if a button isn't pushed, stop the robot
  else
  {
    myRobot->setVel(0);
    myRobot->setRotVel(0);
  }
}
Пример #2
0
void Joydrive::drive(void)
{
    int trans, rot;
    int pan, tilt;
    int zoom, nothing;

    // if both buttons are pushed, zoom the joystick
    if (myJoyHandler.haveJoystick() && myJoyHandler.getButton(1) &&
            myJoyHandler.getButton(2))
    {
        // set its speed so we get desired value range, we only care about y
        myJoyHandler.setSpeeds(0, myZoomValCamera);
        // get the values
        myJoyHandler.getAdjusted(&nothing, &zoom);
        // zoom the camera
        myCam.zoomRel(zoom);
    }
    // if both buttons aren't pushed, see if one button is pushed
    else
    {
        // if button one is pushed, drive the robot
        if (myJoyHandler.haveJoystick() && myJoyHandler.getButton(1))
        {
            // set the speed on the joystick so we get the values we want
            myJoyHandler.setSpeeds(myRotValRobot, myTransValRobot);
            // get the values
            myJoyHandler.getAdjusted(&rot, &trans);
            // set the robots speed
            myRobot->setVel(trans);
            myRobot->setRotVel(-rot);
        }
        // if button one isn't pushed, stop the robot
        else
        {
            myRobot->setVel(0);
            myRobot->setRotVel(0);
        }
        // if button two is pushed, move the camera
        if (myJoyHandler.haveJoystick() && myJoyHandler.getButton(2))
        {
            // set the speeds on the joystick so we get desired value range
            myJoyHandler.setSpeeds(myPanValCamera, myTiltValCamera);
            // get the values
            myJoyHandler.getAdjusted(&pan, &tilt);
            // drive the camera
            myCam.panTiltRel(pan, tilt);
        }
    }

}
// this is the function called in the new thread
void *Joydrive::runThread(void *arg)
{
  threadStarted();

  int trans, rot;

  // only run while running, ie play nice and pay attention to the thread 
  //being shutdown
  while (myRunning)
  {
    // lock the robot before touching it
    myRobot->lock();
    if (!myRobot->isConnected())
    {
      myRobot->unlock();
      break;
    }
    // print out some information about the robot
    printf("\rx %6.1f  y %6.1f  tth  %6.1f vel %7.1f mpacs %3d   ", 
	   myRobot->getX(), myRobot->getY(), myRobot->getTh(), 
	   myRobot->getVel(), myRobot->getMotorPacCount());
    fflush(stdout);
    // if one of the joystick buttons is pushed, drive the robot
    if (myJoyHandler.haveJoystick() && (myJoyHandler.getButton(1) ||
					myJoyHandler.getButton(2)))
    {
      // get out the values from the joystick
      myJoyHandler.getAdjusted(&rot, &trans);
      // drive the robot
      myRobot->setVel(trans);
      myRobot->setRotVel(-rot);
    }
    // if no buttons are pushed stop the robot
    else
    {
      myRobot->setVel(0);
      myRobot->setRotVel(0);
    }
    // unlock the robot, so everything else can run
    myRobot->unlock();
    // now take a little nap
    ArUtil::sleep(50);
  }
  // return out here, means the thread is done
  return NULL;
}
Пример #4
0
int main (int argc, char** argv) {
	
	Aria::init();
	Arnl::init();
	ArRobot robot;
	ArArgumentParser parser(&argc, argv);
	parser.loadDefaultArguments();
	ArRobotConnector robotConnector(&parser, &robot);
	//ArAnalogGyro gyro = new 
	ArAnalogGyro gyro(&robot);
	if (!robotConnector.connectRobot()) {
		ArLog::log(ArLog::Terse, "Could not connect to the robot.");
		if(parser.checkHelpAndWarnUnparsed())
		{
			// -help not given, just exit.
			Aria::logOptions();
			Aria::exit(1);
			return 1;
		}
	}
	
	ArSonarDevice sonarDev;
	robot.addRangeDevice(&sonarDev);
	
	robot.runAsync(true);
	
	ArMap map("office.map");
	// set it up to ignore empty file names (otherwise if a configuration omits
	// the map file, the whole configuration change will fail)
	map.setIgnoreEmptyFileName(true);
	// ignore the case, so that if someone is using MobileEyes or
	// MobilePlanner from Windows and changes the case on a map name,
	// it will still work.
	map.setIgnoreCase(true);
	
	ArSonarLocalizationTask locTask(&robot, &sonarDev, &map);
	
	locTask.localizeRobotAtHomeBlocking();
	
	robot.runAsync(true);
	robot.enableMotors();
	//robot.lock();
	robot.setVel(200);
	//robot.unlock();
	ArPose pose;
	locTask.forceUpdatePose(pose);
	while(true) {
	//while (robot.blockingConnect()){
		//robot.lock();
		//ArPose pose  = robot.getPose();
		//pose.setX(100);
		//robot.moveTo(pose);
		//t = robot.getLastOdometryTime();
		//int a = interp.getPose(t, &pose);
		ArLog::log(ArLog::Normal, "x = %f \t y = %f\n", pose.getX(), pose.getY());
		//robot.unlock();
	} 
}
Пример #5
0
int main(int argc, char **argv)
{
  Aria::init();
  ArRobot robot;

  ArSerialConnection serialConnection;
  ArTcpConnection tcpConnection;
    
  if (tcpConnection.open("localhost", 8101)) {
    robot.setDeviceConnection(&tcpConnection);
  } else {
    serialConnection.setPort("/dev/ttyUSB0");
    robot.setDeviceConnection(&serialConnection);
  }
  robot.blockingConnect();
   
  printf("Setting robot to run async\n");
  robot.runAsync(false);

  printf("Turning off sound\n");
  robot.comInt(ArCommands::SOUNDTOG, 0);

  printf("Enabling motors\n");
  robot.enableMotors();

  // add a set of actions that combine together to effect the wander behavior
  /*ArActionStallRecover recover;
  ArActionBumpers bumpers;
  ArActionAvoidFront avoidFrontNear("Avoid Front Near", 225, 0);
  ArActionAvoidFront avoidFrontFar;
  ArActionConstantVelocity constantVelocity("Constant Velocity", 400);
  robot.addAction(&recover, 100);
  robot.addAction(&bumpers, 75);
  robot.addAction(&avoidFrontNear, 50);
  robot.addAction(&avoidFrontFar, 49);
  robot.addAction(&constantVelocity, 25);*/

  printf("Locking\n");
  robot.lock();
  robot.setVel(100.0);
  robot.unlock();
  printf("Sleeping\n");
  ArUtil::sleep(3*1000);
  printf("Awake\n");

  
  // wait for robot task loop to end before exiting the program
  //while (true);
  //robot.waitForRunExit();
  

  Aria::exit(0);
  return 0;
}
Пример #6
0
void
RosAriaNode::cmdvel_cb( const geometry_msgs::TwistConstPtr &msg)
{
  veltime = ros::Time::now();
#ifdef SPEW
  ROS_INFO( "new speed: [%0.2f,%0.2f](%0.3f)", msg->linear.x*1e3, msg->angular.z, veltime.toSec() );
#endif

  robot->lock();
  robot->setVel(msg->linear.x*1e3);
  robot->setRotVel(msg->angular.z*180/M_PI);
  robot->unlock();
  ROS_DEBUG("RosAria: sent vels to to aria (time %f): x vel %f mm/s, y vel %f mm/s, ang vel %f deg/s", veltime.toSec(),
    (double) msg->linear.x * 1e3, (double) msg->linear.y * 1.3, (double) msg->angular.z * 180/M_PI);
}
Пример #7
0
void RosAriaNode::spin()
{
  //ros::spin();

    while(ros::ok()){
               if(ros::WallTime::now()-last_command_time_ > ros::WallDuration(1)){
                   robot->lock();
                   robot->setVel(0.0);
                   if(robot->hasLatVel())
                     robot->setLatVel(0.0);
                   robot->setRotVel(0.0);
                   robot->unlock();
               }
               ros::spinOnce();
           }
}
Пример #8
0
void setMotors(ArRobot& robot){
	TiObj G_motor;
	G_motor.loadFile("../motors/motors");

	if ( G_motor.has("speed") || G_motor.has("rotation") ){
		cout << G_motor;

		robot.lock();
		if ( G_motor.has("speed") )
			robot.setVel( G_motor.atInt("speed") );
		if ( G_motor.has("rotation") ) 
			robot.setRotVel( G_motor.atInt("rotation") );
		robot.unlock();

		FILE* fd = fopen("../motors/motors","w");
		fclose(fd);
	}
	//
}
Пример #9
0
void RosAriaNode::cmdvel_cb( const geometry_msgs::TwistConstPtr &msg)
{
  veltime = ros::WallTime::now();
  last_command_time_=veltime;
  ROS_INFO( "new speed: [%0.2f,%0.2f](%0.3f)", msg->linear.x*1e3, msg->angular.z, veltime.toSec() );

  twistMsg.linear.x=msg->linear.x;
  twistMsg.angular.z=msg->angular.z;
  twist_pub.publish(twistMsg);


  robot->lock();
  robot->setVel(msg->linear.x*1e3);
  if(robot->hasLatVel())
    robot->setLatVel(msg->linear.y*1e3);
  robot->setRotVel(msg->angular.z*180/M_PI);
  robot->unlock();
  ROS_DEBUG("RosAria: sent vels to to aria (time %f): x vel %f mm/s, y vel %f mm/s, ang vel %f deg/s", veltime.toSec(),
    (double) msg->linear.x * 1e3, (double) msg->linear.y * 1.3, (double) msg->angular.z * 180/M_PI);
}
Пример #10
0
void RosAriaNode::cmdvel_cb( const geometry_msgs::PointStampedConstPtr &msg)
{
	veltime = ros::Time::now();
	//get data from Package
	Vsd =ceil(msg->point.x); //Vsd = Xm/Scale
	Vm=msg->point.y;		// Vm
	fk = msg->point.z;

	//real velocity
	Vs = ceil(robot->getVel());
	//Distance from obstacle
	distance = sonar.currentReadingPolar(-30,30);

	//Environment force
	if (distance<Min_Range)
		fe = -Max_Force;
	else if (distance<Max_Range)
		fe = -Ke*1/distance;
	else
		fe=0;
	//Virtual force
	fs = (Vsd-Vs)/50;

	/*
	 * Xm chanel
	 */

	//PO
	if (fs*Vsd>0)
	{
		//do nothing
		slaE_N_Xm-=fs*Vsd; //output
	}
	else
	{
		//slaE_N_Xm+=fs*Vsd; //output
	}

	//ROS_INFO("Slave Energy: %5f",slaE_N_Xm);
	//fprintf(Energy,"%.3f \n",slaE_N_Xm);
	//PC
#if 1
	if (slaE_N_Xm+mstE_P_Xm<0) //input + output<0 =>active
	{
		//modify Vsd
		slaE_N_Xm+=fs*Vsd;
		Vsd = (slaE_N_Xm+mstE_P_Xm)/fs;
		slaE_N_Xm-=fs*Vsd;
		ROS_INFO("Modified Vs");
	}
#endif

	/*
	 * fe chanel
	 */

	if (fe*Vm>0)
	{
		slaE_P_fe+=fe*Vm;
	}
	else
	{
		//do nothing
	}

	//fprintf(Energy,"%.3f \n",slaE_P_fe);
	/*
	 * fk chanel
	 */

	if (fk*Vs>0)
	{
	//	slaE_P_fk+=fk*Vs;
	}
	else
	{
		slaE_P_fk-=fk*Vs;
		//do nothing
	}

	fprintf(Energy,"%.3f \n",slaE_P_fk);
//	ROS_INFO("Slave Energy: %5f",slaE_P_fk);
	//ROS_INFO("Set velocity: %5f",Vsd);
	//ROS_INFO("Real velocity: %5f",Vs);
	//ROS_INFO("Force: %5f",fe);

	robot->setVel(Vsd);

	//Package and publish

	chanelParameter.point.x = Vs;
	chanelParameter.point.y = fs;
	chanelParameter.point.z = fe;

	chanelEnergy.point.x = slaE_P_fe;
	chanelEnergy.point.y = slaE_P_fk;

	chanelParameter_Pub.publish(chanelParameter);
	chanelEnergy_Pub.publish(chanelEnergy);

	fprintf(Vs_save,"%.3f \n",Vs);
	fprintf(Vsd_save,"%.3f \n",Vsd);
}
Пример #11
0
    void manualControlHandler(){


    	if(firstCall){
    		firstCall=false;
    		showMenu();
    	}

    	if(keyPressedBefore && !driver->estaEjecutando()){
    		keyPressedBefore=false;
    		showMenu();
    	}

    	if(mrpt::system::os::kbhit() ){
    		char c = mrpt::system::os::getch();
    		keyPressedBefore=true;


    		switch(c){

    		case '\033':

    			c=mrpt::system::os::getch(); // skip the [
    			cout << "Siguiente caracter" << (int)c << endl;
    			double v;
    			switch(mrpt::system::os::getch()) { // the real value
    			case 'A':
    				// code for arrow up
    				v=robot->getVel();
    				robot->setVel(v+50);
    				break;
    			case 'B':
    				// code for arrow down
    				v=robot->getVel();
    				robot->setVel(v-50);
    				break;
    			case 'C':
    				// code for arrow right
    				v=robot->getRotVel();
    				robot->setRotVel(v-5);
    				break;
    			case 'D':
    				// code for arrow left
    				v=robot->getRotVel();
    				robot->setRotVel(v+5);
    				break;
    			}
    			break;

    			case ' ':
    				robot->stop();
    				break;


    		case 'x':
    			//Aria::shutdown();
    			driver->stopRunning();
    			robot->stopRunning();
    			break;

    		case 'c':
    			guardarContinuo();
    			break;

    		case 'p':
    			start();
    			break;

    		case 'g':
    			guardar();
    			break;


    		case 'w':
    			guardarTrayectoria();
    			break;

    		case 't':
    			testParado();
    			break;

    		case 's':
    			stop();
    			break;
    		}

    	}
    }
Пример #12
0
void PeoplebotTest::userTask(void)
{
  switch (myState)
  {
    case IDLE:
      // start wandering
      printf("Starting to wander for the first time\n");
      myStateTime.setToNow();
      myState = WANDERING;
      myRobot->addAction(myConstantVelocity, 25);
      printf("Opening up ACTS\n");
      myCmd = "DISPLAY=";
      myCmd += myHostname.c_str();
      myCmd += ":0; /usr/local/acts/bin/acts -G bttv -n 0 &> /dev/null &";
      system(myCmd.c_str());
      break;
    case WANDERING:
      if (timeout(myWanderingTimeout))
      {
	myRobot->comInt(ArCommands::SONAR,0);
	myRobot->remAction(myConstantVelocity);
	myRobot->setVel(0);
	myRobot->setRotVel(0);
	myState = RESTING;
	mySonar = 0;
	printf("Going to rest now\n");
	printf("Killing ACTS\n");
	system("killall -9 acts &> /dev/null");
	myStateTime.setToNow();
	myTotalWanderTime += myWanderingTimeout;
      }
      else if (myRobot->getVel() > 0 && mySonar != 1)
      {
	// ping front sonar
	//printf("Enabling front sonar\n");
	mySonar = 1;
	myRobot->comInt(ArCommands::SONAR, 0);
	myRobot->comInt(ArCommands::SONAR, 1);
	myRobot->comInt(ArCommands::SONAR, 4);
      }
      else if (myRobot->getVel() < 0 && mySonar != -1)
      {
	// ping rear sonar
	//printf("Enabling rear sonar\n");
	mySonar = -1;
	myRobot->comInt(ArCommands::SONAR, 0);
	myRobot->comInt(ArCommands::SONAR, 5);
      }
      break;
    case RESTING:
      if (timeout(myRestingTimeout))
      {
	printf("Going to wander now\n");
	myState = WANDERING;
	myStateTime.setToNow();
	myTotalRestTime += myRestingTimeout;
	myRobot->clearDirectMotion();
	myRobot->addAction(myConstantVelocity, 25);
	printf("Opening up ACTS\n");
	myCmd = "DISPLAY=";
	myCmd += myHostname.c_str();
	myCmd += ":0; /usr/local/acts/bin/acts -G bttv -n 0 &> /dev/null &";
	system(myCmd.c_str());
      }
      break;
    case OTHER:
    default:
      break;
  };
}
Пример #13
0
void RosAriaNode::Mas1ToSla_cb( const geometry_msgs::PointStampedConstPtr &msg)
{
	// Master 1 Position
	Vm1 = msg->point.x;
	Xm1 = Xm1 + Vm1;
	// Master force
	Fk1 = msg->point.y;
	// Master 1 Positive Energy
	mst1_slv_cmd_P = msg->point.z;


	Xsd = Scale *(alpha*Xm1 + (1-alpha)*Xm2);// design position

	Xsprv = Xs;
	Position = robot->getPose();
	Xs = Position.getX();
	delta = Xs - Xsprv;

	Vs = PosController.compute(Xsd,Xs);

	// Fs - Sum
	Fs = K_force*(Xsd - Xs);
	Fs1 = alpha*Fs;
	Fs2 = (1-alpha)*Fs;
	/*
	 * Master 1 - Slave Channel
	 */

	// Calculate Negative Energy and dissipate Active energy
	if (Vm1*Fs1>0)
	{
		mst1_slv_cmd_N -=Vm1*Fs1;
	}
	else
	{
		//Do nothing
	}

	// PC:
	if (mst1_slv_cmd_N+mst1_slv_cmd_P<0)
	{
		mst1_slv_cmd_N +=Vm1*Fs1;    // backward 1 step
		Xm1 = Xm1 - Vm1;			// backward 1 step
		// Modify Vm1
		if (Fs1*Fs1>0)
			Vm1 = (mst1_slv_cmd_N+mst1_slv_cmd_P)/Fs1;
		else
			Vm1 = 0;
		//Update
		Xm1 = Xm1 + Vm1;
		Xsd = Scale *(alpha*Xm1 + (1-alpha)*Xm2);// design position
		Vs = PosController.compute(Xsd,Xs);
		// Modify Fs ????

		mst1_slv_cmd_N -=Vm1*Fs1;
	}

	/*
	 * Slave - Master 1 Channel
	 */
	// Calculate Positive Energy

	if (Fk1*Vs>0)
	{
		//sla_mst1_cmd_P += Fk1*Vs;
		sla_mst1_cmd_P += Fk1*delta;
	}
	else
	{
		//Do nothing
	}

	/*
	 * Master 2 - Slave Channel
	 */

	// Calculate Negative Energy and dissipate Active energy
	if (Vm2*Fs2>0)
	{
		mst2_slv_cmd_N -=Vm2*Fs2;
	}
	else
	{
		//Do nothing
	}

	// PC:
	if (mst2_slv_cmd_N+mst2_slv_cmd_P<0)
	{
		mst2_slv_cmd_N +=Vm2*Fs2;    // backward 1 step
		Xm2 = Xm2 - Vm2;			// backward 1 step
		// Modify Vm1
		if (Fs2*Fs2>0)
			Vm2 = (mst2_slv_cmd_N+mst2_slv_cmd_P)/Fs2;
		else
			Vm2 = 0;
		//Update
		Xm2 = Xm2 + Vm2;
		Xsd = Scale *(alpha*Xm1 + (1-alpha)*Xm2);// design position
		Vs = PosController.compute(Xsd,Xs);
		// Modify Fs ????

		mst2_slv_cmd_N -=Vm2*Fs2;
	}

	/*
	 * Slave - Master 2 Channel
	 */
	// Calculate Positive Energy
	if (Fk2*Vs>0)
	{
		sla_mst2_cmd_P += Fk2*Vs;
	}
	else
	{
		//Do nothing
	}



	//Saturation
	if (Vs>MaxVel) Vs = MaxVel;
	if (Vs< - MaxVel) Vs = -MaxVel;

	//ROS_INFO("Velocity: %5f",Vs);
	//ROS_INFO("Ref - Real - Vel : %5f  -- %5f  --%5f",Xsd,Xs,Vs);
	ROS_INFO("Position: %5f - %5f",Xs,Xsd);
	robot->setVel(Vs);


	// Publisher
	SlaToMas1.point.x = Fs1;
	SlaToMas1.point.y = delta;
	SlaToMas1.point.z = sla_mst1_cmd_P;

	SlaToMas1_Pub.publish(SlaToMas1);

	SlaToMas2.point.x = Fs2;
	SlaToMas2.point.y = Vs;
	SlaToMas2.point.z = sla_mst2_cmd_P;

	SlaToMas2_Pub.publish(SlaToMas2);
}
Пример #14
0
void Joydrive::drive(void)
{
  int trans, rot;
  ArPose pose;
  ArPose rpose;
  ArTransform transform;
  ArRangeDevice *dev;
  ArSensorReading *son;

  if (!myRobot->isConnected())
  {
    printf("Lost connection to the robot, exiting\n");
    exit(0);
  }
  printf("\rx %6.1f  y %6.1f  th  %6.1f", 
	 myRobot->getX(), myRobot->getY(), myRobot->getTh());
  fflush(stdout);
  if (myJoyHandler.haveJoystick() && myJoyHandler.getButton(1))
  {
    if (ArMath::fabs(myRobot->getVel()) < 10.0)
      myRobot->comInt(ArCommands::ENABLE, 1);
    myJoyHandler.getAdjusted(&rot, &trans);
    myRobot->setVel(trans);
    myRobot->setRotVel(-rot);
  }
  else
  {
    myRobot->setVel(0);
    myRobot->setRotVel(0);
  }
  if (myJoyHandler.haveJoystick() && myJoyHandler.getButton(2) &&
      time(NULL) - myLastPress > 1)
  {
    myLastPress = time(NULL);
    printf("\n");
    switch (myTest)
    {
    case 1:
      printf("Moving back to the origin.\n");
      pose.setPose(0, 0, 0);
      myRobot->moveTo(pose);
      break;
    case 2:
      printf("Moving over a meter.\n");
      pose.setPose(myRobot->getX() + 1000, myRobot->getY(), 0);
      myRobot->moveTo(pose);
      break;
    case 3:
      printf("Doing a transform test....\n");
      printf("\nOrigin should be transformed to the robots coords.\n");
      transform = myRobot->getToGlobalTransform();
      pose.setPose(0, 0, 0);
      pose = transform.doTransform(pose);
      rpose = myRobot->getPose();
      printf("Pos:  ");
      pose.log();
      printf("Robot:  ");
      rpose.log();

      if (pose.findDistanceTo(rpose) < .1)
	printf("Success\n");
      else
	printf("#### FAILURE\n");
    
      printf("\nRobot coords should be transformed to the origin.\n");
      transform = myRobot->getToLocalTransform();
      pose = myRobot->getPose();
      pose = transform.doTransform(pose);
      rpose.setPose(0, 0, 0);
      printf("Pos:  ");
      pose.log();
      printf("Robot:  ");
      rpose.log();
      if (pose.findDistanceTo(rpose) < .1)
	printf("Success\n");
      else
	printf("#### FAILURE\n");
      break;
    case 4:
      printf("Doing a tranform test...\n");
      printf("A point 1 meter to the -x from the robot (in local coords) should be transformed into global coordinates.\n");
      transform = myRobot->getToGlobalTransform();
      pose.setPose(-1000, 0, 0);
      pose = transform.doTransform(pose);
      rpose = myRobot->getPose();
      printf("Pos:  ");
      pose.log();
      printf("Robot:  ");
      rpose.log();

      if (ArMath::fabs(pose.findDistanceTo(rpose) - 1000.0) < .1)
	printf("Probable Success\n");
      else
	printf("#### FAILURE\n");
      break;
    case 5:
      printf("Doing a transform test on range devices..\n");
      printf("Moving the robot +4 meters x and +4 meters y and seeing if the moveTo will move the sonar readings along with it.\n");
      dev = myRobot->findRangeDevice("sonar");
      if (dev == NULL)
      {
	printf("No sonar on the robot, can't do the test.\n");
	break;
      }
      printf("Closest sonar reading to the robot is %.0f away\n", dev->currentReadingPolar(1, 0));
      printf("Sonar 0 reading is at ");
      son = myRobot->getSonarReading(0);
      if (son != NULL)
      {
	pose = son->getPose();
	pose.log();
      }
      pose = myRobot->getPose();
      pose.setX(pose.getX() + 4000);
      pose.setY(pose.getY() + 4000);
      myRobot->moveTo(pose);
      printf("Moved robot.\n");
      printf("Closest sonar reading to the robot is %.0f away\n", dev->currentReadingPolar(1, 0));
      printf("Sonar 0 reading is at ");
      son = myRobot->getSonarReading(0);
      if (son != NULL)
      {
	pose = son->getPose();
	pose.log();
      }

      break;
    case 6:
      printf("Robot position now is:\n");
      pose = myRobot->getPose();
      pose.log();
      printf("Disconnecting from the robot, then reconnecting.\n");
      myRobot->disconnect();
      myRobot->blockingConnect();      
      printf("Robot position now is:\n");
      pose = myRobot->getPose();
      pose.log();
      break;
    default:
      printf("No test for second button.\n");
      break;
    } 
  }
}
Пример #15
0
int main(int argc, char **argv) 
{
	struct settings robot_settings;

	robot_settings.min_distance = 500;
	robot_settings.max_velocity = 500;
	robot_settings.tracking_factor = 1.0;

	ArRobot robot;
	Aria::init();
	//laser
	int ret; //Don't know what this variable is for
	ArSick sick; // Laser scanner
	ArSerialConnection laserCon; // Scanner connection
	std::string str; // Standard output
	// sonar, must be added to the robot
	ArSonarDevice sonar;
	// add the sonar to the robot
	robot.addRangeDevice(&sonar);
	// add the laser to the robot
	robot.addRangeDevice(&sick);

	ArArgumentParser argParser(&argc, argv);
	ArSimpleConnector con(&argParser);

	// the connection handler from above
	ConnHandler ch(&robot);

	if(!Aria::parseArgs())
	{
		Aria::logOptions();
		Aria::shutdown();
		return 1;
	}
	if(!con.connectRobot(&robot))
	{
		ArLog::log(ArLog::Normal, "directMotionExample: Could not connect to the robot. Exiting.");
		return 1;
	}
	ArLog::log(ArLog::Normal, "directMotionExample: Connected.");
	robot.runAsync(false);

	///////////////////////////////
	// Attempt to connect to SICK using another hard-coded USB connection
	sick.setDeviceConnection(&laserCon);
	if((ret=laserCon.open("/dev/ttyUSB1")) !=0){
		//If connection fails, shutdown
		Aria::shutdown();
		return 1;
	}
	//Configure the SICK
	sick.configureShort(false,/*not using sim*/ArSick::BAUD38400,ArSick::DEGREES180,ArSick::INCREMENT_HALF);
	//Run the sick
	sick.runAsync();
	// Presumably test to make sure that the connection is good
	if(!sick.blockingConnect()){
		printf("Could not get sick...exiting\n");
		Aria::shutdown();
		return 1; 
	}
	printf("We are connected to the laser!");

	printf("\r\nRobot Entering default resting state.\r\nUse the following commands to run the robot.\r\n");
	printf("r  Run the robot\r\n");
	printf("s  Stop the robot\r\n");
	printf("t  Enter test mode (the robot will do everything except actually move.\r\n");
	printf("w  Save current data to files, and show a plot of what the robot sees.\r\n");
	printf("e  Edit robot control parameters\r\n");
	printf("q  Quit the program\r\n");

	printf("\r\n");
	printf("\r\n");
	printf("NOTE: You must have GNUPLOT installed on your computer, and create a directory entitled 'scan_data' under your current directory in order to use this script. Failure to do so might make the computer crash, which would cause the robot to go on a mad killing spree! Not really, but seriously, go ahead and get GNUPLOT and create that subdirectory before using the 'w' option.\r\n\r\n"); 

	/////////////////////////////////////
	char user_command = 0;
	char plot_option = 0;

	int robot_state = REST;

	tracking_object target;
	tracking_object l_target;

	ofstream fobjects;
	ofstream ftarget;
	ofstream flog;
	ofstream fltarget;


	fobjects.open("./scan_data/objects_new.txt");
	ftarget.open("./scan_data/target_new.txt");
	fltarget.open("./scan_data/ltarget_new.txt");
	fobjects << "\r\n";
	ftarget << "\r\n";
	fltarget << "\r\n";
	fobjects.close();
	ftarget.close();
	fltarget.close();

	flog.open("robot_log.txt");

	std::vector<tracking_object> obj_vector;
	std::vector<tracking_object> new_vector;


	float last_v = 0;

	ArTime start;



	char test_flag = 0;
	char target_lost = 0;


	while(user_command != 'q')
	{
		switch (user_command)
		{
		case STOP:
			robot_state = REST;
			printf("robot has entered resting mode\r\n");

			break;
		case RUN:
			robot_state = TRACKING;
			printf("robot has entered tracking mode\r\n");
			break;
		case QUIT:
			robot_state = -1;
			printf("exiting... goodbye.\r\n");
			break;
		case WRITE:
			plot_option = WRITE;
			break;
		case NO_WRITE:
			plot_option = NO_WRITE;
			break;
		case TEST:
			if(test_flag == TEST)
			{
				test_flag = 0;
				printf("Exiting test mode\r\n");
			}
			else
			{
				test_flag = TEST;
				printf("Entering test mode\r\n");
				robot.lock();
				robot.setVel(0);
				robot.unlock();
			}
			break;

		case EDIT:
			edit_settings(&robot_settings);
			break;

		default:
			robot_state = robot_state;
		}


		unsigned int i = 0;
		unsigned int num_objects = 0;
		int to_ind = -1;
		float min_distance = 999999.9;
		float new_heading = 0;

		system("mv ./scan_data/objects_new.txt ./scan_data/objects.txt");
		system("mv ./scan_data/target_new.txt ./scan_data/target.txt");
		system("mv ./scan_data/ltarget_new.txt ./scan_data/ltarget.txt");

		fobjects.open("./scan_data/objects_new.txt");
		ftarget.open("./scan_data/target_new.txt");
		fltarget.open("./scan_data/ltarget_new.txt");

		switch (robot_state)
		{
		case REST:
			robot.lock();
			robot.setVel(0);
			robot.unlock();

			obj_vector = run_sick_scan(&sick, 2, plot_option);

			target_lost = 0;
			num_objects = obj_vector.size();
			if(num_objects > 0)
			{

				for(i = 0; i < num_objects; i++)
				{
					print_object_to_stream(obj_vector[i], fobjects);	
				}
			}

			break;

		case TRACKING:
			flog << "TRACKING\r\n";

			obj_vector = get_moving_objects(&sick, 2*DT, 10, plot_option);
			num_objects = obj_vector.size();

			target_lost = 0;

			if(min_range(&sick, 45, 135) > ROBOT_SAFETY_MARGIN)
			{
				if(num_objects)
				{
 					for(i = 0; i < obj_vector.size();i++)
					{	
						print_object_to_stream(obj_vector[i], fobjects);
						if(obj_vector[i].vmag > 0.1)
						{	
							if(obj_vector[i].distance < min_distance)
							{
								min_distance = obj_vector[i].distance;
								target = obj_vector[i];
								to_ind = i;
							}
						}
					}

					if(to_ind > -1)
					{
						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";

			i = 0;
			int to_ind = -1;
			float obj_difference = 9999999.9;
			int num_scans = 0;

			if(min_range(&sick, 45, 135) > ROBOT_SAFETY_MARGIN)
			{
				while((num_scans < 3)&&(to_ind == -1))
				{
					obj_vector = run_sick_scan(&sick, 2, 0, 10, 350);
					num_objects = obj_vector.size();
					if(num_objects)
					{
						for(i = 0; i < num_objects;i++)
						{	
							if(r_diff(target, obj_vector[i]) < obj_difference)
							{	
								obj_difference = r_diff(target, obj_vector[i]);
								if (obj_difference < 500.0)
									to_ind = i;

							}
						}

						for(i = 0; i < num_objects;i++)
						{	
							if((int)i == to_ind)
							{
								print_object_to_stream(obj_vector[i], ftarget);
							}
							else
							{
								print_object_to_stream(obj_vector[i], fobjects);
							}
						}
					}
					num_scans++;
				}

				float new_vel = 0;

				if(to_ind > -1)
				{
					printf("Tracking target\r\n");
					flog << "Following target\t";

					target_lost = 0;

					target = obj_vector[to_ind];

					new_vel = (obj_vector[to_ind].distance - robot_settings.min_distance)*robot_settings.tracking_factor;
					if(new_vel < 0)
						new_vel = 0;

					if(new_vel > robot_settings.max_velocity)
						new_vel = robot_settings.max_velocity;

					new_heading = (-90 + target.degree)*0.25;
					new_heading = get_safe_path(&sick, new_heading, target.distance);

					if(test_flag != TEST)
					{
						robot.lock();
						robot.setVel(new_vel);
						robot.unlock();

						last_v = new_vel;

						robot.lock();
						robot.setDeltaHeading(new_heading);
						robot.unlock();
						target.degree = target.degree - new_heading;
					}
					if(new_vel < 1.0)
					{
						robot_state = TRACKING;
						printf("entering tracking mode\r\n");
						flog<<"entering tracking mode\r\n";
					}

				}
				else		      
				{
					if(target_lost)
					{
						
						l_target.distance = l_target.distance - last_v*(start.mSecSince()/1000.0);
 
						int temp_max_v = min_range(&sick, 5, 175);
						if(temp_max_v < last_v)
							last_v = temp_max_v;

						if(last_v > robot_settings.max_velocity)
							last_v = robot_settings.max_velocity;

						if(test_flag != TEST)
						{

							robot.lock();
							robot.setVel(last_v);
							robot.unlock();


							new_heading = -90.0 + l_target.degree;
							new_heading = get_safe_path(&sick, new_heading, l_target.distance);

							l_target.degree = target.degree - new_heading;

							robot.lock();
							robot.setDeltaHeading(new_heading);
							robot.unlock();
						}

						print_object_to_stream(l_target, fltarget);



					}
					else
					{
						target_lost = 1;
						l_target = target;
					}

					printf("target lost\r\n");
					flog << "target lost\r\n";
					start.setToNow();

					if(target.distance < ROBOT_SAFETY_MARGIN)
					{
						robot_state = TRACKING;
						target_lost = 0;
					}

					target_lost = 1;

				}
				printf("Velocity: %f\t",last_v);
				flog << "Velocity:\t";
				flog << last_v;

				printf("Heading: %f\t",new_heading);
				flog << "\tHeading\t";
				flog << new_heading;
				flog << "\r\n";
				printf("\r\n");

			}		
			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 TOO_CLOSE:
		{
			flog << "Too close\r\n";
			if(min_range(&sick, 45, 135) > (ROBOT_SAFETY_MARGIN + 100))
			{		
				robot_state = TRACKING;
				printf("I feel better now! I'm going to reenter tracking mode.\r\n");
				robot.lock();
				robot.setVel(0);
				robot.unlock();
			}
			else
			{
				if(test_flag != TEST)
				{
					printf("Backing up...\r\n");
					robot.lock();
					robot.setVel(-50);
					robot.unlock();
				}

			}
			break;
 		}

		}

		fobjects.close();
		ftarget.close();
		fltarget.close();


		if(plot_option == WRITE)
		{
			system("./scan_data/plot_script.sh >/dev/null");
		}







////////////////////////////////////////////////////////////////////// 
// Everything past this point is code to grab the user input
		user_command = 0;


		fd_set rfds;
		struct timeval tv;
		int retval;

		FD_ZERO(&rfds);
		FD_SET(0, &rfds);

		tv.tv_sec = 0;
		tv.tv_usec = 100;

		retval = select(1, &rfds, NULL, NULL, &tv);

		if(retval == -1)
			perror("select()");
		else if(retval)
		{
			cin >> user_command;
			printf("input detected from user\r\n");
		}

		plot_option = NO_WRITE;
//////////////////////////////////////////////////////////////////////
		
	}
	flog.close();
	Aria::shutdown();
	printf("Shutting down");
	return 0;
}
int main(int argc, char **argv)
{
  bool done;
  double distToTravel = 2300;

  // whether to use the sim for the laser or not, if you use the sim
  // for hte laser, you have to use the sim for the robot too
  bool useSim = false;
  // the laser
  ArSick sick;
  // connection
  ArDeviceConnection *con;
  // Laser connection
  ArSerialConnection laserCon;
  // robot
  ArRobot robot;

  // set a default filename
  //std::string filename = "c:\\log\\1scans.2d";
  std::string filename = "1scans.2d";
  // see if we want to use a different filename
  //if (argc > 1)
  //Lfilename = argv[1];
  printf("Logging to file %s\n", filename.c_str());
  // start the logger with good values
  sick.configureShort(useSim, ArSick::BAUD38400,
		 ArSick::DEGREES180, ArSick::INCREMENT_HALF);
  ArSickLogger logger(&robot, &sick, 300, 25, filename.c_str());
  
  // mandatory init
  Aria::init();

  // add it to the robot
  robot.addRangeDevice(&sick);

  //ArAnalogGyro gyro(&robot);


  // if we're not using the sim, make a serial connection and set it up
  if (!useSim)
  {
    ArSerialConnection *serCon;
    serCon = new ArSerialConnection;
    serCon->setPort();
    //serCon->setBaud(38400);
    con = serCon;
  }
  // if we are using the sim, set up a tcp connection
  else
  {
    ArTcpConnection *tcpCon;
    tcpCon = new ArTcpConnection;
    tcpCon->setPort();
    con = tcpCon;
  }

  // set the connection on the robot
  robot.setDeviceConnection(con);
  // try to connect, if we fail exit
  if (!robot.blockingConnect())
  {
    printf("Could not connect to robot... exiting\n");
    Aria::shutdown();
    return 1;
  }


  // set up a key handler so escape exits and attach to the robot
  ArKeyHandler keyHandler;
  robot.attachKeyHandler(&keyHandler);

  // run the robot, true here so that the run will exit if connection lost
  robot.runAsync(true);



  // if we're not using the sim, set up the port for the laser
  if (!useSim)
  {
    laserCon.setPort(ArUtil::COM3);
    sick.setDeviceConnection(&laserCon);
  }


  // now that we're connected to the robot, connect to the laser
  sick.runAsync();


  if (!sick.blockingConnect())
  {
    printf("Could not connect to SICK laser... exiting\n");
    robot.disconnect();
    Aria::shutdown();
    return 1;
  }

#ifdef WIN32
  // wait until someone pushes the motor button to go
  while (1)
  {
    robot.lock();
    if (!robot.isRunning())
      exit(0);
    if (robot.areMotorsEnabled())
    {
      robot.unlock();
      break;
    }
    robot.unlock();
    ArUtil::sleep(100);
  }
#endif

  // basically from here on down the robot just cruises around a bit

  robot.lock();
  // enable the motors, disable amigobot sounds
  robot.comInt(ArCommands::ENABLE, 1);

  ArTime startTime;
  // move a couple meters
  robot.move(distToTravel);
  robot.unlock();
  startTime.setToNow();
  do {
    ArUtil::sleep(100);
    robot.lock();
    robot.setHeading(0);
    done = robot.isMoveDone(60);
    robot.unlock();
  } while (!done);

  /*
  // rotate a few times
  robot.lock();
  robot.setVel(0);
  robot.setRotVel(60);
  robot.unlock();
  ArUtil::sleep(12000);
  */

  robot.lock();
  robot.setHeading(180);
  robot.unlock();
  do {
    ArUtil::sleep(100);
    robot.lock();
    robot.setHeading(180);
    done = robot.isHeadingDone();
    robot.unlock();
  } while (!done);

  // move a couple meters
  robot.lock();
  robot.move(distToTravel);
  robot.unlock();
  startTime.setToNow();
  do {
    ArUtil::sleep(100);
    robot.lock();
    robot.setHeading(180);
    done = robot.isMoveDone(60);
    robot.unlock();
  } while (!done);

  robot.lock();
  robot.setHeading(0);
  robot.setVel(0);
  robot.unlock();
  startTime.setToNow();
  do {
    ArUtil::sleep(100);
    robot.lock();
    robot.setHeading(0);
    done = robot.isHeadingDone();
    robot.unlock();
  } while (!done);


  sick.lockDevice();
  sick.disconnect();
  sick.unlockDevice();
  robot.lock();
  robot.disconnect();
  robot.unlock();
  // now exit
  Aria::shutdown();
  return 0;
}
Пример #17
0
int main(int argc, char **argv) 
{
  std::string str;
  int ret;
  ArTime start;
  
  // connection to the robot
  ArSerialConnection con;
  // the robot
  ArRobot robot;
  // the connection handler from above
  ConnHandler ch(&robot);

  // init area with a dedicated signal handling thread
  Aria::init(Aria::SIGHANDLE_THREAD);

  // open the connection with the defaults, exit if failed
  if ((ret = con.open()) != 0)
  {
    str = con.getOpenMessage(ret);
    printf("Open failed: %s\n", str.c_str());
    Aria::shutdown();
    return 1;
  }

  // set the robots connection
  robot.setDeviceConnection(&con);
  // try to connect, if we fail, the connection handler should bail
  if (!robot.blockingConnect())
  {
    // this should have been taken care of by the connection handler
    // but just in case
    printf(
    "asyncConnect failed because robot is not running in its own thread.\n");
    Aria::shutdown();
    return 1;
  }
  // run the robot in its own thread, so it gets and processes packets and such
  robot.runAsync(false);

  // just a big long set of printfs, direct motion commands and sleeps,
  // it should be self-explanatory
  printf("Telling the robot to go 300 mm for 5 seconds\n");
  robot.lock();
  robot.setVel(500);
  robot.unlock();
  start.setToNow();
  while (1)
  {
    robot.lock();
    if (start.mSecSince() > 5000)
    {
      robot.unlock();
      break;
    }   
    printf("Trans: %10g Rot: %10g\n", robot.getVel(), robot.getRotVel());
    robot.unlock();
    ArUtil::sleep(100);
  }
  
  printf("Telling the robot to turn at 50 deg/sec for 10 seconds\n");
  robot.lock();
  robot.setVel(0);
  robot.setRotVel(50);
  robot.unlock();
  start.setToNow();
  while (1)
  {
    robot.lock();
    if (start.mSecSince() > 10000)
    {
      robot.unlock();
      break;
    }   
    printf("Trans: %10g Rot: %10g\n", robot.getVel(), robot.getRotVel());
    robot.unlock();
    ArUtil::sleep(100);
  }

  printf("Telling the robot to turn at 100 deg/sec for 10 seconds\n");
  robot.lock();
  robot.setVel(0);
  robot.setRotVel(100);
  robot.unlock();
  start.setToNow();
  while (1)
  {
    robot.lock();
    if (start.mSecSince() > 10000)
    {
      robot.unlock();
      break;
    }   
    printf("Trans: %10g Rot: %10g\n", robot.getVel(), robot.getRotVel());
    robot.unlock();
    ArUtil::sleep(100);
  }

  printf("Done with tests, exiting\n");
  robot.disconnect();
  // shutdown and ge tout
  Aria::shutdown();
  return 0;
}
int main(int argc, char **argv) 
{
  Aria::init();
  
  ArArgumentParser argParser(&argc, argv);

  ArSimpleConnector con(&argParser);

  ArRobot robot;

  // the connection handler from above
  ConnHandler ch(&robot);

  if(!Aria::parseArgs())
  {
    Aria::logOptions();
    Aria::shutdown();
    return 1;
  }

  if(!con.connectRobot(&robot))
  {
    ArLog::log(ArLog::Normal, "directMotionExample: Could not connect to the robot. Exiting.");
    return 1;
  }

  ArLog::log(ArLog::Normal, "directMotionExample: Connected.");

  // Run the robot processing cycle in its own thread. Note that after starting this
  // thread, we must lock and unlock the ArRobot object before and after
  // accessing it.
  robot.runAsync(false);


  // Send the robot a series of motion commands directly, sleeping for a 
  // few seconds afterwards to give the robot time to execute them.
  printf("directMotionExample: Setting rot velocity to 100 deg/sec then sleeping 3 seconds\n");
  robot.lock();
  robot.setRotVel(100);
  robot.unlock();
  ArUtil::sleep(3*1000);
  printf("Stopping\n");
  robot.lock();
  robot.setRotVel(0);
  robot.unlock();
  ArUtil::sleep(200);

  printf("directMotionExample: Telling the robot to go 300 mm on left wheel and 100 mm on right wheel for 5 seconds\n");
  robot.lock();
  robot.setVel2(300, 100);
  robot.unlock();
  ArTime start;
  start.setToNow();
  while (1)
  {
    robot.lock();
    if (start.mSecSince() > 5000)
    {
      robot.unlock();
      break;
    }   
    robot.unlock();
    ArUtil::sleep(50);
  }
  
  printf("directMotionExample: Telling the robot to move forwards one meter, then sleeping 5 seconds\n");
  robot.lock();
  robot.move(1000);
  robot.unlock();
  start.setToNow();
  while (1)
  {
    robot.lock();
    if (robot.isMoveDone())
    {
      printf("directMotionExample: Finished distance\n");
      robot.unlock();
      break;
    }
    if (start.mSecSince() > 5000)
    {
      printf("directMotionExample: Distance timed out\n");
      robot.unlock();
      break;
    }   
    robot.unlock();
    ArUtil::sleep(50);
  }
  printf("directMotionExample: Telling the robot to move backwards one meter, then sleeping 5 seconds\n");
  robot.lock();
  robot.move(-1000);
  robot.unlock();
  start.setToNow();
  while (1)
  {
    robot.lock();
    if (robot.isMoveDone())
    {
      printf("directMotionExample: Finished distance\n");
      robot.unlock();
      break;
    }
    if (start.mSecSince() > 10000)
    {
      printf("directMotionExample: Distance timed out\n");
      robot.unlock();
      break;
    }
    robot.unlock();
    ArUtil::sleep(50);
  }
  printf("directMotionExample: Telling the robot to turn to 180, then sleeping 4 seconds\n");
  robot.lock();
  robot.setHeading(180);
  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: 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;
}
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;
}
Пример #20
0
int main(int argc, char **argv) 
{
  std::string str;
  int ret;
  int dist;
  ArTime start;
  ArPose startPose;
  bool vel2 = false;

  // connection to the robot
  ArSerialConnection con;
  // the robot
  ArRobot robot;
  // the connection handler from above
  ConnHandler ch(&robot);
  
  // init area with a dedicated signal handling thread
  Aria::init(Aria::SIGHANDLE_THREAD);

  if (argc != 2 || (dist = atoi(argv[1])) == 0)
    {
      printf("Usage: %s <distInMM>\n", argv[0]);
      exit(0);
    }
  if (dist < 1000)
    {
      printf("You must go at least a meter\n");
      exit(0);
    }
  // open the connection with the defaults, exit if failed
  if ((ret = con.open()) != 0)
  {
    str = con.getOpenMessage(ret);
    printf("Open failed: %s\n", str.c_str());
    Aria::shutdown();
    return 1;
  }

  // set the robots connection
  robot.setDeviceConnection(&con);
  // try to connect, if we fail, the connection handler should bail
  if (!robot.blockingConnect())
  {
    // this should have been taken care of by the connection handler
    // but just in case
    printf(
    "asyncConnect failed because robot is not running in its own thread.\n");
    Aria::shutdown();
    return 1;
  }
  // run the robot in its own thread, so it gets and processes packets and such
  robot.runAsync(false);

  // just a big long set of printfs, direct motion commands and sleeps,
  // it should be self-explanatory

  robot.lock();

  /*
  robot.setAbsoluteMaxTransVel(2000);
  robot.setTransVelMax(2000);
  robot.setTransAccel(1000);
  robot.setTransDecel(1000);
  robot.comInt(82, 30); // rotkp
  robot.comInt(83, 200); // rotkv
  robot.comInt(84, 0); // rotki
  robot.comInt(85, 30); // transkp
  robot.comInt(86, 450); // transkv
  robot.comInt(87, 4); // transki

  */
  printf("Driving %d mm (going full speed for that far minus a meter then stopping)\n", dist);
  if (vel2)
    robot.setVel2(2200, 2200);
  else
    robot.setVel(2200);
  robot.unlock();
  start.setToNow();
  startPose = robot.getPose();
  while (1)
  {
    robot.lock();
    printf("\r vel: %.0f x: %.0f y: %.0f: dist: %.0f heading: %.2f",
	   robot.getVel(), robot.getX(), robot.getY(), 
	   startPose.findDistanceTo(robot.getPose()),
	   robot.getTh());
    if (startPose.findDistanceTo(robot.getPose()) > abs(dist) - 1000)
    {
      printf("\nFinished distance\n");
      robot.setVel(0);
      robot.unlock();
      break;
    }
    if (start.mSecSince() > 10000)
    {
      printf("\nDistance timed out\n");
      robot.setVel(0);
      robot.unlock();
      break;
    }   
    robot.unlock();
    ArUtil::sleep(50);
  }
  
  if (vel2)
    robot.setVel2(0, 0);
  else
    robot.setVel(0);
  start.setToNow();
  while (1)
    {
      robot.lock();
      if (vel2)
	robot.setVel2(0, 0);
      else
	robot.setVel(0);
      if (fabs(robot.getVel()) < 20)
	{
	  printf("Stopped\n");
	  robot.unlock();
	  break;
	}
      if (start.mSecSince() > 2000)
	{
	  printf("\nStop timed out\n");
	  robot.unlock();
	  break;
	}
      robot.unlock();
      ArUtil::sleep(50);
    }
  robot.lock();
  robot.disconnect();
  robot.unlock();
  // shutdown and ge tout
  Aria::shutdown();
  return 0;
}
int main(int argc, char **argv)
{

  Aria::init();
  ArRobot robot;
  ArArgumentParser parser(&argc, argv);
  parser.loadDefaultArguments();

  ArLog::log(ArLog::Terse, "WARNING: this program does no sensing or avoiding of obstacles, the robot WILL collide with any objects in the way! Make sure the robot has approximately 3 meters of free space on all sides.");

  // ArRobotConnector connects to the robot, get some initial data from it such as type and name,
  // and then loads parameter files for this robot.
  ArRobotConnector robotConnector(&parser, &robot);
  if(!robotConnector.connectRobot())
  {
    ArLog::log(ArLog::Terse, "simpleMotionCommands: Could not connect to the robot.");
    if(parser.checkHelpAndWarnUnparsed())
    {
        Aria::logOptions();
        Aria::exit(1);
        return 1;
    }
  }
  if (!Aria::parseArgs())
  {
    Aria::logOptions();
    Aria::exit(1);
    return 1;
  }
  
  ArLog::log(ArLog::Normal, "simpleMotionCommands: Connected.");

  // Start the robot processing cycle running in the background.
  // True parameter means that if the connection is lost, then the 
  // run loop ends.
  robot.runAsync(true);

  // Print out some data from the SIP.  

  // We must "lock" the ArRobot object
  // before calling its methods, and "unlock" when done, to prevent conflicts
  // with the background thread started by the call to robot.runAsync() above.
  // See the section on threading in the manual for more about this.
  // Make sure you unlock before any sleep() call or any other code that will
  // take some time; if the robot remains locked during that time, then
  // ArRobot's background thread will be blocked and unable to communicate with
  // the robot, call tasks, etc.
  
  robot.lock();
  ArLog::log(ArLog::Normal, "simpleMotionCommands: Pose=(%.2f,%.2f,%.2f), Trans. Vel=%.2f, Rot. Vel=%.2f, Battery=%.2fV",
    robot.getX(), robot.getY(), robot.getTh(), robot.getVel(), robot.getRotVel(), robot.getBatteryVoltage());
  robot.unlock();

  // Sleep for 3 seconds.
  ArLog::log(ArLog::Normal, "simpleMotionCommands: Will start driving in 3 seconds...");
  ArUtil::sleep(3000);

  // Set forward velocity to 50 mm/s
  ArLog::log(ArLog::Normal, "simpleMotionCommands: Driving forward at 250 mm/s for 5 sec...");
  robot.lock();
  robot.enableMotors();
  robot.setVel(250);
  robot.unlock();
  ArUtil::sleep(5000);

  ArLog::log(ArLog::Normal, "simpleMotionCommands: Stopping.");
  robot.lock();
  robot.stop();
  robot.unlock();
  ArUtil::sleep(1000);

  ArLog::log(ArLog::Normal, "simpleMotionCommands: Rotating at 10 deg/s for 5 sec...");
  robot.lock();
  robot.setRotVel(10);
  robot.unlock();
  ArUtil::sleep(5000);

  ArLog::log(ArLog::Normal, "simpleMotionCommands: Rotating at -10 deg/s for 10 sec...");
  robot.lock();
  robot.setRotVel(-10);
  robot.unlock();
  ArUtil::sleep(10000);

  ArLog::log(ArLog::Normal, "simpleMotionCommands: Driving forward at 150 mm/s for 5 sec...");
  robot.lock();
  robot.setRotVel(0);
  robot.setVel(150);
  robot.unlock();
  ArUtil::sleep(5000);

  ArLog::log(ArLog::Normal, "simpleMotionCommands: Stopping.");
  robot.lock();
  robot.stop();
  robot.unlock();
  ArUtil::sleep(1000);


  // Other motion command functions include move(), setHeading(),
  // setDeltaHeading().  You can also adjust acceleration and deceleration
  // values used by the robot with setAccel(), setDecel(), setRotAccel(),
  // setRotDecel().  See the ArRobot class documentation for more.

  
  robot.lock();
  ArLog::log(ArLog::Normal, "simpleMotionCommands: Pose=(%.2f,%.2f,%.2f), Trans. Vel=%.2f, Rot. Vel=%.2f, Battery=%.2fV",
    robot.getX(), robot.getY(), robot.getTh(), robot.getVel(), robot.getRotVel(), robot.getBatteryVoltage());
  robot.unlock();

  
  ArLog::log(ArLog::Normal, "simpleMotionCommands: Ending robot thread...");
  robot.stopRunning();

  // wait for the thread to stop
  robot.waitForRunExit();

  // exit
  ArLog::log(ArLog::Normal, "simpleMotionCommands: Exiting.");
  Aria::exit(0);
  return 0;
}