Go Task 直线行走

首发原创，LEEMANCAFFE

直线往前行走

# include "Aria.h"
# include <iostream>

using namespace std;

ArRobot* robot;

int Go(double Speed,double Dist)
{
	cout << "Go Forward :"<< Dist<< " mm at speed "<<Speed <<"mm/s "<< endl;
	double Dist0 = 0;
	double dt = 200;
	while(Dist0 < Dist)
	{
		robot->lock();
		robot->enableMotors();
		if((Dist-Dist0) < 300)
		{
			robot->setVel((Dist-Dist0)/300*Speed+5);
		}
		else if(Dist0 <300 && !((Dist-Dist0)<300))
		{
			robot->setVel(Dist0/300*Speed + 5);
		}
		else
		{
			robot->setVel(Speed);
		}

		Dist0+=robot->getVel()*dt/1000;
		cout << "D=\t"<<Dist0 <<"S=\t"<<robot->getVel()<<endl;
		robot->unlock();
		ArUtil::sleep(dt);
	}
	robot->lock();
	robot->setVel(0);
	robot->unlock();
	return 1;
}

double sp=200,ds=1000;

int main(int argc, char **argv)
{
    int ret;
	string str;
  // the connection for Remote Host and Simulator
   ArTcpConnection con;
  // the connection through Serial Link to the robot
  //ArSerialConnection con;
  //ArGlobalFunctor updateCB(&update);

  robot = new ArRobot;

  // mandatory init
  Aria::init();

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

  // set the device 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;
  }
// add the sonar to the robot
  ArSonarDevice sonar;
  robot->addRangeDevice(&sonar);

  robot->comInt(ArCommands::SONAR, 1);
  robot->comInt(ArCommands::ENABLE, 1);
  robot->comInt(ArCommands::SOUNDTOG, 0);
  
  // start the robot running, true so that if we lose connection the run stops
  robot->runAsync(true);
  Go(sp,ds);
  robot->lock();
  robot->disconnect();
  robot->unlock();

  // now exit
  Aria::shutdown();
  return 0;

}

非指针写法：

# include "Aria.h"
# include <iostream>

using namespace std;


int Go(ArRobot* robot,double Speed,double Dist)
{
	cout << "Go Forward :"<< Dist<< " mm at speed "<<Speed <<"mm/s "<< endl;
	double Dist0 = 0;
	double dt = 200;
	while(Dist0 < Dist)
	{
		robot->lock();
		robot->enableMotors();
		if((Dist-Dist0) < 300)
		{
			robot->setVel((Dist-Dist0)/300*Speed+5);
		}
		else if(Dist0 <300 && !((Dist-Dist0)<300))
		{
			robot->setVel(Dist0/300*Speed + 5);
		}
		else
		{
			robot->setVel(Speed);
		}

		Dist0+=robot->getVel()*dt/1000;
		cout << "D=\t"<<Dist0 <<"S=\t"<<robot->getVel()<<endl;
		robot->unlock();
		ArUtil::sleep(dt);
	}
	robot->lock();
	robot->setVel(0);
	robot->unlock();
	return 1;
}

double sp=200,ds=1000;

int main(int argc, char **argv)
{
    int ret;
	string str;
  // the connection for Remote Host and Simulator
   ArTcpConnection con;
  // the connection through Serial Link to the robot
  //ArSerialConnection con;
  //ArGlobalFunctor updateCB(&update);

  ArRobot robot;

  // mandatory init
  Aria::init();

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

  // set the device 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;
  }
// add the sonar to the robot
  ArSonarDevice sonar;
  robot.addRangeDevice(&sonar);

  robot.comInt(ArCommands::SONAR, 1);
  robot.comInt(ArCommands::ENABLE, 1);
  robot.comInt(ArCommands::SOUNDTOG, 0);
  
  // start the robot running, true so that if we lose connection the run stops
  robot.runAsync(true);
  Go(&robot,sp,ds);
  robot.lock();
  robot.disconnect();
  robot.unlock();

  // now exit
  Aria::shutdown();
  return 0;

}