diff --git a/dynamixel_manager/include/modules/motion_manager.h b/dynamixel_manager/include/modules/motion_manager.h
index cc92077872c2152dfdd1c5637b077b3139840913..268c0e8b6ead57dd84eb66bf1f2f283b83430b32 100644
--- a/dynamixel_manager/include/modules/motion_manager.h
+++ b/dynamixel_manager/include/modules/motion_manager.h
@@ -6,6 +6,7 @@ extern "C" {
#endif
#include "dyn_module.h"
+#include "dyn_servos.h"
#ifndef MM_MAX_NUM_MOTION_MODULES
#error "Please, specify the maximum number of motion modules with the MM_MAX_NUM_MOTION_MODULES macro"
@@ -17,23 +18,12 @@ typedef enum {MM_NONE = -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;
+ unsigned int current_value;
long long int current_angle;
TModules module;
unsigned char enabled;
@@ -46,11 +36,11 @@ typedef struct {
TDynModule dyn_module;
struct TMotionModule *modules[MM_MAX_NUM_MOTION_MODULES];
unsigned char num_modules;
- TServoConfig servo_configs[DYN_MANAGER_MAX_NUM_DEVICES];
+ 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;
+ OP_HANDLE *motion_op[2];
+ OP_HANDLE *feedback_op[2];
unsigned char balance_enabled;
void (*balance)(short int offsets[DYN_MANAGER_MAX_NUM_DEVICES]);
}TMotionManager;
diff --git a/dynamixel_manager/include/modules/motion_manager_registers.h b/dynamixel_manager/include/modules/motion_manager_registers.h
index 02fbc9ed572a7d596bada1c73b517df5bb13815a..dcb5838b0f3a2f0dcc3e0e75488acbba1ea2398d 100644
--- a/dynamixel_manager/include/modules/motion_manager_registers.h
+++ b/dynamixel_manager/include/modules/motion_manager_registers.h
@@ -3,14 +3,17 @@
#include "dyn_module_registers.h"
-#define RAM_MM_LENGTH (RAM_DYN_MODULE_LENGTH + 0)
+#define RAM_MM_LENGTH (RAM_DYN_MODULE_LENGTH+DYN_MANAGER_MAX_NUM_DEVICES/2)
+
+#define MM_ENABLE_MODULE 0
#define EEPROM_MM_LENGTH (DYN_MANAGER_MAX_NUM_DEVICES)
#define MM_SERVO_OFFSET 0
-#define dyn_mm_eeprom_data(name,section_name,base_address) \
-unsigned short int name##_eeprom_data[] __attribute__ ((section (section_name)))={0,base_address+MM_SERVO_OFFSET};
+#define dyn_mm_eeprom_data(name,section_name,base_address,DEFAULT_PERIOD,initial_offset) \
+dyn_module_eeprom_data(name,section_name,base_address,DEFAULT_PERIOD) \
+unsigned short int name##_eeprom_data[] __attribute__ ((section (section_name)))={initial_offset};
#endif
diff --git a/dynamixel_manager/src/modules/motion_manager.c b/dynamixel_manager/src/modules/motion_manager.c
index ae78e45e9193180b609288c959c116efeda76365..678880eafb6c95bdea0febd5c6941782699ab3f3 100644
--- a/dynamixel_manager/src/modules/motion_manager.c
+++ b/dynamixel_manager/src/modules/motion_manager.c
@@ -1,10 +1,28 @@
#include "motion_manager.h"
#include "motion_manager_registers.h"
#include "motion_module.h"
-#include "dyn_devices.h"
-#include "dyn_servos.h"
/* private functions */
+void mmanager_read_register(TMotionManager *mmanager,unsigned char id,TDynReg *reg,unsigned int *value)
+{
+ unsigned char reg_value[4]={0};
+
+ if(reg->address!=0xFFFF)
+ {
+ dyn_master_read_table(dyn_manager_get_device_master(mmanager->dyn_module.manager,id),id,reg->address,reg->size,reg_value);
+ if(reg->size==1)
+ (*value)=reg_value[0];
+ else if(reg->size==2)
+ (*value)=reg_value[0]+(reg_value[1]<<8);
+ else if(reg->size==4)
+ (*value)=reg_value[0]+(reg_value[1]<<8)+(reg_value[2]<<16)+(reg_value[3]<<24);
+ }
+}
+
+void mmanager_write_register(TMotionManager *mmanager,unsigned char id,TDynReg *reg,unsigned int value)
+{
+}
+
void mmanager_read_cmd(void *module,unsigned short int address,unsigned short int length,unsigned char *data)
{
@@ -18,13 +36,13 @@ void mmanager_write_cmd(void *module,unsigned short int address,unsigned short i
unsigned short int mmanager_angle_to_value(TMotionManager *mmanager,unsigned char servo_id,short int angle);
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;
+ return ((angle+mmanager->servo_configs[servo_id]->center_angle)*mmanager->servo_configs[servo_id]->encoder_resolution)/mmanager->servo_configs[servo_id]->max_angle;
}
short int mmanager_value_to_angle(TMotionManager *mmanager,unsigned char servo_id,unsigned short int value);
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);
+ 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)
@@ -62,129 +80,24 @@ void mmanager_compute_angles(TMotionManager *mmanager)
void mmanager_add_device(TMotionManager *mmanager,unsigned char id,unsigned short int model)
{
- unsigned short int cw_value,ccw_value;
+ unsigned int cw_value,ccw_value;
+ unsigned char i;
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);
- }
+ for(i=0;i<NUM_SERVO_MODELS;i++)
+ if(model==servo_data[i].model)
+ mmanager->servo_configs[id]=&servo_data[i];
+
+ mmanager_read_register(mmanager,id,&mmanager->servo_configs[id]->registers[current_pos],&mmanager->servo_values[id].current_value);
+ mmanager_read_register(mmanager,id,&mmanager->servo_configs[id]->registers[min_angle_limit],&cw_value);
+ mmanager_read_register(mmanager,id,&mmanager->servo_configs[id]->registers[max_angle_limit],&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);
+ 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);
}
}
@@ -194,39 +107,69 @@ void mmanager_setup(TMotionManager *mmanager)
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;
+ unsigned char i,num=0,first=1;
+ unsigned short int start_address=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++;
+ if(mmanager->servo_configs[i]->protocol_ver==1)
+ {
+ data[num]=&mmanager->servo_values[i].cw_compliance;
+ ids[num]=i;
+ num++;
+ if(first==1)
+ {
+ first=0;
+ if(mmanager->servo_configs[i]->pid==1)
+ start_address=mmanager->servo_configs[i]->registers[pos_pid_i].address;
+ else
+ start_address=mmanager->servo_configs[i]->registers[cw_comp_margin].address;
+ }
+ }
+ }
+ }
+ if(num>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++)
+ {
+ if(mmanager->dyn_module.assigned_ids[i]==0x01)
+ {
+ if(mmanager->servo_configs[i]->protocol_ver==2)
+ {
+ data[num]=(unsigned char *)&mmanager->servo_values[i].target_value;
+ ids[num]=i;
+ num++;
+ if(first==1)
+ {
+ first=0;
+ start_address=mmanager->servo_configs[i]->registers[goal_pos].address;
+ }
+ }
}
}
- 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);
+ if(num>0)
+ 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++)
{
if(mmanager->dyn_module.assigned_ids[i]==0x01)
{
- if(HAS_BULK_READ(dyn_manager_get_device_model(mmanager->dyn_module.manager,i)))
+ if(mmanager->servo_configs[i]->protocol_ver==2)
{
- 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;
+ address[num]=mmanager->servo_configs[i]->registers[current_pos].address;
data[num]=(unsigned char *)&mmanager->servo_values[i].current_value;
- length[num]=2;
+ length[num]=mmanager->servo_configs[i]->registers[current_pos].size;
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);
+ 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)
@@ -272,22 +215,8 @@ unsigned char mmanager_init(TMotionManager *mmanager,TMemory *memory,unsigned sh
/* 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);
+ for(i=0;i<NUM_SERVO_MODELS;i++)
+ dyn_module_add_model(&mmanager->dyn_module,servo_data[i].model);
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;
@@ -303,14 +232,7 @@ unsigned char mmanager_init(TMotionManager *mmanager,TMemory *memory,unsigned sh
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;
+ mmanager->servo_configs[i]=0x00000000;
/* servo values */
mmanager->servo_values[i].cw_compliance=0x00;
mmanager->servo_values[i].ccw_compliance=0x00;
@@ -323,8 +245,10 @@ unsigned char mmanager_init(TMotionManager *mmanager,TMemory *memory,unsigned sh
mmanager->servo_values[i].offset=0x00;
}
mmanager->enable_op=0x00000000;
- mmanager->motion_op=0x00000000;
- mmanager->feedback_op=0x00000000;
+ mmanager->motion_op[0]=0x00000000;
+ mmanager->motion_op[1]=0x00000000;
+ mmanager->feedback_op[0]=0x00000000;
+ mmanager->feedback_op[1]=0x00000000;
mmanager->balance=0x00000000;
/* initialize memory module */
@@ -374,7 +298,7 @@ void mmanager_enable_servo(TMotionManager *mmanager,unsigned char servo_id)
/* 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);
+ mmanager->enable_op=dyn_manager_sync_op_new(mmanager->dyn_module.manager,DYN_MANAGER_WRITE,1,&servo_id,mmanager->servo_configs[servo_id]->registers[torque_en].address,1,&data);
dyn_manager_set_op_repetitions(mmanager->dyn_module.manager,mmanager->enable_op,1);
}
else
@@ -392,7 +316,7 @@ void mmanager_disable_servo(TMotionManager *mmanager,unsigned char servo_id)
/* 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);
+ mmanager->enable_op=dyn_manager_sync_op_new(mmanager->dyn_module.manager,DYN_MANAGER_WRITE,1,&servo_id,mmanager->servo_configs[servo_id]->registers[torque_en].address,1,&data);
dyn_manager_set_op_repetitions(mmanager->dyn_module.manager,mmanager->enable_op,1);
}
else