Added a generic dynamixel manager module.

Added the implementation of a motion manager dynamixel module.
Added the action motion manager module.

dynamixel_manager/Makefile

@@ -15,7 +15,7 @@ COMM_PATH = ../comm
 UTILS_PATH = ../utils
 DYN_BASE_PATH = ../dynamixel_base
 
-INCLUDE_DIRS = -I./include/ -I$(COMM_PATH)/include -I$(UTILS_PATH)/include -I$(DYN_BASE_PATH)/include
+INCLUDE_DIRS = -I./include/ -I./include/modules -I$(COMM_PATH)/include -I$(UTILS_PATH)/include -I$(DYN_BASE_PATH)/include
 
 TCHAIN_PREFIX=arm-none-eabi-
 
@@ -32,6 +32,8 @@ DYNAMIXEL_OUT_M3 = ./lib/dynamixel_manager_m3.a
 
 SRC_DIR=./src/
 SRC=$(wildcard $(SRC_DIR)*.c)
+SRC_DIR_MODULES=./src/modules/
+SRC+=$(wildcard $(SRC_DIR_MODULES)*.c)
 
 DYNAMIXEL_M4_FPU_OBJ_DIR=build/m4_fpu/
 DYNAMIXEL_M4_FPU_OBJS_TMP = $(notdir $(SRC:.c=.o))
@@ -52,12 +54,20 @@ DYNAMIXEL_M3_OBJS = $(patsubst %,$(DYNAMIXEL_M3_OBJ_DIR)%,$(DYNAMIXEL_M3_OBJS_TM
 all: $(DYNAMIXEL_OUT_M4_FPU) $(DYNAMIXEL_OUT_M0) $(DYNAMIXEL_OUT_M0plus) $(DYNAMIXEL_OUT_M3)
 $(DYNAMIXEL_M4_FPU_OBJ_DIR)%.o: $(SRC_DIR)%.c
 	$(CC) -c $(CFLAGS) $(COMPILE_OPTS_M4_FPU) -o $@ $<
+$(DYNAMIXEL_M4_FPU_OBJ_DIR)%.o: $(SRC_DIR_MODULES)%.c
+	$(CC) -c $(CFLAGS) $(COMPILE_OPTS_M4_FPU) -o $@ $<
 $(DYNAMIXEL_M0_OBJ_DIR)%.o: $(SRC_DIR)%.c
 	$(CC) -c $(CFLAGS) $(COMPILE_OPTS_M0) -o $@ $<
+$(DYNAMIXEL_M0_OBJ_DIR)%.o: $(SRC_DIR_MODULES)%.c
+	$(CC) -c $(CFLAGS) $(COMPILE_OPTS_M0) -o $@ $<
 $(DYNAMIXEL_M0plus_OBJ_DIR)%.o: $(SRC_DIR)%.c
 	$(CC) -c $(CFLAGS) $(COMPILE_OPTS_M0plus) -o $@ $<
+$(DYNAMIXEL_M0plus_OBJ_DIR)%.o: $(SRC_DIR_MODULES)%.c
+	$(CC) -c $(CFLAGS) $(COMPILE_OPTS_M0plus) -o $@ $<
 $(DYNAMIXEL_M3_OBJ_DIR)%.o: $(SRC_DIR)%.c
 	$(CC) -c $(CFLAGS) $(COMPILE_OPTS_M3) -o $@ $<
+$(DYNAMIXEL_M3_OBJ_DIR)%.o: $(SRC_DIR_MODULES)%.c
+	$(CC) -c $(CFLAGS) $(COMPILE_OPTS_M3) -o $@ $<
 mkdir_build: 
 	mkdir -p build/m4_fpu
 	mkdir -p build/m0

dynamixel_manager/include/dyn_devices.h

@@ -19,9 +19,13 @@
 #define       SERVO_MX106     0x0140
 #define       SERVO_XL320     0x015E
 
-#define       IS_SERVO(model) (model==SERVO_DX113 || model==SERVO_DX116 || model==SERVO_DX117 || model==SERVO_AX12A \
-                               model==SERVO_AX12W || model==SERVO_AX18A || model==SERVO_RX10 || model==SERVO_MX12W \
-                               model==SERVO_MX28 || model==SERVO_RX24F || model==SERVO_RX28 || model==SERVO_RX64 \
+#define       IS_SERVO(model) (model==SERVO_DX113 || model==SERVO_DX116 || model==SERVO_DX117 || model==SERVO_AX12A || \
+                               model==SERVO_AX12W || model==SERVO_AX18A || model==SERVO_RX10 || model==SERVO_MX12W || \
+                               model==SERVO_MX28 || model==SERVO_RX24F || model==SERVO_RX28 || model==SERVO_RX64 || \
                                model==SERVO_MX64 || model==SERVO_EX106 || model==SERVO_MX106 || model==SERVO_XL320)
 
+#define       HAS_BULK_READ(model) (model==SERVO_MX12W || model==SERVO_MX28 || model==SERVO_MX64 || model==SERVO_MX106 || model==SERVO_XL320)
+#define       HAS_BULK_WRITE(model) (model==SERVO_XL320)
+#define       HAS_SYNC_READ(model) (model==SERVO_XL320)
+
 #endif

dynamixel_manager/include/dyn_manager.h

@@ -23,6 +23,8 @@
 #define    DYN_MANAGER_BULK                  0x40
 #define    DYN_MANAGER_NONE                  0x00
 
+#define    DYN_MANAGER_DEFAULT_PERIOD_US     7800
+
 typedef enum {single_rd_op=DYN_MANAGER_SINGLE|DYN_MANAGER_READ,
               single_wr_op=DYN_MANAGER_SINGLE|DYN_MANAGER_WRITE,
               sync_rd_op=DYN_MANAGER_SYNC|DYN_MANAGER_READ,
@@ -58,7 +60,7 @@ typedef struct{
   unsigned char ids[DYN_MANAGER_MAX_NUM_DEVICES];
   unsigned short int address;
   unsigned short int length;
-  TWriteData data[DYN_MANAGER_MAX_NUM_DEVICES];
+  unsigned char *data[DYN_MANAGER_MAX_NUM_DEVICES];
 }TDynManagerSyncOp;
 
 typedef struct{
@@ -67,7 +69,7 @@ typedef struct{
   unsigned char ids[DYN_MANAGER_MAX_NUM_DEVICES];
   unsigned short int address[DYN_MANAGER_MAX_NUM_DEVICES];
   unsigned short int length[DYN_MANAGER_MAX_NUM_DEVICES];
-  TWriteData data[DYN_MANAGER_MAX_NUM_DEVICES];
+  unsigned char *data[DYN_MANAGER_MAX_NUM_DEVICES];
 }TDynManagerBulkOp;
 
 typedef struct{
@@ -96,13 +98,20 @@ 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);
 }TDynManager;
 
 // public functions
 void dyn_manager_init(TDynManager *manager);
+void dyn_manager_set_period(TDynManager *manager,unsigned short int period_us);
+unsigned short int dyn_manager_get_period(TDynManager *manager);
 void dyn_manager_scan(TDynManager *manager);
 void dyn_manager_add_master(TDynManager *manager,TDynamixelMaster *master);
 unsigned char dyn_manager_get_num_masters(TDynManager *manager);
+TDynamixelMaster *dyn_manager_get_device_master(TDynManager *manager,unsigned char device_id);
+unsigned short int dyn_manager_get_device_model(TDynManager *manager,unsigned char device_id);
 void dyn_manager_add_module(TDynManager *manager,struct TDynModule *module);
 unsigned char dyn_manager_get_num_modules(TDynManager *manager);
 void dyn_manager_delete_op(TDynManager *manager,OP_HANDLE *op);
@@ -117,19 +126,19 @@ void dyn_manager_single_op_change_address(TDynManager *manager,OP_HANDLE *op,uns
 void dyn_manager_single_op_change_length(TDynManager *manager,OP_HANDLE *op,unsigned short int length);
 void dyn_manager_single_op_change_data(TDynManager *manager,OP_HANDLE *op,unsigned char *data);
 // sync operation functions
-OP_HANDLE *dyn_manager_sync_op_new(TDynManager *manager,unsigned char mode,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data);
-void dyn_manager_sync_op_add_devices(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids,TWriteData *data);
+OP_HANDLE *dyn_manager_sync_op_new(TDynManager *manager,unsigned char mode,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, unsigned char * const data[]);
+void dyn_manager_sync_op_add_devices(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids,unsigned char * const data[]);
 void dyn_manager_sync_op_remove_devices(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids);
 void dyn_manager_sync_op_change_address(TDynManager *manager,OP_HANDLE *op,unsigned short int address);
 void dyn_manager_sync_op_change_length(TDynManager *manager,OP_HANDLE *op,unsigned short int length);
-void dyn_manager_sync_op_change_data(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids,TWriteData *data);
+void dyn_manager_sync_op_change_data(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids,unsigned char * const data[]);
 // bulk operation functions
-OP_HANDLE *dyn_manager_bulk_op_new(TDynManager *manager,unsigned char mode,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data);
-void dyn_manager_bulk_op_add_devices(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length,TWriteData *data);
+OP_HANDLE *dyn_manager_bulk_op_new(TDynManager *manager,unsigned char mode,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, unsigned char * const data[]);
+void dyn_manager_bulk_op_add_devices(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length,unsigned char * const data[]);
 void dyn_manager_bulk_op_remove_devices(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids);
 void dyn_manager_bulk_op_change_address(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids,unsigned short int *address);
 void dyn_manager_bulk_op_change_length(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids,unsigned short int *length);
-void dyn_manager_bulk_op_change_data(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids,TWriteData *data);
+void dyn_manager_bulk_op_change_data(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids,unsigned char * const data[]);
 void dyn_manager_loop(TDynManager *manager);
 
 #endif 

dynamixel_manager/include/dyn_module.h

@@ -12,26 +12,26 @@ typedef struct TDynModule{
   unsigned char assigned_ids[DYN_MANAGER_MAX_NUM_DEVICES];
   unsigned char num_assigned_ids;
   unsigned char period_count;
-  void (*init)(void);
-  void (*add_device)(unsigned char id,unsigned short int model);
-  void (*setup)(void);
-  void (*pre_process)(void);
-  void (*post_process)(void);
+  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 (*setup)(void *module_data);
+  void (*pre_process)(void *module_data);
+  void (*post_process)(void *module_data);
+  void *data;
 }TDynModule;
 
 //public functions
 void dyn_module_init(TDynModule *module);
+inline void dyn_module_enable(TDynModule *module);
+inline void dyn_module_disable(TDynModule *module);
+inline unsigned char dyn_module_is_enabled(TDynModule *module);
 void dyn_module_add_model(TDynModule *module,unsigned short int model);
-unsigned char dyn_module_get_num_models(TDynModule *module);
+inline unsigned char dyn_module_get_num_models(TDynModule *module);
 void dyn_module_add_device(TDynModule *module,unsigned char id, unsigned short int model);
-unsigned char dyn_module_get_num_assigned_devices(TDynModule *module);
-unsigned char dyn_moudle_device_is_assigned(TDynModule *module,unsigned char id);
+inline unsigned char dyn_module_get_num_assigned_devices(TDynModule *module);
+unsigned char dyn_module_device_is_assigned(TDynModule *module,unsigned char id);
 void dyn_module_set_period(TDynModule *module,unsigned char period);
-unsigned char dyn_module_get_period(TDynModule *module);
-void dyn_module_set_init_fctn(TDynModule *module,void (*init)(void));
-void dyn_module_set_add_device_fctn(TDynModule *module,void (*add_device)(unsigned char id,unsigned short int model));
-void dyn_module_set_setup_fctn(TDynModule *module,void (*setup)(void));
-void dyn_module_set_pre_process_fctn(TDynModule *module,void (*pre_process)(void));
-void dyn_module_set_post_process_fctn(TDynModule *module,void (*post_process)(void));
+inline unsigned char dyn_module_get_period(TDynModule *module);
 
 #endif

dynamixel_manager/include/modules/action.h

+#ifndef _ACTION_H
+#define _ACTION_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "motion_pages.h"
+#include "motion_module.h"
+
+// public functions
+void action_init(void);
+TMotionModule *action_get_module(void);
+unsigned char action_load_page(unsigned char page_id);
+void action_start_page(void);
+void action_stop_page(void);
+unsigned char action_is_running(void);
+unsigned char action_get_current_page(void);
+unsigned char action_get_current_step(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

dynamixel_manager/include/modules/dyn_servos.h

+#ifndef _DYN_SERVOS_H
+#define _DYN_SERVOS_H
+
+// Servo register map
+typedef enum{
+  P_MODEL_NUMBER_L            = 0,
+  P_MODEL_NUMBER_H            = 1,
+  P_VERSION                   = 2,
+  P_ID                        = 3,
+  P_BAUD_RATE                 = 4,
+  P_RETURN_DELAY_TIME         = 5,
+  P_CW_ANGLE_LIMIT_L          = 6,
+  P_CW_ANGLE_LIMIT_H          = 7,
+  P_CCW_ANGLE_LIMIT_L         = 8,
+  P_CCW_ANGLE_LIMIT_H         = 9,
+  P_SYSTEM_DATA2              = 10,
+  P_HIGH_LIMIT_TEMPERATURE    = 11,
+  P_LOW_LIMIT_VOLTAGE         = 12,
+  P_HIGH_LIMIT_VOLTAGE        = 13,
+  P_MAX_TORQUE_L              = 14,
+  P_MAX_TORQUE_H              = 15,
+  P_RETURN_LEVEL              = 16,
+  P_ALARM_LED                 = 17,
+  P_ALARM_SHUTDOWN            = 18,
+  P_OPERATING_MODE            = 19,
+  P_LOW_CALIBRATION_L         = 20,
+  P_LOW_CALIBRATION_H         = 21,
+  P_HIGH_CALIBRATION_L        = 22,
+  P_HIGH_CALIBRATION_H        = 23,
+  P_TORQUE_ENABLE             = 24,
+  P_LED                       = 25,
+  P_CW_COMPLIANCE_MARGIN      = 26,
+  P_CCW_COMPLIANCE_MARGIN     = 27,
+  P_CW_COMPLIANCE_SLOPE       = 28,
+  P_CCW_COMPLIANCE_SLOPE      = 29,
+  P_D_GAIN                    = 26,
+  P_I_GAIN                    = 27,
+  P_P_GAIN                    = 28,
+  P_RESERVED                  = 29,
+  P_GOAL_POSITION_L           = 30,
+  P_GOAL_POSITION_H           = 31,
+  P_MOVING_SPEED_L            = 32,
+  P_MOVING_SPEED_H            = 33,
+  P_TORQUE_LIMIT_L            = 34,
+  P_TORQUE_LIMIT_H            = 35,
+  P_PRESENT_POSITION_L        = 36,
+  P_PRESENT_POSITION_H        = 37,
+  P_PRESENT_SPEED_L           = 38,
+  P_PRESENT_SPEED_H           = 39,
+  P_PRESENT_LOAD_L            = 40,
+  P_PRESENT_LOAD_H            = 41,
+  P_PRESENT_VOLTAGE           = 42,
+  P_PRESENT_TEMPERATURE       = 43,
+  P_REGISTERED_INSTRUCTION    = 44,
+  P_PAUSE_TIME                = 45,
+  P_MOVING                    = 46,
+  P_LOCK                      = 47,
+  P_PUNCH_L                   = 48,
+  P_PUNCH_H                   = 49,
+  P_RESERVED4                 = 50,
+  P_RESERVED5                 = 51,
+  P_POT_L                     = 52,
+  P_POT_H                     = 53,
+  P_PWM_OUT_L                 = 54,
+  P_PWM_OUT_H                 = 55,
+  P_P_ERROR_L                 = 56,
+  P_P_ERROR_H                 = 57,
+  P_I_ERROR_L                 = 58,
+  P_I_ERROR_H                 = 59,
+  P_D_ERROR_L                 = 60,
+  P_D_ERROR_H                 = 61,
+  P_P_ERROR_OUT_L             = 62,
+  P_P_ERROR_OUT_H             = 63,
+  P_I_ERROR_OUT_L             = 64,
+  P_I_ERROR_OUT_H             = 65,
+  P_D_ERROR_OUT_L             = 66,
+  P_D_ERROR_OUT_H             = 67}TDynServo;
+
+typedef enum{
+  XL_MODEL_NUMBER_L            = 0,
+  XL_MODEL_NUMBER_H            = 1,
+  XL_VERSION                   = 2,
+  XL_ID                        = 3,
+  XL_BAUD_RATE                 = 4,
+  XL_RETURN_DELAY_TIME         = 5,
+  XL_CW_ANGLE_LIMIT_L          = 6,
+  XL_CW_ANGLE_LIMIT_H          = 7,
+  XL_CCW_ANGLE_LIMIT_L         = 8,
+  XL_CCW_ANGLE_LIMIT_H         = 9,
+  XL_SYSTEM_DATA2              = 10,
+  XL_CONTROL_MODE              = 11,
+  XL_HIGH_LIMIT_TEMPERATURE    = 12,
+  XL_LOW_LIMIT_VOLTAGE         = 13,
+  XL_HIGH_LIMIT_VOLTAGE        = 14,
+  XL_MAX_TORQUE_L              = 15,
+  XL_MAX_TORQUE_H              = 16,
+  XL_RETURN_LEVEL              = 17,
+  XL_ALARM_SHUTDOWN            = 18,
+  XL_TORQUE_ENABLE             = 24,
+  XL_LED                       = 25,
+  XL_D_GAIN                    = 27,
+  XL_I_GAIN                    = 28,
+  XL_P_GAIN                    = 29,
+  XL_GOAL_POSITION_L           = 30,
+  XL_GOAL_POSITION_H           = 31,
+  XL_MOVING_SPEED_L            = 32,
+  XL_MOVING_SPEED_H            = 33,
+  XL_TORQUE_LIMIT_L            = 35,
+  XL_TORQUE_LIMIT_H            = 36,
+  XL_PRESENT_POSITION_L        = 37,
+  XL_PRESENT_POSITION_H        = 38,
+  XL_PRESENT_SPEED_L           = 39,
+  XL_PRESENT_SPEED_H           = 40,
+  XL_PRESENT_LOAD_L            = 41,
+  XL_PRESENT_LOAD_H            = 42,
+  XL_PRESENT_VOLTAGE           = 45,
+  XL_PRESENT_TEMPERATURE       = 46,
+  XL_REGISTERED_INSTRUCTION    = 47,
+  XL_MOVING                    = 49,
+  XL_HARDWARE_ERROR_STATUS     = 50,
+  XL_PUNCH_L                   = 51,
+  XL_PUNCH_H                   = 51}TXLServo;
+
+
+#endif

dynamixel_manager/include/modules/motion_manager.h

+#ifndef _MOTION_MANAGER_H
+#define _MOTION_MANAGER_H
+
+#include "dyn_module.h"
+
+#define MAX_NUM_MOTION_MODULES        8
+
+typedef enum {MM_NONE          = -1,
+              MM_ACTION        = 0,
+              MM_WALKING       = 1,
+              MM_JOINTS        = 2,
+              MM_HEAD          = 3} TModules;
+
+typedef struct{
+  unsigned short int model;
+  unsigned short int encoder_resolution;
+  unsigned char gear_ratio;
+  unsigned short int max_angle;
+  unsigned short int center_angle;
+  unsigned short int max_speed;
+  long long int cw_angle_limit;
+  long long int ccw_angle_limit;
+}TServoConfig;
+
+typedef struct{
+  unsigned char cw_compliance;
+  unsigned char ccw_compliance;
+  unsigned short int target_value;
+  long long int target_angle;
+  unsigned short int current_value;
+  long long int current_angle;
+  TModules module;
+  unsigned char enabled;
+  char offset;
+}TServoValues;
+
+struct TMotionModule;
+
+typedef struct {
+  TDynModule dyn_module;
+  struct TMotionModule *modules[MAX_NUM_MOTION_MODULES];
+  unsigned char num_modules;
+  TServoConfig servo_configs[DYN_MANAGER_MAX_NUM_DEVICES];
+  TServoValues servo_values[DYN_MANAGER_MAX_NUM_DEVICES];
+  OP_HANDLE *enable_op;
+  OP_HANDLE *motion_op;
+  OP_HANDLE *feedback_op;
+  unsigned char balance_enabled;
+  void (*balance)(short int offsets[DYN_MANAGER_MAX_NUM_DEVICES]);
+}TMotionManager;  
+
+void mmanager_init(TMotionManager *mmanager);
+TDynModule *mmanager_get_dyn_module(TMotionManager *mmanager);
+void mmanager_enable_balance(TMotionManager *mmanager);
+void mmanager_disable_balance(TMotionManager *mmanager);
+unsigned char mmanager_is_balance_enabled(TMotionManager *mmanager);
+void mmanager_set_module(TMotionManager *mmanager,unsigned char servo_id,TModules module);
+TModules mmanager_get_module(TMotionManager *mmanager,unsigned char servo_id);
+void mmanager_enable_servo(TMotionManager *mmanager,unsigned char servo_id);
+void mmanager_disable_servo(TMotionManager *mmanager,unsigned char servo_id);
+unsigned char mmanager_is_servo_enabled(TMotionManager *mmanager,unsigned char servo_id);
+void mmanager_set_offset(TMotionManager *mmanager,unsigned char servo_id,char offset);
+void mmanager_add_module(TMotionManager *mmanager,struct TMotionModule *module);
+
+#endif

dynamixel_manager/include/modules/motion_module.h

+#ifndef _MOTION_MODULE_H
+#define _MOTION_MODULE_H
+
+#include "motion_manager.h"
+
+typedef struct TMotionModule{
+  TModules id;
+  TMotionManager *manager;
+  void (*set_period)(void *module,unsigned short int period_us);
+  void (*set_module)(void *module,unsigned char servo_id);  
+  void (*pre_process)(void *module);
+  void (*post_process)(void *module);
+  void *data;
+}TMotionModule;
+
+void mmodule_init(TMotionModule *module);
+
+#endif 

dynamixel_manager/include/modules/motion_pages.h

+#ifndef _MOTION_PAGES_H
+#define _MOTION_PAGES_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define MAX_PAGES                     256 
+#define PAGE_MAX_NUM_SERVOS           31
+#define POSE_NUMBER_OF_POSES_PER_PAGE 7
+
+typedef struct // Header Structure (total 64unsigned char)
+{
+  unsigned char name[14];         // Name             0~13
+  unsigned char reserved1;        // Reserved1        14
+  unsigned char repeat;           // Repeat count     15
+  unsigned char schedule;         // schedule         16
+  unsigned char reserved2[3];     // reserved2        17~19
+  unsigned char stepnum;          // Number of step   20
+  unsigned char reserved3;        // reserved3        21
+  unsigned char speed;            // Speed            22
+  unsigned char reserved4;        // reserved4        23
+  unsigned char accel;            // Acceleration time 24
+  unsigned char next;             // Link to next     25
+  unsigned char exit;             // Link to exit     26
+  unsigned char reserved5[4];     // reserved5        27~30
+  unsigned char checksum;         // checksum         31
+  unsigned char slope[PAGE_MAX_NUM_SERVOS];        // CW/CCW compliance slope  32~62
+  unsigned char reserved6;        // reserved6        63
+}TPageHeader;
+
+typedef struct // Step Structure (total 64unsigned char)
+{
+  short int position[PAGE_MAX_NUM_SERVOS];    // Joint position   0~61
+  unsigned char pause;            // Pause time       62
+  unsigned char time;             // Time             63
+}TStep;
+
+typedef struct // Page Structure (total 512unsigned char)
+{
+  TPageHeader header;       // Page header  0~64
+  TStep steps[POSE_NUMBER_OF_POSES_PER_PAGE];           // Page step    65~511
+}TPage;
+
+extern TPage motion_pages[MAX_PAGES];
+
+// public functions
+void pages_get_page(unsigned char page_id,TPage *page);
+unsigned char pages_check_checksum(TPage *page);
+void pages_clear_page(TPage *page);
+void pages_copy_page(TPage *src,TPage *dst);
+inline unsigned char pages_get_slope(TPage *page,unsigned char servo_id);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

dynamixel_manager/src/dyn_module.c

@@ -11,11 +11,27 @@ void dyn_module_init(TDynModule *module)
     module->assigned_ids[i]=0x00;
   module->num_assigned_ids=0x00;
   module->period_count=0x00;
-  module->init=0x00000000;
+  module->enabled=0x00;
   module->add_device=0x00000000;
   module->setup=0x00000000;
   module->pre_process=0x00000000;
   module->post_process=0x00000000;
+  module->data=0x00000000;
+}
+
+inline void dyn_module_enable(TDynModule *module)
+{
+  module->enabled=0x01;
+}
+
+inline void dyn_module_disable(TDynModule *module)
+{
+  module->enabled=0x00;
+}
+
+inline unsigned char dyn_module_is_enabled(TDynModule *module)
+{
+  return module->enabled;
 }
 
 void dyn_module_add_model(TDynModule *module,unsigned short int model)
@@ -32,7 +48,7 @@ void dyn_module_add_model(TDynModule *module,unsigned short int model)
   }
 }
 
-unsigned char dyn_module_get_num_models(TDynModule *module)
+inline unsigned char dyn_module_get_num_models(TDynModule *module)
 {
   return module->num_models;
 }
@@ -45,17 +61,16 @@ void dyn_module_add_device(TDynModule *module,unsigned char id, unsigned short i
   {
     if(model==module->models[i])
     {
+      module->assigned_ids[id]=0x01;
+      module->num_assigned_ids++;
       if(module->add_device!=0x00000000)
-      {
-        module->assigned_ids[id]=0x01;
-        module->num_assigned_ids++;
-        module->add_device(id,model);
-      }
+        module->add_device(module->data,id,model);
+      break;
     }
   }
 }
 
-unsigned char dyn_module_get_num_assigned_devices(TDynModule *module)
+inline unsigned char dyn_module_get_num_assigned_devices(TDynModule *module)
 {
   return module->num_assigned_ids;
 }
@@ -76,33 +91,9 @@ void dyn_module_set_period(TDynModule *module,unsigned char period)
     module->period_count=period;
 }
 
-unsigned char dyn_module_get_period(TDynModule *module)
+inline unsigned char dyn_module_get_period(TDynModule *module)
 {
   return module->period_count;
 }
 
-void dyn_module_set_init_fctn(TDynModule *module,void (*init)(void))
-{
-  module->init=init;
-}
-
-void dyn_module_set_add_device_fctn(TDynModule *module,void (*add_device)(unsigned char id,unsigned short int model))
-{
-  module->add_device=add_device;
-}
-
-void dyn_module_set_setup_fctn(TDynModule *module,void (*setup)(void))
-{
-  module->setup=setup;
-}
-
-void dyn_module_set_pre_process_fctn(TDynModule *module,void (*pre_process)(void))
-{
-  module->pre_process=pre_process;
-}
-
-void dyn_module_set_post_process_fctn(TDynModule *module,void (*post_process)(void))
-{
-  module->post_process=post_process;
-}
 

dynamixel_manager/src/modules/motion_manager.c

+#include "motion_manager.h"
+#include "motion_module.h"
+#include "dyn_devices.h"
+#include "dyn_servos.h"
+
+/* private functions */
+inline unsigned short int mmanager_angle_to_value(TMotionManager *mmanager,unsigned char servo_id,short int angle)
+{
+  return ((angle+mmanager->servo_configs[servo_id].center_angle)*mmanager->servo_configs[servo_id].encoder_resolution)/mmanager->servo_configs[servo_id].max_angle;
+}
+
+inline short int mmanager_value_to_angle(TMotionManager *mmanager,unsigned char servo_id,unsigned short int value)
+{
+  return (((short int)((value*mmanager->servo_configs[servo_id].max_angle)/mmanager->servo_configs[servo_id].encoder_resolution))-mmanager->servo_configs[servo_id].center_angle);
+}
+
+void mmanager_compute_targets(TMotionManager *mmanager)
+{
+  short int offsets[DYN_MANAGER_MAX_NUM_DEVICES]={0};
+  unsigned char i;
+  short int angle;
+
+  // compute the balance offsets
+  if(mmanager->balance_enabled==0x01)
+    mmanager->balance(offsets);
+  // convert the angles to digital values
+  for(i=0;i<DYN_MANAGER_MAX_NUM_DEVICES;i++)
+  {
+    if(mmanager->servo_values[i].enabled)// servo is enabled and present
+    {
+      angle=(mmanager->servo_values[i].target_angle>>9)+offsets[i]+(mmanager->servo_values[i].offset<<3);
+      //>>16 from the action codification, <<7 from the manager codification
+      mmanager->servo_values[i].target_value=mmanager_angle_to_value(mmanager,i,angle);
+    }
+  }
+}
+
+void mmanager_compute_angles(TMotionManager *mmanager)
+{
+  unsigned char i;
+
+  // convert the digital values to angles
+  for(i=0;i<DYN_MANAGER_MAX_NUM_DEVICES;i++)
+  {
+    if(mmanager->servo_values[i].enabled)// servo is enabled and present
+      mmanager->servo_values[i].current_angle=mmanager_value_to_angle(mmanager,i,mmanager->servo_values[i].current_value);
+  }
+}
+
+void mmanager_add_device(TMotionManager *mmanager,unsigned char id,unsigned short int model)
+{
+  unsigned short int cw_value,ccw_value;
+
+  if(id<DYN_MANAGER_MAX_NUM_DEVICES)
+  {
+    switch(model)
+    {
+      case SERVO_DX113: mmanager->servo_configs[id].encoder_resolution=1023;
+                        mmanager->servo_configs[id].gear_ratio=193;
+                        mmanager->servo_configs[id].max_angle=300<<7;
+                        mmanager->servo_configs[id].center_angle=150<<7;
+                        mmanager->servo_configs[id].max_speed=324;
+                        break;
+      case SERVO_DX116: mmanager->servo_configs[id].encoder_resolution=1023;
+                        mmanager->servo_configs[id].gear_ratio=143;
+                        mmanager->servo_configs[id].max_angle=300<<7;
+                        mmanager->servo_configs[id].center_angle=150<<7;
+                        mmanager->servo_configs[id].max_speed=354;
+                        break;
+      case SERVO_DX117: mmanager->servo_configs[id].encoder_resolution=1023;
+                        mmanager->servo_configs[id].gear_ratio=193;
+                        mmanager->servo_configs[id].max_angle=300<<7;
+                        mmanager->servo_configs[id].center_angle=150<<7;
+                        mmanager->servo_configs[id].max_speed=510;
+                        break;
+      case SERVO_AX12A: mmanager->servo_configs[id].encoder_resolution=1023;
+                        mmanager->servo_configs[id].gear_ratio=254;
+                        mmanager->servo_configs[id].max_angle=300<<7;
+                        mmanager->servo_configs[id].center_angle=150<<7;
+                        mmanager->servo_configs[id].max_speed=354;
+                        break;
+      case SERVO_AX12W: mmanager->servo_configs[id].encoder_resolution=1023;
+                        mmanager->servo_configs[id].gear_ratio=32;
+                        mmanager->servo_configs[id].max_angle=300<<7;
+                        mmanager->servo_configs[id].center_angle=150<<7;
+                        mmanager->servo_configs[id].max_speed=2830;
+                        break;
+      case SERVO_AX18A: mmanager->servo_configs[id].encoder_resolution=1023;
+                        mmanager->servo_configs[id].gear_ratio=254;
+                        mmanager->servo_configs[id].max_angle=300<<7;
+                        mmanager->servo_configs[id].center_angle=150<<7;
+                        mmanager->servo_configs[id].max_speed=582;
+                        break;
+      case SERVO_RX10: mmanager->servo_configs[id].encoder_resolution=1023;
+                       mmanager->servo_configs[id].gear_ratio=193;
+                       mmanager->servo_configs[id].max_angle=300<<7;
+                       mmanager->servo_configs[id].center_angle=150<<7;
+                       mmanager->servo_configs[id].max_speed=324;
+                       break;
+      case SERVO_MX12W: mmanager->servo_configs[id].encoder_resolution=4095;
+                        mmanager->servo_configs[id].gear_ratio=32;
+                        mmanager->servo_configs[id].max_angle=360<<7;
+                        mmanager->servo_configs[id].center_angle=180<<7;
+                        mmanager->servo_configs[id].max_speed=2820;
+                        break;
+      case SERVO_MX28: mmanager->servo_configs[id].encoder_resolution=4095;
+                       mmanager->servo_configs[id].gear_ratio=193;
+                       mmanager->servo_configs[id].max_angle=360<<7;
+                       mmanager->servo_configs[id].center_angle=180<<7;
+                       mmanager->servo_configs[id].max_speed=330;
+                       break;
+      case SERVO_RX24F: mmanager->servo_configs[id].encoder_resolution=1023;
+                        mmanager->servo_configs[id].gear_ratio=193;
+                        mmanager->servo_configs[id].max_angle=300<<7;
+                        mmanager->servo_configs[id].center_angle=150<<7;
+                        mmanager->servo_configs[id].max_speed=756;
+                        break;
+      case SERVO_RX28: mmanager->servo_configs[id].encoder_resolution=1023;
+                       mmanager->servo_configs[id].gear_ratio=193;
+                       mmanager->servo_configs[id].max_angle=300<<7;
+                       mmanager->servo_configs[id].center_angle=150<<7;
+                       mmanager->servo_configs[id].max_speed=402;
+                       break;
+      case SERVO_RX64: mmanager->servo_configs[id].encoder_resolution=1023;
+                       mmanager->servo_configs[id].gear_ratio=200;
+                       mmanager->servo_configs[id].max_angle=300<<7;
+                       mmanager->servo_configs[id].center_angle=150<<7;
+                       mmanager->servo_configs[id].max_speed=294;
+                       break;
+      case SERVO_MX64: mmanager->servo_configs[id].encoder_resolution=4095;
+                       mmanager->servo_configs[id].gear_ratio=200;
+                       mmanager->servo_configs[id].max_angle=360<<7;
+                       mmanager->servo_configs[id].center_angle=180<<7;
+                       mmanager->servo_configs[id].max_speed=378;
+                       break;
+      case SERVO_EX106: mmanager->servo_configs[id].encoder_resolution=1023;
+                        mmanager->servo_configs[id].gear_ratio=184;
+                        mmanager->servo_configs[id].max_angle=250<<7;
+                        mmanager->servo_configs[id].center_angle=125<<7;
+                        mmanager->servo_configs[id].max_speed=414;
+                        break;
+      case SERVO_MX106: mmanager->servo_configs[id].encoder_resolution=4095;
+                        mmanager->servo_configs[id].gear_ratio=225;
+                        mmanager->servo_configs[id].max_angle=360<<7;
+                        mmanager->servo_configs[id].center_angle=180<<7;
+                        mmanager->servo_configs[id].max_speed=270;
+                        break;
+      case SERVO_XL320: mmanager->servo_configs[id].encoder_resolution=1023;
+                        mmanager->servo_configs[id].gear_ratio=238;
+                        mmanager->servo_configs[id].max_angle=300<<7;
+                        mmanager->servo_configs[id].center_angle=150<<7;
+                        mmanager->servo_configs[id].max_speed=684;
+                        break;
+      default: break;
+    }
+    // get the servo's current position
+    if(model==SERVO_XL320)
+    {
+      dyn_master_read_word(dyn_manager_get_device_master(mmanager->dyn_module.manager,id),id,XL_PRESENT_POSITION_L,&mmanager->servo_values[id].current_value);
+      dyn_master_read_word(dyn_manager_get_device_master(mmanager->dyn_module.manager,id),id,XL_CW_ANGLE_LIMIT_L,&cw_value);
+      dyn_master_read_word(dyn_manager_get_device_master(mmanager->dyn_module.manager,id),id,XL_CCW_ANGLE_LIMIT_L,&ccw_value);
+    }
+    else
+    {
+      dyn_master_read_word(dyn_manager_get_device_master(mmanager->dyn_module.manager,id),id,P_PRESENT_POSITION_L,&mmanager->servo_values[id].current_value);
+      dyn_master_read_word(dyn_manager_get_device_master(mmanager->dyn_module.manager,id),id,P_CW_ANGLE_LIMIT_L,&cw_value);
+      dyn_master_read_word(dyn_manager_get_device_master(mmanager->dyn_module.manager,id),id,P_CCW_ANGLE_LIMIT_L,&ccw_value);
+    }
+    mmanager->servo_values[id].target_value=mmanager->servo_values[id].current_value;
+    mmanager->servo_values[id].current_angle=(mmanager_value_to_angle(mmanager,id,mmanager->servo_values[id].current_value)<<9);
+    mmanager->servo_values[id].target_angle=mmanager->servo_values[id].current_angle;
+    // read the servo limits
+    mmanager->servo_configs[id].cw_angle_limit=(mmanager_value_to_angle(mmanager,id,cw_value)<<9);
+    mmanager->servo_configs[id].ccw_angle_limit=(mmanager_value_to_angle(mmanager,id,ccw_value)<<9);
+  }
+}
+
+void mmanager_setup(TMotionManager *mmanager)
+{
+  unsigned char * data[DYN_MANAGER_MAX_NUM_DEVICES];
+  unsigned char ids[DYN_MANAGER_MAX_NUM_DEVICES];
+  unsigned short int address[DYN_MANAGER_MAX_NUM_DEVICES];
+  unsigned short int length[DYN_MANAGER_MAX_NUM_DEVICES];
+  unsigned char i,num=0;
+
+  // initialize the motion operation
+  for(i=0;i<DYN_MANAGER_MAX_NUM_DEVICES;i++)
+  {
+    if(mmanager->dyn_module.assigned_ids[i]==0x01)
+    {
+      data[num]=&mmanager->servo_values[i].cw_compliance;
+      ids[num]=i;
+      num++;
+    }
+  }
+  mmanager->motion_op=dyn_manager_sync_op_new(mmanager->dyn_module.manager,DYN_MANAGER_WRITE,num,ids,P_CW_COMPLIANCE_SLOPE,4,(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++)
+  {
+    if(mmanager->dyn_module.assigned_ids[i]==0x01)
+    {
+      if(HAS_BULK_READ(dyn_manager_get_device_model(mmanager->dyn_module.manager,i)))
+      {
+        if(dyn_manager_get_device_model(mmanager->dyn_module.manager,i)==SERVO_XL320)
+          address[num]=XL_PRESENT_POSITION_L;
+        else
+          address[num]=P_PRESENT_POSITION_L;
+        data[num]=(unsigned char *)&mmanager->servo_values[i].current_value;
+        length[num]=2;
+        ids[num]=i;
+        num++;
+      }
+    }
+  }
+  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)
+{
+  unsigned char i;
+
+  for(i=0;i<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);
+}
+
+void mmanager_pre_process(TMotionManager *mmanager)
+{
+  unsigned char i;
+
+  for(i=0;i<MAX_NUM_MOTION_MODULES;i++)
+  {
+    if(mmanager->modules[i]!=0x00000000 && mmanager->modules[i]->pre_process!=0x00000000)
+      mmanager->modules[i]->pre_process(mmanager->modules[i]->data);
+  }
+  /* call the balance function */
+  mmanager_compute_targets(mmanager);
+}
+
+void mmanager_post_process(TMotionManager *mmanager)
+{
+  unsigned char i;
+
+  /* transform the current values to angles */
+  mmanager_compute_angles(mmanager);
+  for(i=0;i<MAX_NUM_MOTION_MODULES;i++)
+  {
+    if(mmanager->modules[i]!=0x00000000 && mmanager->modules[i]->post_process!=0x00000000)
+      mmanager->modules[i]->post_process(mmanager->modules[i]->data);
+  }
+}
+
+/* public functions */
+void mmanager_init(TMotionManager *mmanager)
+{
+  unsigned char i;
+
+  /* initialize the base module */
+  dyn_module_init(&mmanager->dyn_module);
+  /* add all known servo models */
+  dyn_module_add_model(&mmanager->dyn_module,SERVO_DX113);
+  dyn_module_add_model(&mmanager->dyn_module,SERVO_DX116);
+  dyn_module_add_model(&mmanager->dyn_module,SERVO_DX117);
+  dyn_module_add_model(&mmanager->dyn_module,SERVO_AX12A);
+  dyn_module_add_model(&mmanager->dyn_module,SERVO_AX12W);
+  dyn_module_add_model(&mmanager->dyn_module,SERVO_AX18A);
+  dyn_module_add_model(&mmanager->dyn_module,SERVO_RX10);
+  dyn_module_add_model(&mmanager->dyn_module,SERVO_MX12W);
+  dyn_module_add_model(&mmanager->dyn_module,SERVO_MX28);
+  dyn_module_add_model(&mmanager->dyn_module,SERVO_RX24F);
+  dyn_module_add_model(&mmanager->dyn_module,SERVO_RX28);
+  dyn_module_add_model(&mmanager->dyn_module,SERVO_RX64);
+  dyn_module_add_model(&mmanager->dyn_module,SERVO_MX64);
+  dyn_module_add_model(&mmanager->dyn_module,SERVO_EX106);
+  dyn_module_add_model(&mmanager->dyn_module,SERVO_MX106);
+  dyn_module_add_model(&mmanager->dyn_module,SERVO_XL320);
+  mmanager->dyn_module.data=mmanager;
+  mmanager->dyn_module.add_device=(void (*)(void *,unsigned char,unsigned short int))mmanager_add_device;
+  mmanager->dyn_module.setup=(void (*)(void *))mmanager_setup;
+  mmanager->dyn_module.set_period=(void (*)(void *,unsigned short int))mmanager_set_period;
+  mmanager->dyn_module.pre_process=(void (*)(void *))mmanager_pre_process;
+  mmanager->dyn_module.post_process=(void (*)(void *))mmanager_post_process;
+  /* initialize internal attributes */
+  mmanager->balance_enabled=0x00;
+  mmanager->num_modules=0x00;
+  /* initialize motion module base attributes */
+  for(i=0;i<MAX_NUM_MOTION_MODULES;i++)
+    mmanager->modules[i]=0x00000000;
+  for(i=0;i<DYN_MANAGER_MAX_NUM_DEVICES;i++)
+  {
+    /* servo configuration */
+    mmanager->servo_configs[i].model=0x0000;
+    mmanager->servo_configs[i].encoder_resolution=0x0000;
+    mmanager->servo_configs[i].gear_ratio=0x00;
+    mmanager->servo_configs[i].max_angle=0x0000;
+    mmanager->servo_configs[i].center_angle=0x0000;
+    mmanager->servo_configs[i].max_speed=0x0000;
+    mmanager->servo_configs[i].cw_angle_limit=0;
+    mmanager->servo_configs[i].ccw_angle_limit=0;
+    /* servo values */
+    mmanager->servo_values[i].cw_compliance=0x00;
+    mmanager->servo_values[i].ccw_compliance=0x00;
+    mmanager->servo_values[i].target_value=0x0000;
+    mmanager->servo_values[i].target_angle=0;
+    mmanager->servo_values[i].current_value=0x0000;
+    mmanager->servo_values[i].current_angle=0;
+    mmanager->servo_values[i].module=MM_NONE;
+    mmanager->servo_values[i].enabled=0x00;
+    mmanager->servo_values[i].offset=0x00;
+  }
+  mmanager->enable_op=0x00000000;
+  mmanager->motion_op=0x00000000;
+  mmanager->feedback_op=0x00000000;
+  mmanager->balance=0x00000000;
+}
+
+TDynModule *mmanager_get_dyn_module(TMotionManager *mmanager)
+{
+  return &mmanager->dyn_module;
+}
+
+void mmanager_enable_balance(TMotionManager *mmanager)
+{
+  if(mmanager->balance!=0x00000000)
+    mmanager->balance_enabled=0x01;
+}
+
+void mmanager_disable_balance(TMotionManager *mmanager)
+{
+  mmanager->balance_enabled=0x00;
+}
+
+unsigned char mmanager_is_balance_enabled(TMotionManager *mmanager)
+{
+  return mmanager->balance_enabled;
+}
+
+void mmanager_set_module(TMotionManager *mmanager,unsigned char servo_id,TModules module)
+{
+  if(servo_id<DYN_MANAGER_MAX_NUM_DEVICES)
+  {
+    if(module!=mmanager->servo_values[servo_id].module)
+    {
+      if(module!=MM_NONE && mmanager->modules[module]->set_module!=0x00000000)
+        mmanager->modules[module]->set_module(mmanager->modules[module]->data,servo_id);
+      mmanager->servo_values[servo_id].module=module;
+    }
+  }
+}
+
+TModules mmanager_get_module(TMotionManager *mmanager,unsigned char servo_id)
+{
+  if(servo_id<DYN_MANAGER_MAX_NUM_DEVICES)
+    return mmanager->servo_values[servo_id].module;
+  else
+    return MM_NONE;
+}
+
+void mmanager_enable_servo(TMotionManager *mmanager,unsigned char servo_id)
+{
+  unsigned char * const data=&(mmanager->servo_values[servo_id].enabled);
+
+  if(servo_id<DYN_MANAGER_MAX_NUM_DEVICES)
+  {
+    mmanager->servo_values[servo_id].enabled=0x01;
+    /* add an operation to enable the servo */
+    if(mmanager->enable_op==0x00000000 || mmanager->enable_op->op_type==no_op)
+    {
+      mmanager->enable_op=dyn_manager_sync_op_new(mmanager->dyn_module.manager,DYN_MANAGER_WRITE,1,&servo_id,P_TORQUE_ENABLE,1,&data);
+      dyn_manager_set_op_repetitions(mmanager->dyn_module.manager,mmanager->enable_op,1);
+    }
+    else
+      dyn_manager_sync_op_add_devices(mmanager->dyn_module.manager,mmanager->enable_op,1,&servo_id,&data);
+  }
+}
+
+void mmanager_disable_servo(TMotionManager *mmanager,unsigned char servo_id)
+{
+  unsigned char * const data=&(mmanager->servo_values[servo_id].enabled);
+
+  if(servo_id<DYN_MANAGER_MAX_NUM_DEVICES)
+  {
+    mmanager->servo_values[servo_id].enabled=0x00;
+    /* add an operation to enable the servo */
+    if(mmanager->enable_op==0x00000000 || mmanager->enable_op->op_type==no_op)
+    {
+      mmanager->enable_op=dyn_manager_sync_op_new(mmanager->dyn_module.manager,DYN_MANAGER_WRITE,1,&servo_id,P_TORQUE_ENABLE,1,&data);
+      dyn_manager_set_op_repetitions(mmanager->dyn_module.manager,mmanager->enable_op,1);
+    }
+    else
+      dyn_manager_sync_op_add_devices(mmanager->dyn_module.manager,mmanager->enable_op,1,&servo_id,&data);
+  }
+}
+
+unsigned char mmanager_is_servo_enabled(TMotionManager *mmanager,unsigned char servo_id)
+{
+  if(servo_id<DYN_MANAGER_MAX_NUM_DEVICES)
+    return mmanager->servo_values[servo_id].enabled;
+  else
+    return 0x00;
+}
+
+void mmanager_set_offset(TMotionManager *mmanager,unsigned char servo_id,char offset)
+{
+  if(servo_id<DYN_MANAGER_MAX_NUM_DEVICES)
+    mmanager->servo_values[servo_id].offset=offset;
+}
+
+void mmanager_add_module(TMotionManager *mmanager,TMotionModule *module)
+{
+  if(module->id!=MM_NONE)
+  {
+    if(mmanager->modules[module->id]==0x00000000)
+      mmanager->num_modules++;
+    mmanager->modules[module->id]=module;
+    module->manager=mmanager;
+  }
+}
+

dynamixel_manager/src/modules/motion_module.c

+#include "motion_module.h"
+
+void mmodule_init(TMotionModule *module)
+{
+  module->id=MM_NONE;
+  module->manager=0x00000000;
+  module->set_period=0x00000000;
+  module->set_module=0x00000000;
+  module->pre_process=0x00000000;
+  module->post_process=0x00000000;
+  module->data=0x00000000;
+}

dynamixel_manager/src/modules/motion_pages.c

+#include "motion_pages.h"
+
+void pages_get_page(unsigned char page_id,TPage *page)
+{
+  unsigned short int i=0;
+
+  for(i=0;i<sizeof(TPage);i++)
+    ((unsigned char *)page)[i]=((unsigned char *)&motion_pages[page_id])[i];
+}
+
+unsigned char pages_check_checksum(TPage *page)
+{
+  unsigned char checksum=0x00;
+  unsigned short int i=0;
+
+  for(i=0;i<sizeof(TPage);i++)
+    checksum+=((unsigned char *)page)[i];
+  if(checksum==0x00)
+    return 0x01;
+  else
+    return 0x00;
+}
+
+void pages_clear_page(TPage *page)
+{
+  unsigned short int i=0;
+
+  for(i=0;i<sizeof(TPage);i++)
+    ((unsigned char *)page)[i]=0x00;
+}
+
+void pages_copy_page(TPage *src,TPage *dst)
+{
+  unsigned short int i=0;
+
+  for(i=0;i<sizeof(TPage);i++)
+    ((unsigned char *)dst)[i]=((unsigned char *)src)[i];
+}
+
+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)) ;