#include "bioloid_robot_driver.h"
#include "bioloid_robot_exceptions.h"
#include "motion/include/action_id.h"
#include <iostream>

std::string robot_device="A8005idG";

int main(int argc, char *argv[])
{
  int num_servos;
  unsigned int present_servos;

  try{
    CBioloidRobot bioloid("Bioloid",robot_device,115200,0x01);

    num_servos=bioloid.mm_get_num_servos();
    present_servos=bioloid.mm_get_present_servos();
    std::cout << "Found " << num_servos << " servos " << std::endl;
    std::cout << "Present servos: " << std::hex << "0x" << present_servos << std::dec << std::endl;
    bioloid.action_load_page(walk_ready);
    bioloid.action_start();
    while(bioloid.action_is_page_running())
    {
      std::cout << "action running ... " << std::endl;
      usleep(100000);
    }
  }catch(CException &e){
    std::cout << e.what() << std::endl;
  }
}