首发原创,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;
}