Solved a bug in the store of the joint motion data (the RAM offset was wrong).

dynamixel_manager/include/modules/joint_motion_registers.h

@@ -9,8 +9,8 @@
   #define JOINT_MOTION_RUNNING                  0x80
 #define JOINT_MOTION_GROUP_SERVOS_OFFSET        NUM_JOINT_GROUPS 
 #define JOINT_MOTION_SERVO_ANGLES_OFFSET        (5*NUM_JOINT_GROUPS)
-#define JOINT_MOTION_SERVO_SPEEDS_OFFSET        (5*NUM_JOINT_GROUPS)+DYN_MANAGER_MAX_NUM_DEVICES*2
-#define JOINT_MOTION_SERVO_ACCELS_OFFSET        (5*NUM_JOINT_GROUPS)+DYN_MANAGER_MAX_NUM_DEVICES*3
+#define JOINT_MOTION_SERVO_SPEEDS_OFFSET        ((5*NUM_JOINT_GROUPS)+DYN_MANAGER_MAX_NUM_DEVICES*2)
+#define JOINT_MOTION_SERVO_ACCELS_OFFSET        ((5*NUM_JOINT_GROUPS)+DYN_MANAGER_MAX_NUM_DEVICES*3)
 
 #endif
 

dynamixel_manager/src/modules/joint_motion.c

@@ -19,7 +19,7 @@ void joint_motion_write_cmd(TJointMModule *module,unsigned short int address,uns
   ram_offset=address-module->ram_base_address;
   for(i=0;i<DYN_MANAGER_MAX_NUM_DEVICES*4;i++)
     if(ram_in_range(module->ram_base_address+JOINT_MOTION_SERVO_ANGLES_OFFSET+i,address,length))
-      module->memory->data[i]=data[JOINT_MOTION_SERVO_ANGLES_OFFSET+i-ram_offset];
+      module->memory->data[module->ram_base_address+JOINT_MOTION_SERVO_ANGLES_OFFSET+i]=data[JOINT_MOTION_SERVO_ANGLES_OFFSET+i-ram_offset];
   // group servos
   for(j=0;j<NUM_JOINT_GROUPS;j++)
   {