#include "darwin_robot.h"
#include "darwin_robot_exceptions.h"

#include <iostream>

std::string robot_device="A603LOBS";

int main(int argc, char *argv[])
{
  int i=0,num_v1_servos,num_v2_servos;

  try{
    CDarwinRobot darwin("Darwin",robot_device,1000000,0xC0);

    darwin.mm_get_num_servos(&num_v1_servos,&num_v2_servos);
    std::cout << "Found " << num_v1_servos << " v1 servos and " << num_v2_servos << " v2 servos" << std::endl;
    // enable all servos and assign them to the action module
    darwin.mm_enable_power();
    for(i=1;i<=num_v1_servos;i++)
    {
      darwin.mm_enable_servo(i);
      darwin.mm_assign_module(i,DARWIN_MM_ACTION);
    }
    darwin.mm_start();
    // execute an action
    darwin.action_load_page(9);
    darwin.action_start();
    while(darwin.action_is_page_running())
      usleep(100000);
    darwin.mm_stop();
    darwin.mm_disable_power();
  }catch(CException &e){
    std::cout << e.what() << std::endl;
  }
}