Added initial support for the motion manager module.

src/bioloid_robot.cpp

@@ -43,6 +43,10 @@ CBioloidRobot::CBioloidRobot(const std::string &name,std::string &serial_dev,int
       this->clear_pending_interrupts();
       /* get the current zigbee status */ 
       this->robot_device->read_byte_register(BIOLOID_ZIGBEE_CNTRL,&this->zigbee_status);
+      /* get the current manager status */ 
+      this->robot_device->read_byte_register(BIOLOID_MM_CNTRL,&this->manager_status);
+      /* get the number of servos detected */ 
+      this->robot_device->read_byte_register(BIOLOID_MM_NUM_SERVOS,&this->manager_num_servos);
     }catch(CException &e){
       if(this->robot_device!=NULL)
         this->dyn_server->free_device(id);
@@ -388,149 +392,283 @@ bool CBioloidRobot::is_zigbee_enabled(void)
 }
 
 // motion manager interface
-/*void CBioloidRobot::mm_set_period(double period_ms)
+void CBioloidRobot::mm_set_period(double period_ms)
 {
   unsigned short period;
-  // internal time in 0|16 fixed point float format in seconds
-  period=(period_ms/1000.0)*65536;
-  this->robot_device->write_word_register(BIOLOID_MM_PERIOD_LOW,period);
+
+  if(this->robot_device!=NULL)
+  {
+    // internal time in 0|16 fixed point float format in seconds
+    period=(period_ms/1000.0)*65536;
+    this->robot_device->write_word_register(BIOLOID_MM_PERIOD_L,period);
+  }
+  else
+    throw CBioloidRobotException(_HERE_,"Invalid robot device");
+}
+
+double CBioloidRobot::mm_get_period(void)
+{
+  unsigned short period;
+
+  if(this->robot_device!=NULL)
+  {
+    this->robot_device->read_word_register(BIOLOID_MM_PERIOD_L,&period);
+    return (((double)period)*1000.0)/65536;
+  }
+  else
+    throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
 unsigned char CBioloidRobot::mm_get_num_servos(void)
 {
-  this->robot_device->read_byte_register(BIOLOID_MM_NUM_SERVOS,&this->num_servos);
+  if(this->robot_device!=NULL)
+  {
+    this->robot_device->read_byte_register(BIOLOID_MM_NUM_SERVOS,&this->manager_num_servos);
 
-  return this->num_servos;
+    return this->manager_num_servos;
+  }
+  else
+    throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
 void CBioloidRobot::mm_start(void)
 {
-  this->robot_device->write_byte_register(BIOLOID_MM_CONTROL,0x01);
+  if(this->robot_device!=NULL)
+  {
+    this->manager_status|=0x01;
+    this->robot_device->write_byte_register(BIOLOID_MM_CNTRL,this->manager_status);
+  }
+  else
+    throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
 void CBioloidRobot::mm_stop(void)
 {
-  this->robot_device->write_byte_register(BIOLOID_MM_CONTROL,0x00);
+  if(this->robot_device!=NULL)
+  {
+    this->manager_status&=0xFE;
+    this->robot_device->write_byte_register(BIOLOID_MM_CNTRL,this->manager_status);
+  }
+  else
+    throw CBioloidRobotException(_HERE_,"Invalid robot device");
+}
+
+bool CBioloidRobot::mm_is_running(void)
+{
+  unsigned char value;
+
+  if(this->robot_device!=NULL)
+  {
+    this->robot_device->read_byte_register(BIOLOID_MM_CNTRL,&value);
+    if(value&0x01)
+      return true;
+    else
+      return false;
+  }
+  else
+    throw CBioloidRobotException(_HERE_,"Invalid robot device");
+}
+
+void CBioloidRobot::mm_enable_balance(void)
+{
+  if(this->robot_device!=NULL)
+  {
+    this->manager_status|=0x02;
+    this->robot_device->write_byte_register(BIOLOID_MM_CNTRL,this->manager_status);
+  }
+  else
+    throw CBioloidRobotException(_HERE_,"Invalid robot device");
+}
+
+void CBioloidRobot::mm_disable_balance(void)
+{
+  if(this->robot_device!=NULL)
+  {
+    this->manager_status&=0xFE;
+    this->robot_device->write_byte_register(BIOLOID_MM_CNTRL,this->manager_status);
+  }
+  else
+    throw CBioloidRobotException(_HERE_,"Invalid robot device");
+
+}
+
+bool CBioloidRobot::mm_is_balance_enabled(void)
+{
+  unsigned char value;
+
+  if(this->robot_device!=NULL)
+  {
+    this->robot_device->read_byte_register(BIOLOID_MM_CNTRL,&value);
+    if(value&0x02)
+      return true;
+    else
+      return false;
+  }
+  else
+    throw CBioloidRobotException(_HERE_,"Invalid robot device");
+
 }
 
 void CBioloidRobot::mm_enable_power(void)
 {
-  this->robot_device->write_byte_register(BIOLOID_DXL_POWER,0x01);
+  if(this->robot_device!=NULL)
+  {
+    this->manager_status|=0x04;
+    this->robot_device->write_byte_register(BIOLOID_MM_CNTRL,this->manager_status);
+  }
+  else
+    throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
 void CBioloidRobot::mm_disable_power(void)
 {
-  this->robot_device->write_byte_register(BIOLOID_DXL_POWER,0x00);
+  if(this->robot_device!=NULL)
+  {
+    this->manager_status|=0xFB;
+    this->robot_device->write_byte_register(BIOLOID_MM_CNTRL,this->manager_status);
+  }
+  else
+    throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
 bool CBioloidRobot::mm_is_power_enabled(void)
 {
   unsigned char value;
 
-  this->robot_device->read_byte_register(BIOLOID_DXL_POWER,&value);
-  if(value==0x01)
-    return true;
+  if(this->robot_device!=NULL)
+  {
+    this->robot_device->read_byte_register(BIOLOID_MM_CNTRL,&value);
+    if(value&0x04)
+      return true;
+    else
+      return false;
+  }
   else
-    return false;
+    throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
 void CBioloidRobot::mm_enable_servo(unsigned char servo_id)
 {
   unsigned char value;
 
-  if(servo_id>MAX_NUM_SERVOS)
+  if(this->robot_device!=NULL)
   {
-    throw CBioloidRobotException(_HERE_,"Invalid servo identifier");
+    if(servo_id>MAX_NUM_SERVOS)
+    {
+      throw CBioloidRobotException(_HERE_,"Invalid servo identifier");
+    }
+    // get the current value
+    this->robot_device->read_byte_register(BIOLOID_MM_MODULE_EN0+servo_id/2,&value);
+    if(servo_id%2)// odd servo
+      value|=0x08;
+    else// even servo
+      value|=0x80;
+    this->robot_device->write_byte_register(BIOLOID_MM_MODULE_EN0+servo_id/2,value);
   }
-  // get the current value
-  this->robot_device->read_byte_register(BIOLOID_MM_MODULE_EN0+servo_id/2,&value);
-  if(servo_id%2)// odd servo
-    value|=0x08;
-  else// even servo
-    value|=0x80;
-  this->robot_device->write_byte_register(BIOLOID_MM_MODULE_EN0+servo_id/2,value);
+  else
+    throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
 void CBioloidRobot::mm_disable_servo(unsigned char servo_id)
 {
   unsigned char value;
 
-  if(servo_id>MAX_NUM_SERVOS)
+  if(this->robot_device!=NULL)
   {
-    throw CBioloidRobotException(_HERE_,"Invalid servo identifier");
+    if(servo_id>MAX_NUM_SERVOS)
+    {
+      throw CBioloidRobotException(_HERE_,"Invalid servo identifier");
+    }
+    // get the current value
+    this->robot_device->read_byte_register(BIOLOID_MM_MODULE_EN0+servo_id/2,&value);
+    if(servo_id%2)// odd servo
+      value&=0xF7;
+    else// even servo
+      value&=0x7F;
+    this->robot_device->write_byte_register(BIOLOID_MM_MODULE_EN0+servo_id/2,value);
   }
-  // get the current value
-  this->robot_device->read_byte_register(BIOLOID_MM_MODULE_EN0+servo_id/2,&value);
-  if(servo_id%2)// odd servo
-    value&=0xF7;
-  else// even servo
-    value&=0x7F;
-  this->robot_device->write_byte_register(BIOLOID_MM_MODULE_EN0+servo_id/2,value);
+  else
+    throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
 bool CBioloidRobot::mm_is_servo_enabled(unsigned char servo_id)
 {
   unsigned char value;
 
-  if(servo_id>MAX_NUM_SERVOS)
-  {
-    throw CBioloidRobotException(_HERE_,"Invalid servo identifier");
-  }
-  // get the current value
-  this->robot_device->read_byte_register(BIOLOID_MM_MODULE_EN0+servo_id/2,&value);
-  if(servo_id%2)// odd servo
-  {
-    if(value&0x01)
-      return true;
-    else
-      return false;
-  }
-  else// even servo
+  if(this->robot_device!=NULL)
   {
-    if(value&0x10)
-      return true;
-    else
-      return false;
+    if(servo_id>MAX_NUM_SERVOS)
+    {
+      throw CBioloidRobotException(_HERE_,"Invalid servo identifier");
+    }
+    // get the current value
+    this->robot_device->read_byte_register(BIOLOID_MM_MODULE_EN0+servo_id/2,&value);
+    if(servo_id%2)// odd servo
+    {
+      if(value&0x08)
+        return true;
+      else
+        return false;
+    }
+    else// even servo
+    {
+      if(value&0x80)
+        return true;
+      else
+        return false;
+    }
   }
+  else
+    throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
 void CBioloidRobot::mm_assign_module(unsigned char servo_id, mm_mode_t mode)
 {
   unsigned char value;
 
-  if(servo_id>MAX_NUM_SERVOS)
-  {
-    throw CBioloidRobotException(_HERE_,"Invalid servo identifier");
-  }
-  // get the current value
-  this->robot_device->read_byte_register(BIOLOID_MM_MODULE_EN0+servo_id/2,&value);
-  if(servo_id%2)// odd servo
-  {
-    value&=0xF8;
-    value|=(unsigned char)mode;
-  }
-  else// even servo
+  if(this->robot_device!=NULL)
   {
-    value&=0x8F;
-    value|=(((unsigned char)mode)<<4);
+    if(servo_id>MAX_NUM_SERVOS)
+    {
+      throw CBioloidRobotException(_HERE_,"Invalid servo identifier");
+    }
+    // get the current value
+    this->robot_device->read_byte_register(BIOLOID_MM_MODULE_EN0+servo_id/2,&value);
+    if(servo_id%2)// odd servo
+    {
+      value&=0xF8;
+      value|=(unsigned char)mode;
+    }
+    else// even servo
+    {
+      value&=0x8F;
+      value|=(((unsigned char)mode)<<4);
+    }
+    this->robot_device->write_byte_register(BIOLOID_MM_MODULE_EN0+servo_id/2,value);
   }
-  this->robot_device->write_byte_register(BIOLOID_MM_MODULE_EN0+servo_id/2,value);
+  else
+    throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
 mm_mode_t CBioloidRobot::mm_get_module(unsigned char servo_id)
 {
   unsigned char value;
 
-  if(servo_id>MAX_NUM_SERVOS)
+  if(this->robot_device!=NULL)
   {
-    throw CBioloidRobotException(_HERE_,"Invalid servo identifier");
+    if(servo_id>MAX_NUM_SERVOS)
+    {
+      throw CBioloidRobotException(_HERE_,"Invalid servo identifier");
+    }
+    // get the current value
+    this->robot_device->read_byte_register(BIOLOID_MM_MODULE_EN0+servo_id/2,&value);
+    if(servo_id%2)// odd servo
+      return (mm_mode_t)(value&0x07);
+    else
+      return (mm_mode_t)((value&0x70)>>4);
   }
-  // get the current value
-  this->robot_device->read_byte_register(BIOLOID_MM_MODULE_EN0+servo_id/2,&value);
-  if(servo_id%2)// odd servo
-    return (mm_mode_t)(value&0x07);
   else
-    return (mm_mode_t)((value&0x70)>>4);
+    throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
 std::vector<double> CBioloidRobot::mm_get_servo_angles(void)
@@ -539,12 +677,17 @@ std::vector<double> CBioloidRobot::mm_get_servo_angles(void)
   short int values[MAX_NUM_SERVOS]; 
   std::vector<double> angles(MAX_NUM_SERVOS);
 
-  this->robot_device->read_registers(BIOLOID_MM_SERVO0_CUR_POS_L,(unsigned char *)values,MAX_NUM_SERVOS*2);
+  if(this->robot_device!=NULL)
+  {
+    this->robot_device->read_registers(BIOLOID_MM_SERVO0_CUR_POS_L,(unsigned char *)values,MAX_NUM_SERVOS*2);
 
-  for(i=0;i<MAX_NUM_SERVOS;i++)
-    angles[i]=((double)values[i])/128.0;
+    for(i=0;i<MAX_NUM_SERVOS;i++)
+      angles[i]=((double)values[i])/128.0;
 
-  return angles;
+    return angles;
+  }
+  else
+    throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
 double CBioloidRobot::mm_get_servo_angle(unsigned char servo_id)
@@ -552,15 +695,20 @@ double CBioloidRobot::mm_get_servo_angle(unsigned char servo_id)
   unsigned short int value;
   double angle;
   
-  if(servo_id>MAX_NUM_SERVOS)
+  if(this->robot_device!=NULL)
   {
-    throw CBioloidRobotException(_HERE_,"Invalid servo identifier");
-  }
-  this->robot_device->read_word_register(BIOLOID_MM_SERVO0_CUR_POS_L+servo_id/2,&value);
-  angle=((double)value)/128.0;
+    if(servo_id>MAX_NUM_SERVOS)
+    {
+      throw CBioloidRobotException(_HERE_,"Invalid servo identifier");
+    }
+    this->robot_device->read_word_register(BIOLOID_MM_SERVO0_CUR_POS_L+servo_id/2,&value);
+    angle=((double)value)/128.0;
 
-  return angle;
-}*/
+    return angle;
+  }
+  else
+    throw CBioloidRobotException(_HERE_,"Invalid robot device");
+}
 
 // motion action interface
 /*void CBioloidRobot::action_load_page(unsigned char page_id)

src/bioloid_robot.h

@@ -10,6 +10,8 @@
 #include "threadserver.h"
 #include "eventserver.h"
 
+#define MAX_NUM_SERVOS        31
+
 /* available motion modules */
 typedef enum {BIOLOD_MM_NONE=0x00,BIOLOID_MM_ACTION=0x01,BIOLOID_MM_WALKING=0x02,BIOLOID_MM_JOINTS=0x03} mm_mode_t;
 /* available leds */
@@ -47,6 +49,9 @@ class CBioloidRobot
     CFunctor *mode_pb;
     /* zigbee status */
     unsigned char zigbee_status;
+    /* motion manager variables */
+    unsigned char manager_status;
+    unsigned char manager_num_servos;
   protected:
     static void *ext_int_thread(void *param);
     void clear_pending_interrupts(void);
@@ -133,10 +138,15 @@ class CBioloidRobot
     void zigbee_disable(void);
     bool is_zigbee_enabled(void);
     // motion manager interface
-/*    void mm_set_period(double period_ms);
+    void mm_set_period(double period_ms);
+    double mm_get_period(void);
     unsigned char mm_get_num_servos(void);
     void mm_start(void);
     void mm_stop(void);
+    bool mm_is_running(void);
+    void mm_enable_balance(void);
+    void mm_disable_balance(void);
+    bool mm_is_balance_enabled(void);
     void mm_enable_power(void);
     void mm_disable_power(void);
     bool mm_is_power_enabled(void);
@@ -146,7 +156,7 @@ class CBioloidRobot
     void mm_assign_module(unsigned char servo_id, mm_mode_t mode);
     mm_mode_t mm_get_module(unsigned char servo_id);
     std::vector<double> mm_get_servo_angles(void);
-    double mm_get_servo_angle(unsigned char servo_id);*/
+    double mm_get_servo_angle(unsigned char servo_id);
     // motion action interface
 /*    void action_load_page(unsigned char page_id);
     unsigned char action_get_current_page(void);

src/examples/bioloid_robot_test.cpp

@@ -75,9 +75,10 @@ int main(int argc, char *argv[])
       std::cout << "Channel 6: " << tina.adc_get_value(ADC_CH6) << std::endl;
       usleep(100000);
     }*/
-    tina.zigbee_enable_power();
-    sleep(10);
-    tina.zigbee_disable_power();
+/*    tina.zigbee_enable_power();
+    sleep(3);
+    tina.zigbee_disable_power();*/
+    std::cout << "Number of servos: " << (int)tina.mm_get_num_servos() << std::endl;
   }catch(CException &e){
     std::cout << e.what() << std::endl;
   }