int arrobot_connect()
{
AR_DEBUG_LOGINFO();
if(!init)
{
ArLog::log(ArLog::Terse, "arrobot_connect: Error: aria_init() must be called before arrobot_connect().");
return 0;
}
if(connected) return 1;
argParser->log();
ArLog::log(ArLog::Normal, "arrobot_connect: Connecting to robot...");
robot = new ArRobot;
robotConnector = new ArRobotConnector(argParser, robot);
robot->addDisconnectOnErrorCB(&disconnectCB);
if(!robotConnector->connectRobot())
{
ArLog::log(ArLog::Normal, "arrobot_connect: Could not connect to the robot. Check connections or set connection options as program arguments in init.");
/*if(argParser->checkHelpAndWarnUnparsed())
{
Aria::logOptions();
}
*/
arrobot_disconnect();
return 0;
}
if(!Aria::parseArgs() || !argParser->checkHelpAndWarnUnparsed())
{
Aria::logOptions();
arrobot_disconnect();
return 0;
}
robot->enableMotors();
robot->runAsync(true);
ArUtil::sleep(300);
ArLog::log(ArLog::Normal, "arrobot_connect: Connected to robot. ArRobot processing cycle is running in background thread.");
connected = true;
return 1;
}
int main(int argc, char **argv)
{
aria_init(argc, argv);
while(1)
{
if(!arrobot_connect())
return 1 ;
printf("radius=%.2f, length=%.2fmm, width=%.2fmm\n",
arrobot_radius(),
arrobot_length(),
arrobot_width()
);
arrobot_setvel(200);
arrobot_setrotvel(10);
for(int c = 0; c < 10; ++c)
{
arrobot_connect(); // should skip connection if already connected.
printf("x=%.1f y=%.1f th=%.1f vel=%.1f rotvel=%.1f left=%.1f right=%.1f stall=%d battv=%.1f\n",
arrobot_getx(),
arrobot_gety(),
arrobot_getth(),
arrobot_getvel(),
arrobot_getrotvel(),
arrobot_getleftvel(),
arrobot_getrightvel(),
arrobot_isstalled(),
arrobot_getbatteryvoltage()
);
printf("sonar");
for(int i = 0; i < arrobot_getnumsonar(); ++i)
{
printf(" %d=%f", i, arrobot_getsonarrange(i));
}
puts("");
sleep(1);
}
arrobot_disconnect();
puts("disconnected, reconnecting in 5 sec...");
sleep(5);
}
return 0;
}
void disconnected()
{
ArLog::log(ArLog::Terse, "ariac: robot disconnected.");
arrobot_disconnect();
}
