예제 #1
0
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;
}
예제 #2
0
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;
}
예제 #3
0
void disconnected()
{
    ArLog::log(ArLog::Terse, "ariac: robot disconnected.");
    arrobot_disconnect();
}