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