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