int main(void)
{
int ret;
std::string str;
CBTest cbTest;
ArFunctorC<CBTest> connectCB(&cbTest, &CBTest::connected);
ArFunctorC<CBTest> failedConnectCB(&cbTest, &CBTest::failedConnect);
ArFunctorC<CBTest> disconnectCB(&cbTest, &CBTest::disconnected);
ArFunctorC<CBTest> disconnectErrorCB(&cbTest, &CBTest::disconnectedError);
ArSerialConnection con;
ArRobot robot;
printf("If a robot is attached to your port you should see:\n");
printf("Failed connect, Connected, Disconnected Error, Connected, Disconnected\n");
printf("If no robot is attached you should see:\n");
printf("Failed connect, Failed connect, Failed connect\n");
printf("-------------------------------------------------------\n");
ArLog::init(ArLog::None, ArLog::Terse);
srand(time(NULL));
robot.setDeviceConnection(&con);
robot.addConnectCB(&connectCB, ArListPos::FIRST);
robot.addFailedConnectCB(&failedConnectCB, ArListPos::FIRST);
robot.addDisconnectNormallyCB(&disconnectCB, ArListPos::FIRST);
robot.addDisconnectOnErrorCB(&disconnectErrorCB, ArListPos::FIRST);
// this should fail since there isn't an open port yet
robot.blockingConnect();
if ((ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
exit(0);
}
robot.blockingConnect();
con.close();
robot.loopOnce();
if ((ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
exit(0);
}
robot.blockingConnect();
robot.disconnect();
exit(0);
}
int main(int argc, char **argv)
{
std::string str;
int ret;
ArGlobalRetFunctor1<bool, ArRobotPacket *> tcm2PrinterCB(&tcm2Printer);
ArSerialConnection con;
Aria::init();
robot = new ArRobot;
robot->addPacketHandler(&tcm2PrinterCB, ArListPos::FIRST);
if ((ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
exit(0);
}
robot->setDeviceConnection(&con);
if (!robot->blockingConnect())
{
printf("Could not connect to robot... exiting\n");
exit(0);
}
printf("%6s %6s %6s %6s %6s %6s %6s %10s %4s %4s %6s\n",
"comp", "pitch", "roll", "magX", "magY", "magZ", "temp", "error",
"calH", "calV", "calM");
robot->comInt(ArCommands::TCM2, 3);
robot->run(true);
Aria::shutdown();
}
int main(int argc, char **argv)
{
std::string str;
int ret;
ArTime start;
// 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);
// 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);
int i;
while (Aria::getRunning())
{
robot.lock();
robot.comStr(ArCommands::TTY3, "1234567890");
robot.unlock();
}
robot.disconnect();
// shutdown and ge tout
Aria::shutdown();
return 0;
}
int main(int argc, char **argv)
{
// just some stuff for returns
std::string str;
int ret;
// robots connection
ArSerialConnection con;
// the robot, this turns state reflection off
ArRobot robot(NULL, false);
// the joydrive as defined above, this also adds itself as a user task
Joydrive joyd(&robot);
// mandatory init
Aria::init();
// open the connection, if it 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 connection o nthe robot
robot.setDeviceConnection(&con);
// connect, if we fail, exit
if (!robot.blockingConnect())
{
printf("Could not connect to robot... exiting\n");
Aria::shutdown();
return 1;
}
// turn off the sonar, enable the motors, turn off amigobot sounds
robot.comInt(ArCommands::SONAR, 0);
robot.comInt(ArCommands::ENABLE, 1);
robot.comInt(ArCommands::SOUNDTOG, 0);
// run, if we lose connection to the robot, exit
robot.run(true);
// shutdown and go away
Aria::shutdown();
return 0;
}
int main()
{
ArModuleLoader::Status status;
ArSerialConnection con;
ArRobot robot;
int ret;
std::string str;
Aria::init();
status=ArModuleLoader::load("./joydriveActionMod", &robot);
printStatus(status);
if (status == ArModuleLoader::STATUS_INIT_FAILED)
return(1);
if ((ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
Aria::shutdown();
return 1;
}
robot.setDeviceConnection(&con);
if (!robot.blockingConnect())
{
printf("Could not connect to robot... exiting\n");
Aria::shutdown();
return 1;
}
robot.comInt(ArCommands::SONAR, 0);
robot.comInt(ArCommands::ENABLE, 1);
robot.comInt(ArCommands::SOUNDTOG, 0);
robot.run(true);
status=ArModuleLoader::close("./joydriveActionMod");
printStatus(status);
Aria::shutdown();
return 0;
}
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)
{
std::string str;
int ret;
time_t lastTime;
int trans, rot;
ArJoyHandler joyHandler;
ArSerialConnection con;
ArRobot robot(NULL, false);
joyHandler.init();
joyHandler.setSpeeds(100, 700);
if (joyHandler.haveJoystick())
{
printf("Have a joystick\n\n");
}
else
{
printf("Do not have a joystick, set up the joystick then rerun the program\n\n");
// exit(0);
}
if ((ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
exit(0);
}
robot.setDeviceConnection(&con);
if (!robot.blockingConnect())
{
printf("Could not connect to robot... exiting\n");
exit(0);
}
robot.comInt(ArCommands::SONAR, 0);
robot.comInt(ArCommands::ENABLE, 1);
robot.comInt(ArCommands::SOUNDTOG, 0);
//robot.comInt(ArCommands::ENCODER, 1);
//robot.comStrN(ArCommands::SAY, "\1\6\2\105", 4);
lastTime = time(NULL);
while (1)
{
if (!robot.isConnected())
{
printf("No longer connected to robot, exiting.\n");
exit(0);
}
robot.loopOnce();
if (lastTime != time(NULL))
{
printf("\rx %6.1f y %6.1f tth %6.1f vel %7.1f mpacs %3d", robot.getX(),
robot.getY(), robot.getTh(), robot.getVel(),
robot.getMotorPacCount());
fflush(stdout);
lastTime = time(NULL);
}
if (joyHandler.haveJoystick() && (joyHandler.getButton(1) ||
joyHandler.getButton(2)))
{
if (ArMath::fabs(robot.getVel()) < 10.0)
robot.comInt(ArCommands::ENABLE, 1);
joyHandler.getAdjusted(&rot, &trans);
robot.comInt(ArCommands::VEL, trans);
robot.comInt(ArCommands::RVEL, -rot);
}
else
{
robot.comInt(ArCommands::VEL, 0);
robot.comInt(ArCommands::RVEL, 0);
}
ArUtil::sleep(100);
}
}
int main(int argc, char **argv)
{
std::string str;
int ret;
int successes = 0, failures = 0;
int action;
bool exitOnFailure = true;
ArSerialConnection con;
ArRobot robot;
//ArLog::init(ArLog::StdOut, ArLog::Verbose);
srand(time(NULL));
robot.runAsync(false);
// if (!exitOnFailure)
// ArLog::init(ArLog::None, ArLog::Terse);
//else
//ArLog::init(ArLog::None);
while (1)
{
if (con.getStatus() != ArDeviceConnection::STATUS_OPEN &&
(ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
++failures;
if (exitOnFailure)
{
printf("Failed\n");
exit(0);
}
else
{
ArUtil::sleep(200);
robot.unlock();
continue;
}
}
robot.lock();
robot.setDeviceConnection(&con);
robot.unlock();
ArUtil::sleep((rand() % 5) * 100);
if (robot.asyncConnect())
{
robot.waitForConnectOrConnFail();
robot.lock();
if (!robot.isConnected())
{
if (exitOnFailure)
{
printf("Failed after %d tries.\n", successes);
exit(0);
}
printf("Failed to connect successfully");
++failures;
}
robot.comInt(ArCommands::SONAR, 0);
robot.comInt(ArCommands::SOUNDTOG, 0);
//robot.comInt(ArCommands::PLAYLIST, 0);
robot.comInt(ArCommands::ENCODER, 1);
ArUtil::sleep(((rand() % 20) + 3) * 100);
++successes;
// okay, now try to leave it in a messed up state
action = rand() % 8;
robot.dropConnection();
switch (action) {
case 0:
printf("Discon 0 ");
robot.disconnect();
ArUtil::sleep(100);
robot.com(0);
break;
case 1:
printf("Discon 1 ");
robot.disconnect();
ArUtil::sleep(100);
robot.com(0);
ArUtil::sleep(100);
robot.com(1);
break;
case 2:
printf("Discon 2 ");
robot.disconnect();
ArUtil::sleep(100);
robot.com(0);
ArUtil::sleep(100);
robot.com(1);
ArUtil::sleep(100);
robot.com(2);
break;
case 3:
printf("Discon 10 ");
robot.disconnect();
ArUtil::sleep(100);
robot.com(10);
break;
case 4:
printf("Discon ");
robot.disconnect();
break;
default:
printf("Leave ");
break;
}
robot.unlock();
}
else
{
if (exitOnFailure)
{
printf("Failed after %d tries.\n", successes);
exit(0);
}
printf("Failed to start connect ");
++failures;
}
if ((rand() % 2) == 0)
{
printf(" ! RadioDisconnect ! ");
con.write("|||\15", strlen("!!!\15"));
ArUtil::sleep(100);
con.write("WMD\15", strlen("WMD\15"));
ArUtil::sleep(200);
}
if ((rand() % 2) == 0)
{
printf(" ! ClosePort !\n");
con.close();
}
else
printf("\n");
printf("#### %d successes %d failures, %% %.2f success\n", successes, failures,
(float)successes/(float)(successes+failures)*100);
ArUtil::sleep((rand() % 2)* 1000);
}
return 0;
}
int main(void)
{
ArSerialConnection con;
ArRobot robot;
int ret;
std::string str;
ArActionLimiterForwards limiter("speed limiter near", 225, 600, 250);
ArActionLimiterForwards limiterFar("speed limiter far", 225, 1100, 400);
ArActionTableSensorLimiter tableLimiter;
ArActionLimiterBackwards backwardsLimiter;
ArActionConstantVelocity stop("stop", 0);
ArSonarDevice sonar;
ArACTS_1_2 acts;
ArPTZ *ptz;
ptz = new ArVCC4(&robot, true);
ArGripper gripper(&robot);
Acquire acq(&acts, &gripper);
DriveTo driveTo(&acts, &gripper, ptz);
DropOff dropOff(&acts, &gripper, ptz);
PickUp pickUp(&acts, &gripper, ptz);
TakeBlockToWall takeBlock(&robot, &gripper, ptz, &acq, &driveTo, &pickUp,
&dropOff, &tableLimiter);
if (!acts.openPort(&robot))
{
printf("Could not connect to acts, exiting\n");
exit(0);
}
Aria::init();
robot.addRangeDevice(&sonar);
//con.setBaud(38400);
if ((ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
Aria::shutdown();
return 1;
}
robot.setDeviceConnection(&con);
if (!robot.blockingConnect())
{
printf("Could not connect to robot... exiting\n");
Aria::shutdown();
return 1;
}
ptz->init();
ArUtil::sleep(8000);
printf("### 2222\n");
ptz->panTilt(0, -40);
printf("### whee\n");
ArUtil::sleep(8000);
robot.setAbsoluteMaxTransVel(400);
robot.setStateReflectionRefreshTime(250);
robot.comInt(ArCommands::ENABLE, 1);
robot.comInt(ArCommands::SOUNDTOG, 0);
ArUtil::sleep(200);
robot.addAction(&tableLimiter, 100);
robot.addAction(&limiter, 99);
robot.addAction(&limiterFar, 98);
robot.addAction(&backwardsLimiter, 97);
robot.addAction(&acq, 77);
robot.addAction(&driveTo, 76);
robot.addAction(&pickUp, 75);
robot.addAction(&dropOff, 74);
robot.addAction(&stop, 30);
robot.run(true);
Aria::shutdown();
return 0;
}
int main(void)
{
ArSerialConnection con;
ArRobot robot;
int ret;
std::string str;
ArActionLimiterForwards limiter("speed limiter near", 300, 600, 250);
ArActionLimiterForwards limiterFar("speed limiter far", 300, 1100, 400);
ArActionLimiterBackwards backwardsLimiter;
ArActionConstantVelocity stop("stop", 0);
ArActionConstantVelocity backup("backup", -200);
ArSonarDevice sonar;
ArACTS_1_2 acts;
ArSonyPTZ sony(&robot);
ArGripper gripper(&robot, ArGripper::GENIO);
Acquire acq(&acts, &gripper);
DriveTo driveTo(&acts, &gripper, &sony);
PickUp pickUp(&acts, &gripper, &sony);
TakeBlockToWall takeBlock(&robot, &gripper, &sony, &acq, &driveTo, &pickUp,
&backup);
Aria::init();
if (!acts.openPort(&robot))
{
printf("Could not connect to acts\n");
exit(1);
}
robot.addRangeDevice(&sonar);
//con.setBaud(38400);
if ((ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
Aria::shutdown();
return 1;
}
robot.setDeviceConnection(&con);
if (!robot.blockingConnect())
{
printf("Could not connect to robot... exiting\n");
Aria::shutdown();
return 1;
}
sony.init();
ArUtil::sleep(1000);
//robot.setAbsoluteMaxTransVel(400);
robot.setStateReflectionRefreshTime(250);
robot.comInt(ArCommands::ENABLE, 1);
robot.comInt(ArCommands::SOUNDTOG, 0);
ArUtil::sleep(200);
robot.addAction(&limiter, 100);
robot.addAction(&limiterFar, 99);
robot.addAction(&backwardsLimiter, 98);
robot.addAction(&acq, 77);
robot.addAction(&driveTo, 76);
robot.addAction(&pickUp, 75);
robot.addAction(&backup, 50);
robot.addAction(&stop, 30);
robot.run(true);
Aria::shutdown();
return 0;
}
int main(int argc, char **argv)
{
int ret;
std::string str;
ArSerialConnection con;
double dist, angle;
std::list<ArPoseWithTime *> *readings;
std::list<ArPoseWithTime *>::iterator it;
double farDist, farAngle;
bool found;
ArGlobalFunctor failedConnectCB(&failedConnect);
Aria::init();
sick = new ArSick;
// open the connection, if it fails, exit
if ((ret = con.open("/dev/ttyS2")) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
Aria::shutdown();
return 1;
}
sick->configure(false);
sick->setDeviceConnection(&con);
sick->setFilterNearDist(0);
sick->setMinRange(0);
sick->addFailedConnectCB(&failedConnectCB, ArListPos::FIRST);
sick->runAsync();
ArUtil::sleep(100);
sick->lockDevice();
sick->asyncConnect();
sick->unlockDevice();
while (!sick->isConnected())
ArUtil::sleep(100);
printf("Connected\n");
// while (sick->isConnected())
int times = 0;
while (sick->getRunning())
{
//dist = sick->getCurrentBuffer().getClosestPolar(-90, 90, ArPose(0, 0), 30000, &angle);
sick->lockDevice();
dist = sick->currentReadingPolar(-90, 90, &angle);
if (dist < sick->getMaxRange())
printf("Closest reading %.2f mm away at %.2f degrees\n", dist, angle);
else
printf("No close reading.\n");
readings = sick->getCurrentBuffer();
int i = 0;
for (it = readings->begin(), found = false; it != readings->end(); it++)
{
i++;
dist = (*it)->findDistanceTo(ArPose(0, 0));
angle = (*it)->findAngleTo(ArPose(0, 0));
if (!found || dist > farDist)
{
found = true;
farDist = dist;
farAngle = angle;
}
}
if (found)
printf("Furthest reading %.2f mm away at %.2f degrees\n",
farDist, farAngle);
else
printf("No far reading found.\n");
printf("%d readings\n\n", i);
sick->unlockDevice();
ArUtil::sleep(100);
}
sick->lockDevice();
sick->stopRunning();
sick->disconnect();
sick->unlockDevice();
system("echo 'succeeded' >> results");
return 0;
}
int main(int argc, char **argv)
{
int ret; //Don't know what this variable is for
ArRobot robot;// Robot object
ArSick sick; // Laser scanner
ArSerialConnection laserCon; // Scanner connection
ArSerialConnection con; // Robot connection
std::string str; // Standard output
// sonar, must be added to the robot
ArSonarDevice sonar;
// the actions we'll use to wander
// recover from stalls
ArActionStallRecover recover;
// react to bumpers
ArActionBumpers bumpers;
// limiter for close obstacles
ArActionLimiterForwards limiter("speed limiter near", 300, 600, 250, 1.1);
// limiter for far away obstacles
ArActionLimiterForwards limiterFar("speed limiter far", 300, 1100, 600, 1.1);
// limiter for the table sensors
ArActionLimiterTableSensor tableLimiter;
// actually move the robot
ArActionConstantVelocity constantVelocity("Constant Velocity", 400);
// turn the orbot if its slowed down
ArActionTurn turn;
// mandatory init
Aria::init();
// Parse all our args
ArSimpleConnector connector(&argc, argv);
connector.parseArgs();
if (argc > 1)
{
connector.logOptions();
exit(1);
}
// add the sonar to the robot
robot.addRangeDevice(&sonar);
// add the laser to the robot
robot.addRangeDevice(&sick);
// NOTE: HARDCODED USB PORT!
// Attempt to open hard-coded USB to robot
if ((ret = con.open("/dev/ttyUSB2")) != 0){
// If connection fails, exit
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
Aria::shutdown();
return 1;
}
// set the robot to use the given connection
robot.setDeviceConnection(&con);
// do a blocking connect, if it fails exit
if (!robot.blockingConnect())
{
printf("Could not connect to robot... exiting\n");
Aria::shutdown();
return 1;
}
// turn on the motors, turn off amigobot sounds
//robot.comInt(ArCommands::SONAR, 0);
robot.comInt(ArCommands::SOUNDTOG, 0);
// start the robot running, true so that if we lose connection the run stops
robot.runAsync(true);
// Attempt to connect to SICK using another hard-coded USB connection
sick.setDeviceConnection(&laserCon);
if((ret=laserCon.open("/dev/ttyUSB3")) !=0) {
//If connection fails, shutdown
Aria::shutdown();
return 1;
}
//Configure the SICK
sick.configureShort(false,/*not using sim*/ArSick::BAUD38400,ArSick::DEGREES180,ArSick::INCREMENT_HALF);
//Run the sick
sick.runAsync();
// Presumably test to make sure that the connection is good
if(!sick.blockingConnect()){
printf("Could not get sick...exiting\n");
Aria::shutdown();
return 1;
}
printf("We are connected to the laser!");
/*
robot.lock();
robot.comInt(ArCommands::ENABLE, 1);
robot.unlock();
*/
int range [361] = {0};
int drange [360] = {0};
int i = 0;
int obj_range [2];
int old_range [360]={0};
clock_t now, prev;
while(1){
range [361] = {0};
drange [360] = {0};
i = 0;
obj_range[2];
std::list<ArSensorReading *> *readings;
std::list<ArSensorReading *>::iterator it;
sick.lockDevice();
readings=(list<ArSensorReading *,allocator<ArSensorReading *> > *)sick.getRawReadings();
if(NULL!=readings){
if ((readings->end() != readings->begin())){
for (it = readings->begin(); it!= readings->end(); it++){
// std::cout << (*it)->getRange()<<" ";
range[i] = ((*it)->getRange());
if(i){
drange[i-1] = range[i] - range[i-1];
printf("%f %i %i\r\n", (float)i/2.0, range[i], drange[i-1]);
}
i++;
}
int i = 0;
//detect the object range
while (i < 360) {
if (range[i]>Default_Distance + alpha) {
;
} else {
if (obj_range[0]=0)
obj_range[0]=i;
else
obj_range[1]=i;
}
}
if (!now)
prev=now;
now=clock();
duration=now-prev;
/******moving straight*******/
float speed = avg_speed(obj_range,old_range,range,(float)duration)
/*while(i < 360){
int r_edge = 0;
int l_edge = 0;
float obsticle_degree = 0;
if(drange[i] > D_DISTANCE){
r_edge = i;
while(drange[i] > -(D_DISTANCE)){
i++;
}
l_edge = i;
obsticle_degree = (r_edge + (l_edge - r_edge)/2.0)/2.0;
printf("\r\n object detected at %f\r\n", obsticle_degree);
}
std::cout<<std::endl;
}*/
}
else{
std::cout << "(readings->end() == readings -> begin())" << std::endl;
}
}
else{
int main(int argc, char **argv)
{
int ret;
std::string str;
ArSerialConnection con;
const std::list<ArSensorReading *> *readings;
std::list<ArSensorReading *>::const_iterator it;
int i;
ArGlobalFunctor failedConnectCB(&failedConnect);
Aria::init();
sick = new ArSick;
// open the connection, if it fails, exit
if ((ret = con.open("/dev/ttyS2")) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
Aria::shutdown();
return 1;
}
sick->configure(false);
sick->setDeviceConnection(&con);
sick->addFailedConnectCB(&failedConnectCB, ArListPos::FIRST);
sick->runAsync();
ArUtil::sleep(100);
sick->lockDevice();
sick->asyncConnect();
sick->unlockDevice();
while (!sick->isConnected())
ArUtil::sleep(100);
printf("Connected\n");
// while (sick->isConnected())
while (1)
{
//dist = sick->getCurrentBuffer().getClosestPolar(-90, 90, ArPose(0, 0), 30000, &angle);
sick->lockDevice();
if (!sick->getRunning() || !sick->isConnected())
{
break;
}
printf("\r");
readings = sick->getRawReadings();
if (readings != NULL)
{
for (i = 0, it = readings->begin(); it != readings->end(); it++, i++)
{
if (abs(i - readings->size()/2) < 3)
printf("(%.2f %d) ", (*it)->getSensorTh(), (*it)->getRange());
}
}
// switch the commenting in out of the fflush and the
// printf("\n"); if you want to print on the same line or have it
// scrolling
fflush(stdout);
//printf("\n");
sick->unlockDevice();
ArUtil::sleep(100);
}
sick->lockDevice();
sick->stopRunning();
sick->disconnect();
sick->unlockDevice();
return 0;
}
int main(int argc, char **argv)
{
int ret;
std::string str;
ArSerialConnection con;
ArSickPacket sick;
ArSickPacket *packet;
ArSickPacketReceiver receiver(&con);
ArTime start;
unsigned int value;
int numReadings;
ArTime lastReading;
ArTime packetTime;
start.setToNow();
// open the connection, if it fails, exit
if ((ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
Aria::shutdown();
return 1;
}
start.setToNow();
printf("Waiting for laser to power on\n");
sick.empty();
sick.uByteToBuf(0x10);
sick.finalizePacket();
con.write(sick.getBuf(), sick.getLength());
while (start.secSince() < 70 &&
((packet = receiver.receivePacket(100)) == NULL
|| (packet->getID() != 0x90)));
if (packet != NULL)
printf("Laser powered on\n");
else
exit(1);
printf("Changing baud\n");
sick.empty();
sick.byteToBuf(0x20);
sick.byteToBuf(0x40);
sick.finalizePacket();
con.write(sick.getBuf(), sick.getLength());
ArUtil::sleep(10);
if (!con.setBaud(38400))
{
printf("Could not set baud, exiting\n");
}
/*packet = receiver.receivePacket(100);
if (packet != NULL)
packet->log();
*/
sick.empty();
sick.uByteToBuf(0x3B);
sick.uByte2ToBuf(180);
sick.uByte2ToBuf(100);
sick.finalizePacket();
con.write(sick.getBuf(), sick.getLength());
packet = receiver.receivePacket(100);
if (packet != NULL)
packet->log();
sick.empty();
sick.byteToBuf(0x20);
sick.byteToBuf(0x24);
sick.finalizePacket();
con.write(sick.getBuf(), sick.getLength());
packet = receiver.receivePacket(100);
if (packet != NULL)
packet->log();
printf("Starting to report back from port, it took %ld ms to get here:\n",
start.mSecSince());
start.setToNow();
while (start.secSince() < 6)
{
packetTime.setToNow();
packet = receiver.receivePacket();
if (packet != NULL)
printf("####### %ld ms was how long the packet took\n", packetTime.mSecSince());
if (packet != NULL)
{
if (packet->getLength() < 10)
packet->log();
else if (packet->getID() == 0x90)
{
char strBuf[512];
packet->log();
//printf("%x\n", packet->bufToUByte());
packet->bufToStr(strBuf, 512);
printf("0x%x %s\n", packet->getID(), strBuf);
sick.empty();
sick.uByteToBuf(0x3B);
sick.uByte2ToBuf(180);
sick.uByte2ToBuf(100);
sick.finalizePacket();
con.write(sick.getBuf(), sick.getLength());
packet = receiver.receivePacket(100);
sick.empty();
sick.uByteToBuf(0x20);
sick.uByteToBuf(0x24);
sick.finalizePacket();
con.write(sick.getBuf(), sick.getLength());
}
else
{
value = packet->bufToUByte2();
numReadings = value & 0x3ff;
printf("%ld ms after last reading.\n", lastReading.mSecSince());
/*
printf("Reading number %d, complete %d, unit: %d %d:\n", value & 0x3ff, !(bool)(value & ArUtil::BIT13), (bool)(value & ArUtil::BIT14), (bool)(value & ArUtil::BIT15));
for (i = 0; i < numReadings; i++)
{
value = packet->bufToUByte2();
if (value & ArUtil::BIT13)
printf("D");
printf("%d ", value & 0x1fff);
}
printf("\n");
*/
lastReading.setToNow();
}
}
else
{
//printf("No packet\n");
}
}
}
int main(int argc, char **argv)
{
std::string str;
int ret;
ArTime start;
// 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);
// 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
printf("Telling the robot to go 300 mm for 5 seconds\n");
robot.lock();
robot.setVel(500);
robot.unlock();
start.setToNow();
while (1)
{
robot.lock();
if (start.mSecSince() > 5000)
{
robot.unlock();
break;
}
printf("Trans: %10g Rot: %10g\n", robot.getVel(), robot.getRotVel());
robot.unlock();
ArUtil::sleep(100);
}
printf("Telling the robot to turn at 50 deg/sec for 10 seconds\n");
robot.lock();
robot.setVel(0);
robot.setRotVel(50);
robot.unlock();
start.setToNow();
while (1)
{
robot.lock();
if (start.mSecSince() > 10000)
{
robot.unlock();
break;
}
printf("Trans: %10g Rot: %10g\n", robot.getVel(), robot.getRotVel());
robot.unlock();
ArUtil::sleep(100);
}
printf("Telling the robot to turn at 100 deg/sec for 10 seconds\n");
robot.lock();
robot.setVel(0);
robot.setRotVel(100);
robot.unlock();
start.setToNow();
while (1)
{
robot.lock();
if (start.mSecSince() > 10000)
{
robot.unlock();
break;
}
printf("Trans: %10g Rot: %10g\n", robot.getVel(), robot.getRotVel());
robot.unlock();
ArUtil::sleep(100);
}
printf("Done with tests, exiting\n");
robot.disconnect();
// shutdown and ge tout
Aria::shutdown();
return 0;
}