GoTo Point Task

首发原创，LEEMANCAFFE

# include "Aria.h"# include <iostream>using namespace std;ArRobot* robot;double sp=200,ds=1000;int Go(ArRobot* robot,double Speed,double Dist){    cout << "Go Forward_LINE :"<< 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 spr=20,ag=360;int Turn(ArRobot* robot,double Speedr,double Angle){    cout << "Turn__ROT:"<< Angle << " deg at speed "<<Speedr <<"deg/s "<< endl;    double Angle0 = 0;    double dt = 200;    double dirction=0;    while(Angle0 < Angle)    {        robot->lock();        robot->enableMotors();        if((Angle-Angle0) < 45)        {            if(Speedr < 0)                robot->setRotVel((Angle-Angle0)/45*Speedr - 2);            else                robot->setRotVel((Angle-Angle0)/45*Speedr + 2);        }        else if(Angle0 <30 && !((Angle-Angle0)<45))        {            if(Speedr < 0 )                robot->setRotVel(Angle0/45*Speedr - 2);            else                robot->setRotVel(Angle0/45*Speedr + 2);        }        else        {            robot->setRotVel(Speedr);        }        if(robot->getRotVel()<0)            Angle0+=-robot->getRotVel()*dt/1000;        else            Angle0+=robot->getRotVel()*dt/1000;        cout << "A="<<Angle0 <<"\tSPEED-ROT=\t"<<robot->getRotVel()<<endl;        robot->unlock();        ArUtil::sleep(dt);    }    robot->lock();    robot->setRotVel(0);    robot->unlock();    return 1;}double Posx = 0,Posy=0,Posteta=0;int GoTo(ArRobot *robot,double x,double y){    cout<<"GOTO POINT "<<"x:\t"<<x<<"y:\t"<<y<<endl;    double Angle;    double Dist;    if((x-Posx)>0 && (y-Posy)>=0)        Angle = atan((y-Posy)/(x-Posx));    else if((x-Posx)<= 0 && (y-Posy)>0)        Angle = atan((y-Posy)/(x-Posx))+3.14159;    else if((x-Posx)< 0 && (y-Posy)<=0)        Angle = atan((y-Posy)/(x-Posx))+3.14159;    else if((x-Posx)>= 0 && (y-Posy)>0)        Angle = atan((y-Posy)/(x-Posx))+2*3.14159;    double SaveAngle;    SaveAngle = Angle;    Angle-=Posteta;    Dist = sqrt((x-Posx)*(x-Posx)+(y-Posy)*(y-Posy));    if(Angle>3.14159)        Turn(robot,-20,360-Angle/3.14159*180);    else if(Angle<0 && Angle>-3.14159)        Turn(robot,-20,2*3.14159-Angle/3.14159*180);    else if(Angle<0)        Turn(robot,20,-Angle/3.14159*180);    else        Turn(robot,20,Angle/3.14159*180);    Go(robot,250,Dist);    Posx =x ;    Posy = y;    return 1;}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(robot,sp,ds);  //Turn(robot,spr,ag);  GoTo(robot,5000,5000);  robot->lock();  robot->disconnect();  robot->unlock();  // now exit  Aria::shutdown();  return 0;}