int main(int argc, char **argv)
{
std::string str;
int ret;
int dist;
ArTime start;
ArPose startPose;
bool vel2 = false;
// connection to the robot
ArSerialConnection con;
// the robot
ArRobot robot;
// the connection handler from above
ConnHandler ch(&robot);
// init area with a dedicated signal handling thread
Aria::init(Aria::SIGHANDLE_THREAD);
if (argc != 2 || (dist = atoi(argv[1])) == 0)
{
printf("Usage: %s <distInMM>\n", argv[0]);
exit(0);
}
if (dist < 1000)
{
printf("You must go at least a meter\n");
exit(0);
}
// open the connection with the defaults, exit if failed
if ((ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
Aria::shutdown();
return 1;
}
// set the robots connection
robot.setDeviceConnection(&con);
// try to connect, if we fail, the connection handler should bail
if (!robot.blockingConnect())
{
// this should have been taken care of by the connection handler
// but just in case
printf(
"asyncConnect failed because robot is not running in its own thread.\n");
Aria::shutdown();
return 1;
}
// run the robot in its own thread, so it gets and processes packets and such
robot.runAsync(false);
// just a big long set of printfs, direct motion commands and sleeps,
// it should be self-explanatory
robot.lock();
/*
robot.setAbsoluteMaxTransVel(2000);
robot.setTransVelMax(2000);
robot.setTransAccel(1000);
robot.setTransDecel(1000);
robot.comInt(82, 30); // rotkp
robot.comInt(83, 200); // rotkv
robot.comInt(84, 0); // rotki
robot.comInt(85, 30); // transkp
robot.comInt(86, 450); // transkv
robot.comInt(87, 4); // transki
*/
printf("Driving %d mm (going full speed for that far minus a meter then stopping)\n", dist);
if (vel2)
robot.setVel2(2200, 2200);
else
robot.setVel(2200);
robot.unlock();
start.setToNow();
startPose = robot.getPose();
while (1)
{
robot.lock();
printf("\r vel: %.0f x: %.0f y: %.0f: dist: %.0f heading: %.2f",
robot.getVel(), robot.getX(), robot.getY(),
startPose.findDistanceTo(robot.getPose()),
robot.getTh());
if (startPose.findDistanceTo(robot.getPose()) > abs(dist) - 1000)
{
printf("\nFinished distance\n");
robot.setVel(0);
robot.unlock();
break;
}
if (start.mSecSince() > 10000)
{
printf("\nDistance timed out\n");
robot.setVel(0);
robot.unlock();
break;
}
robot.unlock();
ArUtil::sleep(50);
}
if (vel2)
robot.setVel2(0, 0);
else
robot.setVel(0);
start.setToNow();
while (1)
{
robot.lock();
if (vel2)
robot.setVel2(0, 0);
else
robot.setVel(0);
if (fabs(robot.getVel()) < 20)
{
printf("Stopped\n");
robot.unlock();
break;
}
if (start.mSecSince() > 2000)
{
printf("\nStop timed out\n");
robot.unlock();
break;
}
robot.unlock();
ArUtil::sleep(50);
}
robot.lock();
robot.disconnect();
robot.unlock();
// shutdown and ge tout
Aria::shutdown();
return 0;
}
int main(int argc, char **argv)
{
Aria::init();
ArArgumentParser argParser(&argc, argv);
ArSimpleConnector con(&argParser);
ArRobot robot;
// the connection handler from above
ConnHandler ch(&robot);
if(!Aria::parseArgs())
{
Aria::logOptions();
Aria::shutdown();
return 1;
}
if(!con.connectRobot(&robot))
{
ArLog::log(ArLog::Normal, "directMotionExample: Could not connect to the robot. Exiting.");
return 1;
}
ArLog::log(ArLog::Normal, "directMotionExample: Connected.");
// Run the robot processing cycle in its own thread. Note that after starting this
// thread, we must lock and unlock the ArRobot object before and after
// accessing it.
robot.runAsync(false);
// Send the robot a series of motion commands directly, sleeping for a
// few seconds afterwards to give the robot time to execute them.
printf("directMotionExample: Setting rot velocity to 100 deg/sec then sleeping 3 seconds\n");
robot.lock();
robot.setRotVel(100);
robot.unlock();
ArUtil::sleep(3*1000);
printf("Stopping\n");
robot.lock();
robot.setRotVel(0);
robot.unlock();
ArUtil::sleep(200);
printf("directMotionExample: Telling the robot to go 300 mm on left wheel and 100 mm on right wheel for 5 seconds\n");
robot.lock();
robot.setVel2(300, 100);
robot.unlock();
ArTime start;
start.setToNow();
while (1)
{
robot.lock();
if (start.mSecSince() > 5000)
{
robot.unlock();
break;
}
robot.unlock();
ArUtil::sleep(50);
}
printf("directMotionExample: Telling the robot to move forwards one meter, then sleeping 5 seconds\n");
robot.lock();
robot.move(1000);
robot.unlock();
start.setToNow();
while (1)
{
robot.lock();
if (robot.isMoveDone())
{
printf("directMotionExample: Finished distance\n");
robot.unlock();
break;
}
if (start.mSecSince() > 5000)
{
printf("directMotionExample: Distance timed out\n");
robot.unlock();
break;
}
robot.unlock();
ArUtil::sleep(50);
}
printf("directMotionExample: Telling the robot to move backwards one meter, then sleeping 5 seconds\n");
robot.lock();
robot.move(-1000);
robot.unlock();
start.setToNow();
while (1)
{
robot.lock();
if (robot.isMoveDone())
{
printf("directMotionExample: Finished distance\n");
robot.unlock();
break;
}
if (start.mSecSince() > 10000)
{
printf("directMotionExample: Distance timed out\n");
robot.unlock();
break;
}
robot.unlock();
ArUtil::sleep(50);
}
printf("directMotionExample: Telling the robot to turn to 180, then sleeping 4 seconds\n");
robot.lock();
robot.setHeading(180);
robot.unlock();
start.setToNow();
while (1)
{
robot.lock();
if (robot.isHeadingDone(5))
{
printf("directMotionExample: Finished turn\n");
robot.unlock();
break;
}
if (start.mSecSince() > 5000)
{
printf("directMotionExample: Turn timed out\n");
robot.unlock();
break;
}
robot.unlock();
ArUtil::sleep(100);
}
printf("directMotionExample: Telling the robot to turn to 90, then sleeping 2 seconds\n");
robot.lock();
robot.setHeading(90);
robot.unlock();
start.setToNow();
while (1)
{
robot.lock();
if (robot.isHeadingDone(5))
{
printf("directMotionExample: Finished turn\n");
robot.unlock();
break;
}
if (start.mSecSince() > 5000)
{
printf("directMotionExample: turn timed out\n");
robot.unlock();
break;
}
robot.unlock();
ArUtil::sleep(100);
}
printf("directMotionExample: Setting vel2 to 200 mm/sec on both wheels, then sleeping 3 seconds\n");
robot.lock();
robot.setVel2(200, 200);
robot.unlock();
ArUtil::sleep(3000);
printf("directMotionExample: Stopping the robot, then sleeping for 2 seconds\n");
robot.lock();
robot.stop();
robot.unlock();
ArUtil::sleep(2000);
printf("directMotionExample: Setting velocity to 200 mm/sec then sleeping 3 seconds\n");
robot.lock();
robot.setVel(200);
robot.unlock();
ArUtil::sleep(3000);
printf("directMotionExample: Stopping the robot, then sleeping for 2 seconds\n");
robot.lock();
robot.stop();
robot.unlock();
ArUtil::sleep(2000);
printf("directMotionExample: Setting vel2 with 0 on left wheel, 200 mm/sec on right, then sleeping 5 seconds\n");
robot.lock();
robot.setVel2(0, 200);
robot.unlock();
ArUtil::sleep(5000);
printf("directMotionExample: Telling the robot to rotate at 50 deg/sec then sleeping 5 seconds\n");
robot.lock();
robot.setRotVel(50);
robot.unlock();
ArUtil::sleep(5000);
printf("directMotionExample: Telling the robot to rotate at -50 deg/sec then sleeping 5 seconds\n");
robot.lock();
robot.setRotVel(-50);
robot.unlock();
ArUtil::sleep(5000);
printf("directMotionExample: Setting vel2 with 0 on both wheels, then sleeping 3 seconds\n");
robot.lock();
robot.setVel2(0, 0);
robot.unlock();
ArUtil::sleep(3000);
printf("directMotionExample: Now having the robot change heading by -125 degrees, then sleeping for 6 seconds\n");
robot.lock();
robot.setDeltaHeading(-125);
robot.unlock();
ArUtil::sleep(6000);
printf("directMotionExample: Now having the robot change heading by 45 degrees, then sleeping for 6 seconds\n");
robot.lock();
robot.setDeltaHeading(45);
robot.unlock();
ArUtil::sleep(6000);
printf("directMotionExample: Setting vel2 with 200 on left wheel, 0 on right wheel, then sleeping 5 seconds\n");
robot.lock();
robot.setVel2(200, 0);
robot.unlock();
ArUtil::sleep(5000);
printf("directMotionExample: Done, shutting down Aria and exiting.\n");
Aria::shutdown();
return 0;
}
