Updated the mmeory map.

Implemented the functions of the motion manager and action interfaces.

src/darwin_registers.h

@@ -11,19 +11,23 @@
   #define MANAGER_RUNNING             0x40
   #define MANAGER_SCANNING            0x80
 #define MANAGER_NUM_DEVICES           131
+#define MANAGER_PRESENT_DEVICES       132
 
 #define MMANAGER_PERIOD               17
 #define MMANAGER_OFFSETS              18
-#define MMANAGER_NUM_MODELS           132
-#define MMANAGER_NUM_DEVICES          133
-#define MMANAGER_ENABLE               134
+#define MMANAGER_NUM_MODELS           136
+#define MMANAGER_NUM_DEVICES          137
+#define MMANAGER_PRESENT_SERVOS       138
+#define MMANAGER_ENABLE               142
   #define MMANAGER_EVEN_SER_EN        0x80
   #define MMANAGER_EVEN_SER_MOD       0x70
   #define MMANAGER_ODD_SER_EN         0x08
   #define MMANAGER_ODD_SER_MOD        0x07
 
-#define ACTION_NUM_MODULES            150
-#define ACTION_NUM_DEVICES            151
-#define ACTION_CONTROL                152
+#define ACTION_CONTROL                158
+  #define ACTION_START                0x01
+  #define ACTION_STOP                 0x02
+  #define ACTION_STATUS               0x80
+#define ACTION_PAGE_ID                159
 
 #endif

src/darwin_robot.cpp

@@ -471,7 +471,7 @@ unsigned int CDarwinRobot::managet_get_num_masters(void)
 
   if(this->robot_device==NULL)
     throw CDarwinRobotException(_HERE_,"Invalid robot device");
-  this->robot_device->read_byte_register(MANAGER_PERIOD,&num);
+  this->robot_device->read_byte_register(MANAGER_NUM_MASTERS,&num);
   return num;
 }
 
@@ -526,7 +526,33 @@ unsigned char CDarwinRobot::manager_get_num_devices(void)
   return num;
 }
 
+unsigned int CDarwinRobot::manager_get_present_devices(void)
+{
+  unsigned int present_devices;
+
+  if(this->robot_device!=NULL)
+  {
+    this->robot_device->read_registers(MANAGER_PRESENT_DEVICES,(unsigned char *)&present_devices,4);
+    return present_devices;
+  }
+  else
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
 /* motion manager interface */
+unsigned int CDarwinRobot::mm_get_present_servos(void)
+{
+  unsigned int present_servos;
+
+  if(this->robot_device!=NULL)
+  { 
+    this->robot_device->read_registers(MMANAGER_PRESENT_SERVOS,(unsigned char *)&present_servos,4);
+    return present_servos;
+  }
+  else
+    throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
 void CDarwinRobot::mm_set_period(double period_s)
 {
   unsigned char period;
@@ -697,19 +723,6 @@ mm_mode_t CDarwinRobot::mm_get_module(unsigned char servo_id)
 }
 
 /*
-unsigned int CDarwinRobot::mm_get_present_servos(void)
-{
-  unsigned int present_servos;
-
-  if(this->robot_device!=NULL)
-  {
-    this->robot_device->read_registers(DARWIN_MM_PRESENT_SERVOS1,(unsigned char *)&present_servos,4);
-    return present_servos;
-  }
-  else
-    throw CDarwinRobotException(_HERE_,"Invalid robot device");
-}
-
 void CDarwinRobot::mm_enable_balance(void)
 {
   if(this->robot_device!=NULL)
@@ -981,49 +994,24 @@ void CDarwinRobot::mm_get_balance_gains(double *knee,double *ankle_pitch,double
 }
 */
 // motion action interface
-unsigned int CDarwinRobot::action_get_num_models(void)
-{
-  unsigned char num;
-
-  if(this->robot_device==NULL)
-    throw CDarwinRobotException(_HERE_,"Invalid robot device");
-  this->robot_device->read_byte_register(MANAGER_NUM_MODULES,&num);
-  return num;
-}
-
-unsigned int CDarwinRobot::action_get_num_devices(void)
-{
-  unsigned char num;
-
-  if(this->robot_device==NULL)
-    throw CDarwinRobotException(_HERE_,"Invalid robot device");
-  this->robot_device->read_byte_register(MANAGER_NUM_MODULES,&num);
-  return num;
-}
-
-/*
 void CDarwinRobot::action_load_page(unsigned char page_id)
 {
-  if(this->robot_device!=NULL)
-    this->robot_device->write_byte_register(DARWIN_ACTION_PAGE,page_id);
-  else
+  if(this->robot_device==NULL)
     throw CDarwinRobotException(_HERE_,"Invalid robot device");
+  this->robot_device->write_byte_register(ACTION_PAGE_ID,page_id);
 }
 
 unsigned char CDarwinRobot::action_get_current_page(void)
 {
   unsigned char page_id;
 
-  if(this->robot_device!=NULL)
-  {
-    this->robot_device->read_byte_register(DARWIN_ACTION_PAGE,&page_id);
-
-    return page_id;
-  }
-  else
+  if(this->robot_device==NULL)
     throw CDarwinRobotException(_HERE_,"Invalid robot device");
+  this->robot_device->read_byte_register(ACTION_PAGE_ID,&page_id);
+  return page_id;
 }
 
+/*
 unsigned char CDarwinRobot::action_get_current_step(void)
 {
   unsigned char step_id;
@@ -1037,45 +1025,37 @@ unsigned char CDarwinRobot::action_get_current_step(void)
   else
     throw CDarwinRobotException(_HERE_,"Invalid robot device");
 }
+*/
 
 void CDarwinRobot::action_start(void)
 {
-  if(this->robot_device!=NULL)
-  {
-    this->action_status|=ACTION_START;
-    this->robot_device->write_byte_register(DARWIN_ACTION_CNTRL,this->action_status);
-  }
-  else
+  if(this->robot_device==NULL)
     throw CDarwinRobotException(_HERE_,"Invalid robot device");
+  this->action_status|=ACTION_START;
+  this->robot_device->write_byte_register(ACTION_CONTROL,this->action_status);
 }
 
 void CDarwinRobot::action_stop(void)
 {
-  if(this->robot_device!=NULL)
-  {
-    this->action_status|=ACTION_STOP;
-    this->robot_device->write_byte_register(DARWIN_ACTION_CNTRL,this->action_status);
-  }
-  else
+  if(this->robot_device==NULL)
     throw CDarwinRobotException(_HERE_,"Invalid robot device");
+  this->action_status|=ACTION_STOP;
+  this->robot_device->write_byte_register(ACTION_CONTROL,this->action_status);
 }
 
 bool CDarwinRobot::action_is_page_running(void)
 {
   unsigned char status;
 
-  if(this->robot_device!=NULL)
-  {
-    this->robot_device->read_byte_register(DARWIN_ACTION_CNTRL,&status);
-    if(status&ACTION_STATUS)
-      return true;
-    else
-      return false;
-  }
-  else
+  if(this->robot_device==NULL)
     throw CDarwinRobotException(_HERE_,"Invalid robot device");
+  this->robot_device->read_byte_register(ACTION_CONTROL,&status);
+  if(status&ACTION_STATUS)
+    return true;
+  else
+    return false;
 }
-*/
+
 /* walking interface */
 /*
 void CDarwinRobot::walk_set_x_offset(double offset_m)

src/darwin_robot.h

@@ -10,7 +10,7 @@
 
 extern const std::string servo_names[MAX_NUM_SERVOS];
 /* available motion modules */
-typedef enum {DARWIN_MM_NONE=0x00,DARWIN_MM_ACTION=0x01,DARWIN_MM_WALKING=0x02,DARWIN_MM_JOINTS=0x03,DARWIN_MM_HEAD=0x04,DARWIN_MM_GRIPPER=0x05} mm_mode_t;
+typedef enum {DARWIN_MM_NONE=0x07,DARWIN_MM_ACTION=0x00,DARWIN_MM_WALKING=0x01,DARWIN_MM_JOINTS=0x02,DARWIN_MM_HEAD=0x03,DARWIN_MM_GRIPPER=0x04} mm_mode_t;
 /* available grippers */
 typedef enum {LEFT_GRIPPER=0,RIGHT_GRIPPER=1} grippers_t;
 /* available leds */
@@ -111,7 +111,9 @@ class CDarwinRobot
     void manager_start_scan(void);
     bool manager_is_scanning(void);
     unsigned char manager_get_num_devices(void);
+    unsigned int manager_get_present_devices(void);
     // motion manager interface
+    unsigned int mm_get_present_servos(void);
     void mm_set_period(double period_s);
     double mm_get_period(void);
     void mm_enable_servo(unsigned char servo_id);
@@ -120,7 +122,6 @@ class CDarwinRobot
     void mm_assign_module(unsigned char servo_id, mm_mode_t mode);
     void mm_assign_module(std::string &servo,std::string &module);
     mm_mode_t mm_get_module(unsigned char servo_id);
-//    unsigned int mm_get_present_servos(void);
 //    void mm_enable_balance(void);
 //    void mm_disable_balance(void);
 //    bool mm_is_balance_enabled(void);
@@ -141,8 +142,6 @@ class CDarwinRobot
 //    void mm_set_balance_gains(double knee,double ankle_pitch,double hip_roll,double ankle_roll);
 //    void mm_get_balance_gains(double *knee,double *ankle_pitch,double *hip_roll,double *ankle_roll);
     // motion action interface
-    unsigned int action_get_num_models(void);
-    unsigned int action_get_num_devices(void);
     void action_load_page(unsigned char page_id);
     unsigned char action_get_current_page(void);
     unsigned char action_get_current_step(void);