Added an scheduler object to the dynamixel manager to handle the periodic call...

Added an scheduler object to the dynamixel manager to handle the periodic call of the main function.
Chnaged the period resolution to ms.
Solved a bug in the set_period() function call period.

dynamixel_manager/Makefile

@@ -15,8 +15,9 @@ COMM_PATH = ../comm
 UTILS_PATH = ../utils
 DYN_BASE_PATH = ../dynamixel_base
 MEMORY_PATH = ../memory
+SCHEDULER_PATH = ../scheduler
 
-INCLUDE_DIRS = -I./include/ -I./include/modules -I$(COMM_PATH)/include -I$(UTILS_PATH)/include -I$(DYN_BASE_PATH)/include -I$(MEMORY_PATH)/include
+INCLUDE_DIRS = -I./include/ -I./include/modules -I$(COMM_PATH)/include -I$(UTILS_PATH)/include -I$(DYN_BASE_PATH)/include -I$(MEMORY_PATH)/include -I$(SCHEDULER_PATH)/include
 
 MACROS = -DMAX_DYN_MASTER_TX_BUFFER_LEN=256 -DMAX_DYN_MASTER_RX_BUFFER_LEN=256 -DDYN_MANAGER_MAX_NUM_MASTERS=4 -DDYN_MANAGER_MAX_NUM_MODULES=8 -DDYN_MANAGER_MAX_NUM_DEVICES=32 -DDYN_MANAGER_MAX_NUM_SINGLE_OP=16 -DDYN_MANAGER_MAX_NUM_SYNC_OP=4 -DDYN_MANAGER_MAX_NUM_BULK_OP=4 -DMODULE_MAX_NUM_MODELS=32 -DMM_MAX_NUM_MOTION_MODULES=8
 
@@ -37,6 +38,8 @@ SRC_DIR=./src/
 SRC=$(wildcard $(SRC_DIR)*.c)
 SRC_DIR_MODULES=./src/modules/
 SRC+=$(wildcard $(SRC_DIR_MODULES)*.c)
+SRC_DIR_SCHEDULER=$(SCHEDULER_PATH)/src
+SRC+=$(wildcard $(SRC_DIR_SCHEDULER)*.c)
 
 DYNAMIXEL_M4_FPU_OBJ_DIR=build/m4_fpu/
 DYNAMIXEL_M4_FPU_OBJS_TMP = $(notdir $(SRC:.c=.o))

dynamixel_manager/include/dyn_manager.h

@@ -8,6 +8,7 @@ extern "C" {
 #include "dyn_manager_registers.h"
 #include "dynamixel_master.h"
 #include "memory.h"
+#include "scheduler.h"
 
 #ifndef DYN_MANAGER_MAX_NUM_MASTERS
   #error "Please, specify the maximum number of masters with the DYN_MANAGER_MAX_NUM_MASTERS macro"
@@ -116,19 +117,19 @@ typedef struct{
   unsigned char num_devices;
   OP_HANDLE op_handles[DYN_MANAGER_MAX_NUM_OP];
   unsigned char num_ops;
-  unsigned short int period_us;
-  void (*init_timer)(void);
-  void (*set_period)(unsigned short int period_us);
+  unsigned char period_ms;
+  TScheduler *scheduler;
+  sched_channel_t sch_channel;
   TMemory *memory;
   TMemModule mem_module;
 }TDynManager;
 
 // public functions
-unsigned char dyn_manager_init(TDynManager *manager,TMemory *memory,unsigned short int eeprom_base_address,unsigned short int ram_base_address);
-void dyn_manager_set_period(TDynManager *manager,unsigned short int period_us);
-static inline unsigned short int dyn_manager_get_period(TDynManager *manager)
+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);
+void dyn_manager_set_period(TDynManager *manager,unsigned char period_ms);
+static inline unsigned char dyn_manager_get_period(TDynManager *manager)
 {
-  return manager->period_us;
+  return manager->period_ms;
 }
 void dyn_manager_scan(TDynManager *manager);
 void dyn_manager_add_master(TDynManager *manager,TDynamixelMaster *master);

dynamixel_manager/include/dyn_manager_registers.h

@@ -3,7 +3,7 @@
 
 #define RAM_DYN_MANAGER_LENGTH                2
 
-#define EEPROM_DYN_MANAGER_LENGTH             2
+#define EEPROM_DYN_MANAGER_LENGTH             1
 
 #define DYN_MANAGER_PERIOD                    0
 
@@ -11,8 +11,7 @@
 #define DYN_MANAGER_NUM_MASTERS               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, \
-                                                                                  (DEFAULT_PERIOD>>8)&0x00FF,base_address+DYN_MANAGER_PERIOD+1};
+unsigned short int name##_eeprom_data[] __attribute__ ((section (section_name)))={DEFAULT_PERIOD&0x00FF,base_address+DYN_MANAGER_PERIOD};
 
 #endif
 

dynamixel_manager/include/dyn_module.h

@@ -21,7 +21,7 @@ typedef struct TDynModule{
   unsigned char period_count;
   unsigned char enabled;
   void (*add_device)(void *module_data,unsigned char id,unsigned short int model);
-  void (*set_period)(void *module_data,unsigned short int period_us);
+  void (*set_period)(void *module_data,unsigned short int period_ms);
   void (*setup)(void *module_data);
   void (*pre_process)(void *module_data);
   void (*post_process)(void *module_data);

dynamixel_manager/include/modules/motion_module.h

@@ -10,7 +10,7 @@ extern "C" {
 typedef struct TMotionModule{
   TModules id;
   TMotionManager *manager;
-  void (*set_period)(void *module,unsigned short int period_us);
+  void (*set_period)(void *module,unsigned short int period_ms);
   void (*set_module)(void *module,unsigned char servo_id);  
   void (*pre_process)(void *module);
   void (*post_process)(void *module);

dynamixel_manager/src/dyn_manager.c

@@ -20,18 +20,16 @@ typedef enum {start_single_ops,wait_single_ops,start_sync_ops,wait_sync_ops,star
 // private functions
 void dyn_manager_write_cmd(TDynManager *module,unsigned short int address,unsigned short int length,unsigned char *data)
 {
-  unsigned short int uint16_value;
+  unsigned char uint8_value;
   unsigned char *data_ptr;
 
-  if(ram_in_window(DYN_MANAGER_PERIOD,2,address,length))
+  if(ram_in_window(DYN_MANAGER_PERIOD,1,address,length))
   {
-    uint16_value=dyn_manager_get_period(module);
-    data_ptr=(unsigned char *)&uint16_value;
+    uint8_value=dyn_manager_get_period(module);
+    data_ptr=(unsigned char *)&uint8_value;
     if(ram_in_range(DYN_MANAGER_PERIOD,address,length))
       data_ptr[0]=data[DYN_MANAGER_PERIOD-address];
-    if(ram_in_range(DYN_MANAGER_PERIOD+1,address,length))
-      data_ptr[1]=data[DYN_MANAGER_PERIOD+1-address];
-    dyn_manager_set_period(module,uint16_value);
+    dyn_manager_set_period(module,uint8_value);
   }
 }
 
@@ -211,7 +209,7 @@ void dyn_manager_delete_op_single_master(TDynManager *manager,unsigned char mast
 }
 
 // public functions
-unsigned char dyn_manager_init(TDynManager *manager,TMemory *memory,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 eeprom_base_address,unsigned short int ram_base_address)
 {
   unsigned char i,j,k;
 
@@ -281,9 +279,10 @@ unsigned char dyn_manager_init(TDynManager *manager,TMemory *memory,unsigned sho
     manager->op_handles[i].handle_index=i;
   }
   manager->num_ops=0x00;
-  /* initialize timer */
-  if(manager->init_timer!=0x00000000)
-    manager->init_timer();
+  /* initialize scheduler */
+  scheduler_set_channel(scheduler,sch_channel,(void(*)(void *))dyn_manager_loop,100,manager);
+  manager->scheduler=scheduler;
+  manager->sch_channel=sch_channel;
 
   /* initialize memory module */
   mem_module_init(&manager->mem_module);
@@ -303,20 +302,16 @@ unsigned char dyn_manager_init(TDynManager *manager,TMemory *memory,unsigned sho
   return 0x01;
 }
 
-void dyn_manager_set_period(TDynManager *manager,unsigned short int period_us)
+void dyn_manager_set_period(TDynManager *manager,unsigned char period_ms)
 {
   unsigned char i;
 
-  if(manager->set_period!=0x00000000)
-  {
-    manager->set_period(period_us);
-    for(i=0;i<DYN_MANAGER_MAX_NUM_MODULES;i++)
-      if(manager->modules[i]!=0x00000000 && manager->modules[i]->set_period!=0x00000000)
-         manager->modules[i]->set_period(manager->modules[i]->data,period_us);
-    manager->period_us=period_us;
-    manager->memory->data[DYN_MANAGER_PERIOD]=period_us%256;
-    manager->memory->data[DYN_MANAGER_PERIOD+1]=period_us/256;
-  }
+  scheduler_change_period(manager->scheduler,manager->sch_channel,period_ms);
+  for(i=0;i<DYN_MANAGER_MAX_NUM_MODULES;i++)
+    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[DYN_MANAGER_PERIOD]=period_ms;
 }
 
 void dyn_manager_scan(TDynManager *manager)
@@ -390,6 +385,7 @@ void dyn_manager_add_module(TDynManager *manager,TDynModule *module)
     manager->num_modules++;
     module->manager=manager;
     manager->memory->data[DYN_MANAGER_NUM_MODULES]++;
+    manager->modules[i]->set_period(manager->modules[i]->data,manager->period_ms*manager->modules[i]->period_count);
   }
 }
 

dynamixel_manager/src/dyn_module.c

@@ -13,6 +13,7 @@ unsigned char dyn_module_init(TDynModule *module)
   module->period_count=0x00;
   module->enabled=0x00;
   module->add_device=0x00000000;
+  module->set_period=0x00000000;
   module->setup=0x00000000;
   module->pre_process=0x00000000;
   module->post_process=0x00000000;
@@ -54,7 +55,7 @@ void dyn_module_add_device(TDynModule *module,unsigned char id, unsigned short i
   }
 }
 
-unsigned char dyn_moudle_device_is_assigned(TDynModule *module,unsigned char id)
+unsigned char dyn_module_device_is_assigned(TDynModule *module,unsigned char id)
 {
   if(id<DYN_MANAGER_MAX_NUM_DEVICES)
     return module->assigned_ids[id];
@@ -65,7 +66,8 @@ unsigned char dyn_moudle_device_is_assigned(TDynModule *module,unsigned char id)
 void dyn_module_set_period(TDynModule *module,unsigned char period)
 {
   if(period==0x00)
-    module->period_count=0x01;
-  else
-    module->period_count=period;
+    period=0x01;
+  module->period_count=period;
+  if(module->set_period!=0x00000000)
+    module->set_period(module->data,period*module->manager->period_ms);
 }

dynamixel_manager/src/modules/action.c

@@ -224,10 +224,10 @@ void action_finish(void)
   action_running=0x00;
 }
 
-void action_set_period(void *module,unsigned short int period_us)
+void action_set_period(void *module,unsigned short int period_ms)
 {
   // load the current period 
-  action_period=(period_us<<16)/1000000;
+  action_period=(period_ms<<16)/1000;
 }
 
 void action_set_module(void *module,unsigned char servo_id)

dynamixel_manager/src/modules/motion_manager.c

@@ -229,13 +229,13 @@ void mmanager_setup(TMotionManager *mmanager)
   mmanager->feedback_op=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 int period_us)
+void mmanager_set_period(TMotionManager *mmanager,unsigned short period_ms)
 {
   unsigned char i;
 
   for(i=0;i<MM_MAX_NUM_MOTION_MODULES;i++)
-    if(mmanager->modules[i]->set_period!=0x00000000)
-      mmanager->modules[i]->set_period(mmanager->modules[i]->data,period_us*mmanager->dyn_module.period_count);
+    if(mmanager->modules[i]!=0x00000000 && mmanager->modules[i]->set_period!=0x00000000)
+      mmanager->modules[i]->set_period(mmanager->modules[i]->data,period_ms);
 }
 
 void mmanager_pre_process(TMotionManager *mmanager)