Skip to content
Snippets Groups Projects
Commit 92d70a57 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Updated the code to use the register mappings. 

Added a function to read a register.
Updated the operations configuration of the motion manager.
parent d7546826
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
dynamixel_manager/include/modules/motion_manager.h
+ 5
− 15
View file @ 92d70a57
...@@ -6,6 +6,7 @@ extern "C" { ...@@ -6,6 +6,7 @@ extern "C" {
#endif #endif
#include "dyn_module.h" #include "dyn_module.h"
#include "dyn_servos.h"
#ifndef MM_MAX_NUM_MOTION_MODULES #ifndef MM_MAX_NUM_MOTION_MODULES
#error "Please, specify the maximum number of motion modules with the MM_MAX_NUM_MOTION_MODULES macro" #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, ...@@ -17,23 +18,12 @@ typedef enum {MM_NONE = -1,
MM_JOINTS = 2, MM_JOINTS = 2,
MM_HEAD = 3} TModules; 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{ typedef struct{
unsigned char cw_compliance; unsigned char cw_compliance;
unsigned char ccw_compliance; unsigned char ccw_compliance;
unsigned short int target_value; unsigned short int target_value;
long long int target_angle; long long int target_angle;
unsigned short int current_value; unsigned int current_value;
long long int current_angle; long long int current_angle;
TModules module; TModules module;
unsigned char enabled; unsigned char enabled;
...@@ -46,11 +36,11 @@ typedef struct { ...@@ -46,11 +36,11 @@ typedef struct {
TDynModule dyn_module; TDynModule dyn_module;
struct TMotionModule *modules[MM_MAX_NUM_MOTION_MODULES]; struct TMotionModule *modules[MM_MAX_NUM_MOTION_MODULES];
unsigned char num_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]; TServoValues servo_values[DYN_MANAGER_MAX_NUM_DEVICES];
OP_HANDLE *enable_op; OP_HANDLE *enable_op;
OP_HANDLE *motion_op; OP_HANDLE *motion_op[2];
OP_HANDLE *feedback_op; OP_HANDLE *feedback_op[2];
unsigned char balance_enabled; unsigned char balance_enabled;
void (*balance)(short int offsets[DYN_MANAGER_MAX_NUM_DEVICES]); void (*balance)(short int offsets[DYN_MANAGER_MAX_NUM_DEVICES]);
}TMotionManager; }TMotionManager;
......
dynamixel_manager/include/modules/motion_manager_registers.h
+ 6
− 3
View file @ 92d70a57
...@@ -3,14 +3,17 @@ ...@@ -3,14 +3,17 @@
#include "dyn_module_registers.h" #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 EEPROM_MM_LENGTH (DYN_MANAGER_MAX_NUM_DEVICES)
#define MM_SERVO_OFFSET 0 #define MM_SERVO_OFFSET 0
#define dyn_mm_eeprom_data(name,section_name,base_address) \ #define dyn_mm_eeprom_data(name,section_name,base_address,DEFAULT_PERIOD,initial_offset) \
unsigned short int name##_eeprom_data[] __attribute__ ((section (section_name)))={0,base_address+MM_SERVO_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 #endif
dynamixel_manager/src/modules/motion_manager.c
+ 84
− 160
View file @ 92d70a57
#include "motion_manager.h" #include "motion_manager.h"
#include "motion_manager_registers.h" #include "motion_manager_registers.h"
#include "motion_module.h" #include "motion_module.h"
#include "dyn_devices.h"
#include "dyn_servos.h"
/* private functions */ /* 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) 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 ...@@ -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); 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) 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); 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) 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) void mmanager_compute_targets(TMotionManager *mmanager)
...@@ -62,129 +80,24 @@ void mmanager_compute_angles(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) 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) if(id<DYN_MANAGER_MAX_NUM_DEVICES)
{ {
switch(model) for(i=0;i<NUM_SERVO_MODELS;i++)
{ if(model==servo_data[i].model)
case SERVO_DX113: mmanager->servo_configs[id].encoder_resolution=1023; mmanager->servo_configs[id]=&servo_data[i];
mmanager->servo_configs[id].gear_ratio=193;
mmanager->servo_configs[id].max_angle=300<<7; mmanager_read_register(mmanager,id,&mmanager->servo_configs[id]->registers[current_pos],&mmanager->servo_values[id].current_value);
mmanager->servo_configs[id].center_angle=150<<7; mmanager_read_register(mmanager,id,&mmanager->servo_configs[id]->registers[min_angle_limit],&cw_value);
mmanager->servo_configs[id].max_speed=324; mmanager_read_register(mmanager,id,&mmanager->servo_configs[id]->registers[max_angle_limit],&ccw_value);
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].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].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; mmanager->servo_values[id].target_angle=mmanager->servo_values[id].current_angle;
// read the servo limits // read the servo limits
mmanager->servo_configs[id].cw_angle_limit=(mmanager_value_to_angle(mmanager,id,cw_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); 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) ...@@ -194,39 +107,69 @@ void mmanager_setup(TMotionManager *mmanager)
unsigned char ids[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 address[DYN_MANAGER_MAX_NUM_DEVICES];
unsigned short int length[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 // initialize the motion operation
for(i=0;i<DYN_MANAGER_MAX_NUM_DEVICES;i++) for(i=0;i<DYN_MANAGER_MAX_NUM_DEVICES;i++)
{ {
if(mmanager->dyn_module.assigned_ids[i]==0x01) if(mmanager->dyn_module.assigned_ids[i]==0x01)
{ {
data[num]=&mmanager->servo_values[i].cw_compliance; if(mmanager->servo_configs[i]->protocol_ver==1)
ids[num]=i; {
num++; 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 */ /* create a feedback operation with the servos that support bulk read */
num=0; num=0;
for(i=0;i<DYN_MANAGER_MAX_NUM_DEVICES;i++) for(i=0;i<DYN_MANAGER_MAX_NUM_DEVICES;i++)
{ {
if(mmanager->dyn_module.assigned_ids[i]==0x01) 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]=mmanager->servo_configs[i]->registers[current_pos].address;
address[num]=XL_PRESENT_POSITION_L;
else
address[num]=P_PRESENT_POSITION_L;
data[num]=(unsigned char *)&mmanager->servo_values[i].current_value; 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; ids[num]=i;
num++; 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) void mmanager_set_period(TMotionManager *mmanager,unsigned short period_ms)
...@@ -272,22 +215,8 @@ unsigned char mmanager_init(TMotionManager *mmanager,TMemory *memory,unsigned sh ...@@ -272,22 +215,8 @@ unsigned char mmanager_init(TMotionManager *mmanager,TMemory *memory,unsigned sh
/* initialize the base module */ /* initialize the base module */
dyn_module_init(&mmanager->dyn_module); dyn_module_init(&mmanager->dyn_module);
/* add all known servo models */ /* add all known servo models */
dyn_module_add_model(&mmanager->dyn_module,SERVO_DX113); for(i=0;i<NUM_SERVO_MODELS;i++)
dyn_module_add_model(&mmanager->dyn_module,SERVO_DX116); dyn_module_add_model(&mmanager->dyn_module,servo_data[i].model);
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.data=mmanager;
mmanager->dyn_module.add_device=(void (*)(void *,unsigned char,unsigned short int))mmanager_add_device; 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.setup=(void (*)(void *))mmanager_setup;
...@@ -303,14 +232,7 @@ unsigned char mmanager_init(TMotionManager *mmanager,TMemory *memory,unsigned sh ...@@ -303,14 +232,7 @@ unsigned char mmanager_init(TMotionManager *mmanager,TMemory *memory,unsigned sh
for(i=0;i<DYN_MANAGER_MAX_NUM_DEVICES;i++) for(i=0;i<DYN_MANAGER_MAX_NUM_DEVICES;i++)
{ {
/* servo configuration */ /* servo configuration */
mmanager->servo_configs[i].model=0x0000; mmanager->servo_configs[i]=0x00000000;
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 */ /* servo values */
mmanager->servo_values[i].cw_compliance=0x00; mmanager->servo_values[i].cw_compliance=0x00;
mmanager->servo_values[i].ccw_compliance=0x00; mmanager->servo_values[i].ccw_compliance=0x00;
...@@ -323,8 +245,10 @@ unsigned char mmanager_init(TMotionManager *mmanager,TMemory *memory,unsigned sh ...@@ -323,8 +245,10 @@ unsigned char mmanager_init(TMotionManager *mmanager,TMemory *memory,unsigned sh
mmanager->servo_values[i].offset=0x00; mmanager->servo_values[i].offset=0x00;
} }
mmanager->enable_op=0x00000000; mmanager->enable_op=0x00000000;
mmanager->motion_op=0x00000000; mmanager->motion_op[0]=0x00000000;
mmanager->feedback_op=0x00000000; mmanager->motion_op[1]=0x00000000;
mmanager->feedback_op[0]=0x00000000;
mmanager->feedback_op[1]=0x00000000;
mmanager->balance=0x00000000; mmanager->balance=0x00000000;
/* initialize memory module */ /* initialize memory module */
...@@ -374,7 +298,7 @@ void mmanager_enable_servo(TMotionManager *mmanager,unsigned char servo_id) ...@@ -374,7 +298,7 @@ void mmanager_enable_servo(TMotionManager *mmanager,unsigned char servo_id)
/* add an operation to enable the servo */ /* add an operation to enable the servo */
if(mmanager->enable_op==0x00000000 || mmanager->enable_op->op_type==no_op) 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); dyn_manager_set_op_repetitions(mmanager->dyn_module.manager,mmanager->enable_op,1);
} }
else else
...@@ -392,7 +316,7 @@ void mmanager_disable_servo(TMotionManager *mmanager,unsigned char servo_id) ...@@ -392,7 +316,7 @@ void mmanager_disable_servo(TMotionManager *mmanager,unsigned char servo_id)
/* add an operation to enable the servo */ /* add an operation to enable the servo */
if(mmanager->enable_op==0x00000000 || mmanager->enable_op->op_type==no_op) 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); dyn_manager_set_op_repetitions(mmanager->dyn_module.manager,mmanager->enable_op,1);
} }
else else
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment