Solved a bug in the dynamixel manager: the period of the modules was not properly updated.

Solved a bug in the motion manager: write data is stored in RAM.
Problems with muliple calls to the setup function(): still not solved.

dynamixel_manager/src/dyn_manager.c

@@ -351,6 +351,7 @@ void dyn_manager_scan(TDynManager *manager)
   unsigned short int model;
 
   manager->present_devices=0x00000000;
+  manager->num_devices=0;
   // scan all dynamixel buses
   for(i=0;i<manager->num_masters;i++)
   {
@@ -939,7 +940,7 @@ void dyn_manager_loop(TDynManager *manager)
   TDynManagerSingleOp *single_op[DYN_MANAGER_MAX_NUM_MASTERS];
   TDynManagerSyncOp *sync_op[DYN_MANAGER_MAX_NUM_MASTERS];
   TDynManagerBulkOp *bulk_op[DYN_MANAGER_MAX_NUM_MASTERS];
-  static unsigned char modules_period_count[DYN_MANAGER_MAX_NUM_MASTERS]={0};
+  static unsigned char modules_period_count[DYN_MANAGER_MAX_NUM_MODULES]={0};
   static unsigned char single_op_period_count[DYN_MANAGER_MAX_NUM_SINGLE_OP]={0};
   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};
@@ -973,6 +974,7 @@ void dyn_manager_loop(TDynManager *manager)
       {
         if(manager->modules[i]->pre_process!=0x00000000)
           manager->modules[i]->pre_process(manager->modules[i]->data);
+        modules_period_count[i]=0;
       }
     }
   }

dynamixel_manager/src/dyn_module.c

@@ -30,7 +30,7 @@ unsigned char dyn_module_init(TDynModule *module,TMemory *memory,unsigned short
     module->assigned_ids[i]=0x00;
   module->num_assigned_ids=0x00;
   module->period_count=0x00;
-  module->enabled=0x00;
+  module->enabled=0x01;
   module->add_device=0x00000000;
   module->set_period=0x00000000;
   module->setup=0x00000000;

dynamixel_manager/src/modules/motion_manager.c

@@ -65,6 +65,7 @@ void mmanager_write_cmd(void *module,unsigned short int address,unsigned short i
       }
     }
   }
+  ram_write_table(manager->memory,address,length,data);
 }
 
 unsigned short int mmanager_angle_to_value(TMotionManager *mmanager,unsigned char servo_id,short int angle);
@@ -171,7 +172,11 @@ void mmanager_setup(TMotionManager *mmanager)
     }
   }
   if(num>0)
+  {
+    if(mmanager->motion_op[0]!=0x00000000)
+      dyn_manager_delete_op(mmanager->dyn_module.manager,mmanager->motion_op[0]);
     mmanager->motion_op[0]=dyn_manager_sync_op_new(mmanager->dyn_module.manager,DYN_MANAGER_WRITE,num,ids,start_address,4,(unsigned char * const*)&data);
+  }
   num=0;
   for(i=0;i<DYN_MANAGER_MAX_NUM_DEVICES;i++)
   {
@@ -191,7 +196,11 @@ void mmanager_setup(TMotionManager *mmanager)
     }
   }
   if(num>0)
+  {
+    if(mmanager->motion_op[1]!=0x00000000)
+      dyn_manager_delete_op(mmanager->dyn_module.manager,mmanager->motion_op[1]);
     mmanager->motion_op[1]=dyn_manager_sync_op_new(mmanager->dyn_module.manager,DYN_MANAGER_WRITE,num,ids,start_address,2,(unsigned char * const*)&data);
+  }
   /* create a feedback operation with the servos that support bulk read */
   num=0;
   for(i=0;i<DYN_MANAGER_MAX_NUM_DEVICES;i++)
@@ -209,7 +218,11 @@ void mmanager_setup(TMotionManager *mmanager)
     }
   }
   if(num>0)
+  {
+    if(mmanager->feedback_op[0]!=0x00000000)
+      dyn_manager_delete_op(mmanager->dyn_module.manager,mmanager->feedback_op[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)

dynamixel_manager/src/modules/motion_pages.c

@@ -42,4 +42,4 @@ inline unsigned char pages_get_slope(TPage *page,unsigned char servo_id)
   return 0x01<<(page->header.slope[servo_id]&0x0F);
 }
 
-TPage motion_pages[MAX_PAGES] __attribute__ ((section (".pages"))) __attribute__((weak)) ;
+TPage motion_pages[MAX_PAGES] __attribute__ ((section (".pages"))) __attribute__((weak));