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;

}