/*
Very simple robot movement:
   move foward two meters
*/

#include "Aria.h"

int main(int argc, char **argv)
{
  ArRobot robot; 		// robot
  ArSonarDevice sonar; 		// sonar

  /*** Initialize the System ***/
  printf("initializing aria...\n");
  Aria::init(); 		// mandatory init
  printf("initializing connection...\n");
  ArSimpleConnector connector(&argc, argv);
  if (!connector.parseArgs() || argc > 1)
  {
    connector.logOptions();
    exit(1);
  }
  printf("initializing sonar...\n");
  robot.addRangeDevice(&sonar);
  printf("connecting to robot...\n");
  if (!connector.connectRobot(&robot))
  {
    printf("Could not connect to robot... exiting\n");
    Aria::shutdown();
    return 1;
  }
  printf("turning on motors...\n");
  robot.comInt(ArCommands::ENABLE, 1);
  robot.comInt(ArCommands::SOUNDTOG, 0);
  printf("running the robot...\n");
  robot.runAsync(false);
  
  /**************************************/
  printf("moving 2000\n");
  robot.move(2000);
  while(1)
    { 
      ArUtil::sleep(1000);
      if (robot.isMoveDone(50))
         break;
    }
  printf("done moving\n");

  /*** Shut down ***/
  robot.disconnect();
  robot.stopRunning();
  Aria::shutdown();
  return 0;
}