Turn Task 转弯

首发原创，LEEMANCAFFE

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

using namespace std;


double sp=200,ds=1000;
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 spr=20,ag=360;
int Turn(ArRobot* robot,double Speedr,double Angle)
{
	cout << "Turn:"<< 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;
}


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);
  Turn(&robot,spr,ag);
  robot.lock();
  robot.disconnect();
  robot.unlock();

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

}