diff --git a/dynamixel_manager/include/modules/dyn_servos.h b/dynamixel_manager/include/modules/dyn_servos.h
index 07d4e55521b0279fda66c9fe001c91c7623ce551..f3dc02d85a8e1f20bfe293e8d8e9bfc336db1db7 100755
--- a/dynamixel_manager/include/modules/dyn_servos.h
+++ b/dynamixel_manager/include/modules/dyn_servos.h
@@ -1,125 +1,107 @@
#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;
+#define NUM_REG 62
-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;
+typedef struct{
+ unsigned short int address;
+ unsigned char size;
+ unsigned char eeprom;
+}TDynReg;
+extern TDynReg ax_reg[NUM_REG];
+extern TDynReg mx_1_0_reg[NUM_REG];
+extern TDynReg mx_2_0_reg[NUM_REG];
+extern TDynReg xl_reg[NUM_REG];
+
+typedef enum {
+ // [Access] [Description]
+ // Info
+ model_number=0, // (R) Model Number
+ model_info, // (R)
+ firmware_version, // (R) Version of the Firmware
+ id, // (RW) ID of Dynamixel
+ baudrate, // (RW) Baud Rate of Dynamixel
+ return_delay_time, // (RW) Return Delay Time
+ second_id, // (RW)
+ protocol, // (RW)
+ return_level, // (RW) Status Return Level
+ // Configuration
+ drive_mode, // (RW) Drive mode
+ op_mode, // (RW) Joint or wheel mode
+ homming_offset, // (RW)
+ moving_threshold, // (RW)
+ min_angle_limit, // (RW) Clockwise Angle Limit
+ max_angle_limit, // (RW) Counterclockwise Angle Limit
+ temp_limit, // (RW) Internal Temperature Limit
+ min_voltage_limit, // (RW) Lowest Voltage Limit
+ max_voltage_limit, // (RW) Highest Voltage Limit
+ pwm_limit, // (RW) Maximum Torque
+ max_pwm, // (RW) Maximum Torque
+ current_limit, // (RW)
+ max_current, // (RW)
+ velocity_limit, // (RW)
+ multi_turn_off, // (RW) Multi turn offset position
+ resolution_div, // (RW) Resolution divider
+ // Status
+ alarm_led, // (RW) LED for Alarm
+ alarm_shtdwn, // (RW) Shutdown for Alarm
+ led, // (RW) LED On/Off
+ bus_watchdog, // (RW)
+ moving, // (R) Means if there is any movement
+ moving_status, // (R)
+ lock, // (RW) Locking EEPROM
+ reg_inst, // (RW) Means if Instruction is registered
+ hardware_error, // (R) Means if there is any movement
+ // control
+ torque_en, // (RW) Torque On/Off
+ vel_pid_i, // (RW)
+ vel_pid_p, // (RW)
+ pos_pid_p, // (RW)
+ pos_pid_i, // (RW)
+ pos_pid_d, // (RW)
+ feedfwd_gain_2, // (RW)
+ feedfwd_gain_1, // (RW)
+ cw_comp_margin, // (RW) CW Compliance Margin
+ ccw_comp_margin, // (RW) CCW Compliance Margin
+ cw_comp_slope, // (RW) CW Compliance Slope
+ ccw_comp_slope, // (RW) CCW Compliance Slope
+ punch, // (RW) Punch
+ // Set point
+ goal_pos, // (RW) Goal Position
+ goal_speed, // (RW) Moving Speed
+ goal_pwm, // (RW)
+ goal_current, // (RW)
+ profile_accel, // (RW)
+ profile_vel, // (RW)
+ // Feedback
+ current_pos, // (R) Current Position
+ current_speed, // (R) Current Speed
+ current_load, // (R) Current Load
+ current_voltage, // (R) Current Voltage
+ current_temp, // (R) Current Temperature
+ current_pwm, // (R)
+ realtime_tick, // (R) Realtime tick
+ velocity_traj, // (R)
+ pos_traj // (R)
+} reg_id;
+
+#define NUM_SERVO_MODELS 11
+
+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;
+ TDynReg *registers;
+ unsigned char pid;
+ unsigned char protocol_ver;
+}TServoConfig;
+
+extern TServoConfig servo_data[NUM_SERVO_MODELS];
#endif
diff --git a/dynamixel_manager/src/modules/dyn_servos.c b/dynamixel_manager/src/modules/dyn_servos.c
new file mode 100644
index 0000000000000000000000000000000000000000..4a7ba4a90f29903fa52bbd3bf4fb2dce2b9396ff
--- /dev/null
+++ b/dynamixel_manager/src/modules/dyn_servos.c
@@ -0,0 +1,291 @@
+#include "dyn_servos.h"
+
+TDynReg ax_reg[NUM_REG]={// Info
+ {0x0000,2,1},
+ {0xFFFF,0,0},
+ {0x0002,1,1},
+ {0x0003,1,1},
+ {0x0004,1,1},
+ {0x0005,1,1},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0x0010,1,1},
+ // Configuration
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0x0006,2,1},
+ {0x0008,2,1},
+ {0x000B,1,1},
+ {0x000C,1,1},
+ {0x000D,1,1},
+ {0x000E,2,1},
+ {0x0022,2,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ // Status
+ {0x0011,1,1},
+ {0x0012,1,1},
+ {0x0019,1,0},
+ {0xFFFF,0,0},
+ {0x002E,1,0},
+ {0xFFFF,0,0},
+ {0x002F,1,0},
+ {0x002C,1,0},
+ {0xFFFF,0,0},
+ // Control
+ {0x0018,1,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0x001A,1,0},
+ {0x001B,1,0},
+ {0x001C,1,0},
+ {0x001D,1,0},
+ {0x0030,2,0},
+ // Set point
+ {0x001E,2,0},
+ {0x0020,2,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ // Feedback
+ {0x0024,2,0},
+ {0x0026,2,0},
+ {0x0028,2,0},
+ {0x002A,1,0},
+ {0x002B,1,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0}};
+
+TDynReg mx_1_0_reg[NUM_REG]={// Info
+ {0x0000,2,1},
+ {0xFFFF,0,0},
+ {0x0002,1,1},
+ {0x0003,1,1},
+ {0x0004,1,1},
+ {0x0005,1,1},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0x0010,1,1},
+ // Configuration
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0x0006,2,1},
+ {0x0008,2,1},
+ {0x000B,1,1},
+ {0x000C,1,1},
+ {0x000D,1,1},
+ {0x000E,2,1},
+ {0x0022,2,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0x0020,2,0},
+ {0x0014,2,1},
+ {0x0016,1,1},
+ // Status
+ {0x0011,1,1},
+ {0x0012,1,1},
+ {0x0019,1,0},
+ {0xFFFF,0,0},
+ {0x002E,1,0},
+ {0xFFFF,0,0},
+ {0x002F,1,0},
+ {0x002C,1,0},
+ {0xFFFF,0,0},
+ // Control
+ {0x0018,1,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0x001C,1,0},
+ {0x001B,1,0},
+ {0x001A,1,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0x0030,2,0},
+ // Set point
+ {0x001E,2,0},
+ {0x0020,2,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0x0049,1,0},
+ {0xFFFF,0,0},
+ // Feedback
+ {0x0024,2,0},
+ {0x0026,2,0},
+ {0x0028,2,0},
+ {0x002A,1,0},
+ {0x002B,1,0},
+ {0xFFFF,0,0},
+ {0x0032,2,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0}};
+
+TDynReg xl_reg[NUM_REG]={// Info
+ {0x0000,2,1},
+ {0xFFFF,0,0},
+ {0x0002,1,1},
+ {0x0003,1,1},
+ {0x0004,1,1},
+ {0x0005,1,1},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0x0011,1,1},
+ // Configuration
+ {0xFFFF,0,0},
+ {0x000B,1,1},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0x0006,2,1},
+ {0x0008,2,1},
+ {0x000C,1,1},
+ {0x000D,1,1},
+ {0x000E,1,1},
+ {0x000F,2,1},
+ {0x0023,2,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ // Status
+ {0x0012,1,1},
+ {0x0012,1,1},
+ {0x0019,1,0},
+ {0xFFFF,0,0},
+ {0x0031,1,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0x002F,1,0},
+ {0x0032,1,0},
+ // Control
+ {0x0018,1,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0x001D,1,0},
+ {0x001C,1,0},
+ {0x001B,1,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0x0033,2,0},
+ // Set point
+ {0x001E,2,0},
+ {0x0020,2,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0x0049,1,0},
+ {0xFFFF,0,0},
+ // Feedback
+ {0x0025,2,0},
+ {0x0027,2,0},
+ {0x0029,2,0},
+ {0x002D,1,0},
+ {0x002E,1,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0}};
+
+TDynReg mx_2_0_reg[NUM_REG]={// Info
+ {0x0000,2,1},
+ {0x0002,4,1},
+ {0x0006,1,1},
+ {0x0007,1,1},
+ {0x0008,1,1},
+ {0x0009,1,1},
+ {0x000C,1,1},
+ {0x000D,1,1},
+ {0x0044,1,0},
+ // Configuration
+ {0x000A,1,1},
+ {0x000B,1,1},
+ {0x0014,4,1},
+ {0x0018,4,1},
+ {0x0034,4,1},
+ {0x0030,4,1},
+ {0x001F,1,1},
+ {0x0022,2,1},
+ {0x0020,2,1},
+ {0x0024,2,1},
+ {0xFFFF,0,0},
+ {0x0026,2,1},
+ {0xFFFF,0,0},
+ {0x002C,4,1},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ // Status
+ {0x003F,1,1},
+ {0x003F,1,1},
+ {0x0041,1,0},
+ {0x0062,1,0},
+ {0x007A,1,0},
+ {0x007B,1,0},
+ {0xFFFF,0,0},
+ {0x0045,1,0},
+ {0x0046,1,0},
+ // Control
+ {0x0040,1,0},
+ {0x004C,2,0},
+ {0x004E,2,0},
+ {0x0054,2,0},
+ {0x0052,2,0},
+ {0x0050,2,0},
+ {0x0058,2,0},
+ {0x005A,2,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ {0xFFFF,0,0},
+ // Set point
+ {0x0074,4,0},
+ {0x0068,4,0},
+ {0x0064,2,0},
+ {0x0066,2,0},
+ {0x006C,4,0},
+ {0x0070,4,0},
+ // Feedback
+ {0x0084,4,0},
+ {0x0080,4,0},
+ {0x007E,2,0},
+ {0x0090,2,0},
+ {0x0092,1,0},
+ {0x007C,2,0},
+ {0x0078,2,0},
+ {0x0088,5,0},
+ {0x008C,4,0}};
+
+TServoConfig servo_data[NUM_SERVO_MODELS]={
+//model | enc_res | gear | max_angle | center_angle | max_speed | cw_limit | ccw_limit | registers | pid | protocol
+{300 , 1023 , 32 , 300<<7 , 150<<7 , 324 , 0 , 300<<7 , ax_reg , 0 , 1},
+{12 , 1023 , 254 , 300<<7 , 150<<7 , 354 , 0 , 300<<7 , ax_reg , 0 , 1},
+{18 , 1023 , 254 , 300<<7 , 150<<7 , 582 , 0 , 300<<7 , ax_reg , 0 , 1},
+{360 , 4095 , 32 , 360<<7 , 180<<7 , 2820 , 0 , 360<<7 , mx_1_0_reg, 1 , 1},
+{29 , 4095 , 193 , 360<<7 , 180<<7 , 330 , 0 , 360<<7 , mx_1_0_reg, 1 , 1},
+{310 , 4095 , 200 , 360<<7 , 180<<7 , 378 , 0 , 360<<7 , mx_1_0_reg, 1 , 1},
+{320 , 4095 , 225 , 360<<7 , 180<<7 , 270 , 0 , 360<<7 , mx_1_0_reg, 1 , 1},
+{30 , 4095 , 193 , 360<<7 , 180<<7 , 378 , 0 , 360<<7 , mx_2_0_reg, 1 , 2},
+{311 , 4095 , 200 , 360<<7 , 180<<7 , 378 , 0 , 360<<7 , mx_2_0_reg, 1 , 2},
+{321 , 4095 , 225 , 360<<7 , 180<<7 , 270 , 0 , 360<<7 , mx_2_0_reg, 1 , 2},
+{350 , 1023 , 238 , 300<<7 , 150<<7 , 684 , 0 , 300<<7 , xl_reg , 1 , 2}};