Added dynamixel slave interface registers to all modules.

Added support ot perform a scan on demand.

dynamixel_manager/include/dyn_manager.h

@@ -122,6 +122,8 @@ typedef struct{
   sched_channel_t sch_channel;
   TMemory *memory;
   TMemModule mem_module;
+  unsigned char running;
+  unsigned char do_scan;
 }TDynManager;
 
 // public functions
@@ -131,6 +133,8 @@ static inline unsigned char dyn_manager_get_period(TDynManager *manager)
 {
   return manager->period_ms;
 }
+void dyn_manager_start(TDynManager *manager);
+void dyn_manager_stop(TDynManager *manager);
 void dyn_manager_scan(TDynManager *manager);
 void dyn_manager_add_master(TDynManager *manager,TDynamixelMaster *master);
 static inline unsigned char dyn_manager_get_num_masters(TDynManager *manager)

dynamixel_manager/include/dyn_manager_registers.h

 #ifndef _DYN_MANAGER_REGISTERS_H
 #define _DYN_MANAGER_REGISTERS_H
 
-#define RAM_DYN_MANAGER_LENGTH                2
+#define RAM_DYN_MANAGER_LENGTH                4
+
+#define DYN_MANAGER_NUM_MODULES_OFFSET        0
+#define DYN_MANAGER_NUM_MASTERS_OFFSET        1
+#define DYN_MANAGER_CONTROL_OFFSET            2 //   bit 7  |  bit 6  | bit 5 | bit 4 | bit 3 |    bit 2   | bit 1 | bit 0
+                                                // scanning | running |       |       |       | start scan | stop  | start 
+#define DYN_MANAGER_START                     0x01
+#define DYN_MANAGER_STOP                      0x02
+#define DYN_MANAGER_START_SCAN                0x04
+#define DYN_MANAGER_RUNNING                   0x40
+#define DYN_MANAGER_SCANNING                  0x80
+
+#define DYN_MANAGER_NUM_DEVICES_OFFSET        3
 
 #define EEPROM_DYN_MANAGER_LENGTH             1
 
 #define DYN_MANAGER_PERIOD_OFFSET             0
 
-#define DYN_MANAGER_NUM_MODULES_OFFSET        0
-#define DYN_MANAGER_NUM_MASTERS_OFFSET        1
-
 #define dyn_manager_eeprom_data(name,section_name,base_address,DEFAULT_PERIOD) \
-unsigned short int name##_eeprom_data[] __attribute__ ((section (section_name)))={DEFAULT_PERIOD&0x00FF,base_address+DYN_MANAGER_PERIOD};
+unsigned short int name##_eeprom_data[] __attribute__ ((section (section_name))) __attribute__ ((aligned (4)))={ \
+  DEFAULT_PERIOD&0x00FF,base_address+DYN_MANAGER_PERIOD_OFFSET \
+};
 
 #endif
 

dynamixel_manager/include/dyn_module_registers.h

@@ -3,15 +3,17 @@
 
 #define RAM_DYN_MODULE_LENGTH                2
 
+#define DYN_MODULE_NUM_MODELS_OFFSET         0
+#define DYN_MODULE_NUM_DEVICES_OFFSET        1
+
 #define EEPROM_DYN_MODULE_LENGTH             1
 
 #define DYN_MODULE_PERIOD_OFFSET             0
 
-#define DYN_MODULE_NUM_MODELS_OFFSET         0
-#define DYN_MODULE_NUM_DEVICES_OFFSET        1
-
 #define dyn_module_eeprom_data(name,section_name,base_address,DEFAULT_PERIOD) \
-unsigned short int name##_eeprom_data[] __attribute__ ((section (section_name)))={DEFAULT_PERIOD,base_address+DYN_MODULE_PERIOD};
+unsigned short int name##_eeprom_data[] __attribute__ ((section (section_name))) __attribute__ ((aligned (4)))={ \
+  DEFAULT_PERIOD,base_address+DYN_MODULE_PERIOD_OFFSET \
+};
 
 #endif
 

dynamixel_manager/include/modules/motion_manager.h

@@ -7,6 +7,7 @@ extern "C" {
 
 #include "dyn_module.h"
 #include "dyn_servos.h"
+#include "motion_manager_registers.h"
 
 #ifndef MM_MAX_NUM_MOTION_MODULES
   #error "Please, specify the maximum number of motion modules with the MM_MAX_NUM_MOTION_MODULES macro"

dynamixel_manager/include/modules/motion_manager_registers.h

@@ -6,14 +6,20 @@
 #define RAM_MM_LENGTH                        (DYN_MANAGER_MAX_NUM_DEVICES/2)
 
 #define MM_ENABLE_MODULE_OFFSET              0
+  #define MM_EVEN_SER_EN                     0x80
+  #define MM_EVEN_SER_MOD                    0x70
+  #define MM_ODD_SER_EN                      0x08
+  #define MM_ODD_SER_MOD                     0x07
 
 #define EEPROM_MM_LENGTH                     (DYN_MANAGER_MAX_NUM_DEVICES)
 
 #define MM_SERVO_OFFSET_OFFSET               0
 
-#define dyn_mm_eeprom_data(name,section_name,base_address,DEFAULT_PERIOD,initial_offset) \
-dyn_module_eeprom_data(name,section_name,base_address,DEFAULT_PERIOD) \
-unsigned short int name##_eeprom_data[] __attribute__ ((section (section_name)))={initial_offset};
+#define dyn_mm_eeprom_data(name,section_name,base_address,DEFAULT_PERIOD,...) \
+dyn_module_eeprom_data(name##_module,section_name,base_address,DEFAULT_PERIOD) \
+unsigned short int name##_eeprom_data[] __attribute__ ((section (section_name))) __attribute__ ((aligned (4)))={ \
+  __VA_ARGS__ \
+};
 
 #endif
 

dynamixel_manager/src/dyn_manager.c

@@ -18,14 +18,32 @@ extern unsigned char dyn_master_is_bulk_read_done(TDynamixelMaster *master,unsig
 typedef enum {start_single_ops,wait_single_ops,start_sync_ops,wait_sync_ops,start_bulk_ops,wait_bulk_ops,ops_done} dyn_manager_states_t;
 
 // private functions
+void dyn_manager_start_scan(TDynManager *manager)
+{
+  if(manager->running==0x00)
+    dyn_manager_start(manager);
+  manager->do_scan=0x01;
+  manager->memory->data[manager->ram_base_address+DYN_MANAGER_CONTROL_OFFSET]|=DYN_MANAGER_SCANNING;
+}
+
 void dyn_manager_write_cmd(TDynManager *module,unsigned short int address,unsigned short int length,unsigned char *data)
 {
-  unsigned short int eeprom_offset;
+  unsigned short int eeprom_offset,ram_offset;
 
   eeprom_offset=address-module->eeprom_base_address;
   if(ram_in_window(module->eeprom_base_address+DYN_MANAGER_PERIOD_OFFSET,1,address,length))
     dyn_manager_set_period(module,data[DYN_MANAGER_PERIOD_OFFSET-eeprom_offset]);
-  ram_write_table(module->memory,address,length,data);
+
+  ram_offset=address-module->ram_base_address;
+  if(ram_in_range(module->ram_base_address+DYN_MANAGER_CONTROL_OFFSET,address,length))
+  {
+    if(data[DYN_MANAGER_CONTROL_OFFSET-ram_offset]&DYN_MANAGER_START)
+      dyn_manager_start(module);
+    if(data[DYN_MANAGER_CONTROL_OFFSET-ram_offset]&DYN_MANAGER_STOP)
+      dyn_manager_stop(module);
+    if(data[DYN_MANAGER_CONTROL_OFFSET-ram_offset]&DYN_MANAGER_START_SCAN)
+      dyn_manager_start_scan(module);
+  }
 }
 
 void dyn_manager_read_cmd(TDynManager *module,unsigned short int address,unsigned short int length,unsigned char *data)
@@ -204,7 +222,7 @@ void dyn_manager_delete_op_single_master(TDynManager *manager,unsigned char mast
 }
 
 // public functions
-unsigned char dyn_manager_init(TDynManager *manager,TMemory *memory,TScheduler *scheduler,sched_channel_t sch_channel,unsigned short int eeprom_base_address,unsigned short int ram_base_address)
+unsigned char dyn_manager_init(TDynManager *manager,TMemory *memory,TScheduler *scheduler,sched_channel_t sch_channel,unsigned short int ram_base_address,unsigned short int eeprom_base_address)
 {
   unsigned char i,j,k;
 
@@ -293,6 +311,8 @@ unsigned char dyn_manager_init(TDynManager *manager,TMemory *memory,TScheduler *
   manager->eeprom_base_address=eeprom_base_address;
   manager->ram_base_address=ram_base_address;
   manager->memory=memory;
+  manager->running=0x00;
+  manager->do_scan=0x00;
   
   return 0x01;
 }
@@ -306,6 +326,21 @@ void dyn_manager_set_period(TDynManager *manager,unsigned char period_ms)
     if(manager->modules[i]!=0x00000000 && manager->modules[i]->set_period!=0x00000000)
        manager->modules[i]->set_period(manager->modules[i]->data,period_ms*manager->modules[i]->period_count);
   manager->period_ms=period_ms;
+  manager->memory->data[manager->eeprom_base_address+DYN_MANAGER_PERIOD_OFFSET]=period_ms;
+}
+
+void dyn_manager_start(TDynManager *manager)
+{
+  scheduler_enable_channel(manager->scheduler,manager->sch_channel);
+  manager->running=0x01;
+  manager->memory->data[manager->ram_base_address+DYN_MANAGER_CONTROL_OFFSET]|=DYN_MANAGER_RUNNING;
+}
+
+void dyn_manager_stop(TDynManager *manager)
+{
+  scheduler_disable_channel(manager->scheduler,manager->sch_channel);
+  manager->running=0x00;
+  manager->memory->data[manager->ram_base_address+DYN_MANAGER_CONTROL_OFFSET]&=(~DYN_MANAGER_RUNNING);
 }
 
 void dyn_manager_scan(TDynManager *manager)
@@ -337,6 +372,8 @@ void dyn_manager_scan(TDynManager *manager)
       // else ignore device
     }
   }
+  manager->memory->data[manager->ram_base_address+DYN_MANAGER_NUM_DEVICES_OFFSET]=manager->num_devices;
+  manager->memory->data[manager->ram_base_address+DYN_MANAGER_CONTROL_OFFSET]&=(~DYN_MANAGER_SCANNING);
   // configure the operations
   for(i=0;i<manager->num_modules;i++)
     if(manager->modules[i]->setup!=0x00000000)
@@ -900,6 +937,16 @@ void dyn_manager_loop(TDynManager *manager)
   static unsigned char sync_op_period_count[DYN_MANAGER_MAX_NUM_SINGLE_OP]={0};
   static unsigned char bulk_op_period_count[DYN_MANAGER_MAX_NUM_SINGLE_OP]={0};
 
+  // do scan
+  if(manager->do_scan)
+  {
+    manager->do_scan=0x00;
+//    dyn_manager_stop(manager);
+    dyn_manager_scan(manager);
+//    dyn_manager_start(manager);
+    return;
+  }
+
   // initialize the remaining operations
   for(i=0;i<manager->num_masters;i++)
   {

dynamixel_manager/src/dyn_module.c

@@ -13,7 +13,7 @@ void dyn_module_write_cmd(TDynModule *module,unsigned short int address,unsigned
   ram_write_table(module->memory,address,length,data);
 }
 
-void dyn_module_read_cmd(TDynManager *module,unsigned short int address,unsigned short int length,unsigned char *data)
+void dyn_module_read_cmd(TDynModule *module,unsigned short int address,unsigned short int length,unsigned char *data)
 {
   ram_read_table(module->memory,address,length,data);
 }

dynamixel_manager/src/modules/motion_manager.c

 #include "motion_manager.h"
 #include "motion_manager_registers.h"
 #include "motion_module.h"
+#include "ram.h"
 
 /* private functions */
 void mmanager_read_register(TMotionManager *mmanager,unsigned char id,TDynReg *reg,unsigned int *value)
@@ -25,12 +26,45 @@ void mmanager_write_register(TMotionManager *mmanager,unsigned char id,TDynReg *
 
 void mmanager_read_cmd(void *module,unsigned short int address,unsigned short int length,unsigned char *data)
 {
+  TMotionManager *manager=(TMotionManager *)module;
 
+  ram_read_table(manager->memory,address,length,data);
 }
 
 void mmanager_write_cmd(void *module,unsigned short int address,unsigned short int length,unsigned char *data)
 {
+  TMotionManager *manager=(TMotionManager *)module;
+  unsigned short int eeprom_offset,ram_offset,i,j;
+  unsigned char byte_value,mod;
 
+  eeprom_offset=address-manager->eeprom_base_address;
+  if(ram_in_window(manager->eeprom_base_address+MM_SERVO_OFFSET_OFFSET,DYN_MANAGER_MAX_NUM_DEVICES,address,length))
+  {
+    for(i=0;i<DYN_MANAGER_MAX_NUM_DEVICES;i++)
+    {
+      if(ram_in_range(manager->eeprom_base_address+MM_SERVO_OFFSET_OFFSET+i,address,length))
+        mmanager_set_offset(manager,i,(signed char)(data[MM_SERVO_OFFSET_OFFSET-eeprom_offset+i]));
+    }
+  }
+  ram_offset=address-manager->ram_base_address;
+  if(ram_in_window(manager->ram_base_address+MM_ENABLE_MODULE_OFFSET,DYN_MANAGER_MAX_NUM_DEVICES/2,address,length))
+  {
+    for(i=0,j=0;i<DYN_MANAGER_MAX_NUM_DEVICES/2;i++,j+=2)
+    {
+      if(ram_in_range(manager->ram_base_address+MM_ENABLE_MODULE_OFFSET+i,address,length))
+      {
+        byte_value=data[MM_ENABLE_MODULE_OFFSET-ram_offset+i];
+        if(byte_value&MM_EVEN_SER_EN) mmanager_enable_servo(manager,j);
+        else mmanager_disable_servo(manager,j);
+        mod=(byte_value&MM_EVEN_SER_MOD)>>4;
+        mmanager_set_module(manager,j,(TModules)mod);
+        if(byte_value&MM_ODD_SER_EN) mmanager_enable_servo(manager,j+1);
+        else mmanager_disable_servo(manager,j+1);
+        mod=byte_value&MM_ODD_SER_MOD;
+        mmanager_set_module(manager,j+1,(TModules)mod);
+      }
+    }
+  }
 }
 
 unsigned short int mmanager_angle_to_value(TMotionManager *mmanager,unsigned char servo_id,short int angle);
@@ -124,7 +158,7 @@ void mmanager_setup(TMotionManager *mmanager)
         {
           first=0;
           if(mmanager->servo_configs[i]->pid==1)
-            start_address=mmanager->servo_configs[i]->registers[pos_pid_i].address;
+            start_address=mmanager->servo_configs[i]->registers[pos_pid_p].address;
           else
             start_address=mmanager->servo_configs[i]->registers[cw_comp_slope].address;
         }
@@ -169,7 +203,8 @@ void mmanager_setup(TMotionManager *mmanager)
       }
     }
   }
-  mmanager->feedback_op[0]=dyn_manager_bulk_op_new(mmanager->dyn_module.manager,DYN_MANAGER_READ,num,ids,address,length,(unsigned char * const*)data);
+  if(num>0)
+    mmanager->feedback_op[0]=dyn_manager_bulk_op_new(mmanager->dyn_module.manager,DYN_MANAGER_READ,num,ids,address,length,(unsigned char * const*)data);
 }
 
 void mmanager_set_period(TMotionManager *mmanager,unsigned short period_ms)
@@ -258,10 +293,10 @@ unsigned char mmanager_init(TMotionManager *mmanager,TMemory *memory,unsigned sh
   mmanager->mem_module.read_cmd=mmanager_read_cmd;
   if(!mem_module_add_ram_segment(&mmanager->mem_module,ram_base_address+RAM_DYN_MODULE_LENGTH,RAM_MM_LENGTH))
     return 0x00;
-  mmanager->ram_base_address=ram_base_address;
+  mmanager->ram_base_address=ram_base_address+RAM_DYN_MODULE_LENGTH;
   if(!mem_module_add_eeprom_segment(&mmanager->mem_module,eeprom_base_address+EEPROM_DYN_MODULE_LENGTH,EEPROM_MM_LENGTH))
     return 0x00;
-  mmanager->eeprom_base_address=eeprom_base_address;
+  mmanager->eeprom_base_address=eeprom_base_address+EEPROM_DYN_MODULE_LENGTH;
   if(!mem_add_module(memory,&mmanager->mem_module))
     return 0x00;
   mmanager->memory=memory;