int main(int argc, char **argv)
{
bool done;
double distToTravel = 2300;
// whether to use the sim for the laser or not, if you use the sim
// for hte laser, you have to use the sim for the robot too
bool useSim = false;
// the laser
ArSick sick;
// connection
ArDeviceConnection *con;
// Laser connection
ArSerialConnection laserCon;
// robot
ArRobot robot;
// set a default filename
//std::string filename = "c:\\log\\1scans.2d";
std::string filename = "1scans.2d";
// see if we want to use a different filename
//if (argc > 1)
//Lfilename = argv[1];
printf("Logging to file %s\n", filename.c_str());
// start the logger with good values
sick.configureShort(useSim, ArSick::BAUD38400,
ArSick::DEGREES180, ArSick::INCREMENT_HALF);
ArSickLogger logger(&robot, &sick, 300, 25, filename.c_str());
// mandatory init
Aria::init();
// add it to the robot
robot.addRangeDevice(&sick);
//ArAnalogGyro gyro(&robot);
// if we're not using the sim, make a serial connection and set it up
if (!useSim)
{
ArSerialConnection *serCon;
serCon = new ArSerialConnection;
serCon->setPort();
//serCon->setBaud(38400);
con = serCon;
}
// if we are using the sim, set up a tcp connection
else
{
ArTcpConnection *tcpCon;
tcpCon = new ArTcpConnection;
tcpCon->setPort();
con = tcpCon;
}
// set the 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;
}
// set up a key handler so escape exits and attach to the robot
ArKeyHandler keyHandler;
robot.attachKeyHandler(&keyHandler);
// run the robot, true here so that the run will exit if connection lost
robot.runAsync(true);
// if we're not using the sim, set up the port for the laser
if (!useSim)
{
laserCon.setPort(ArUtil::COM3);
sick.setDeviceConnection(&laserCon);
}
// now that we're connected to the robot, connect to the laser
sick.runAsync();
if (!sick.blockingConnect())
{
printf("Could not connect to SICK laser... exiting\n");
robot.disconnect();
Aria::shutdown();
return 1;
}
#ifdef WIN32
// wait until someone pushes the motor button to go
while (1)
{
robot.lock();
if (!robot.isRunning())
exit(0);
if (robot.areMotorsEnabled())
{
robot.unlock();
break;
}
robot.unlock();
ArUtil::sleep(100);
}
#endif
// basically from here on down the robot just cruises around a bit
robot.lock();
// enable the motors, disable amigobot sounds
robot.comInt(ArCommands::ENABLE, 1);
ArTime startTime;
// move a couple meters
robot.move(distToTravel);
robot.unlock();
startTime.setToNow();
do {
ArUtil::sleep(100);
robot.lock();
robot.setHeading(0);
done = robot.isMoveDone(60);
robot.unlock();
} while (!done);
/*
// rotate a few times
robot.lock();
robot.setVel(0);
robot.setRotVel(60);
robot.unlock();
ArUtil::sleep(12000);
*/
robot.lock();
robot.setHeading(180);
robot.unlock();
do {
ArUtil::sleep(100);
robot.lock();
robot.setHeading(180);
done = robot.isHeadingDone();
robot.unlock();
} while (!done);
// move a couple meters
robot.lock();
robot.move(distToTravel);
robot.unlock();
startTime.setToNow();
do {
ArUtil::sleep(100);
robot.lock();
robot.setHeading(180);
done = robot.isMoveDone(60);
robot.unlock();
} while (!done);
robot.lock();
robot.setHeading(0);
robot.setVel(0);
robot.unlock();
startTime.setToNow();
do {
ArUtil::sleep(100);
robot.lock();
robot.setHeading(0);
done = robot.isHeadingDone();
robot.unlock();
} while (!done);
sick.lockDevice();
sick.disconnect();
sick.unlockDevice();
robot.lock();
robot.disconnect();
robot.unlock();
// now exit
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;
}
