diff --git a/include/motion_manager.h b/include/motion_manager.h index c74a16e43e467203f13ebd93526ee02eef8ea822..ce7a58006bbb6267892dc8fd79bcf2659bdce483 100644 --- a/include/motion_manager.h +++ b/include/motion_manager.h @@ -84,6 +84,7 @@ inline TModules manager_get_module(uint8_t servo_id); inline void manager_enable_servo(uint8_t servo_id); inline void manager_disable_servo(uint8_t servo_id); inline uint8_t manager_is_servo_enabled(uint8_t servo_id); +void manager_set_offset(uint8_t servo_id,int8_t offset); inline int16_t manager_get_cw_angle_limit(uint8_t servo_id); inline int16_t manager_get_ccw_angle_limit(uint8_t servo_id); // motion modules handling diff --git a/src/motion_manager.c b/src/motion_manager.c index 86b5b751cb7b1a5bbfd42ca5b5357bdbbdd3bec9..97b7cf0da77fdb593f1c9fc11a82f5adc83622fc 100644 --- a/src/motion_manager.c +++ b/src/motion_manager.c @@ -25,6 +25,8 @@ int8_t manager_current_slopes[MANAGER_MAX_NUM_SERVOS]; // balance values and configuration int64_t manager_balance_offset[MANAGER_MAX_NUM_SERVOS]; uint8_t manager_balance_enabled; +// manager offsets +int16_t manager_offset[MANAGER_MAX_NUM_SERVOS]; // private functions void manager_send_motion_command(void) @@ -99,7 +101,7 @@ void manager_get_target_position(void) { if(manager_servos[i].module==MM_ACTION) { - manager_servos[i].current_angle=((manager_current_angles[i]>>9)+manager_balance_offset[i]); + manager_servos[i].current_angle=((manager_current_angles[i]>>9)+manager_balance_offset[i]+manager_offset[i]); //>>16 from the action codification, <<7 from the manager codification manager_servos[i].current_value=manager_angle_to_value(i,manager_servos[i].current_angle); ram_data[BIOLOID_MM_SERVO0_CUR_POS_L+i*2]=(manager_servos[i].current_angle&0xFF); @@ -320,8 +322,12 @@ void manager_init(uint16_t period_us) manager_motion_period_us=period_us; /* initialize balance parameters */ + /* initialize the manager offsets from EEPROM */ for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++) + { manager_balance_offset[i]=0; + manager_set_offset(i,(signed char)ram_data[BIOLOID_MM_SERVO0_OFFSET+i]); + } manager_balance_enabled=0x00; gpio_set_led(TXD_LED); @@ -454,6 +460,15 @@ inline uint8_t manager_is_servo_enabled(uint8_t servo_id) return 0x00; } +void manager_set_offset(uint8_t servo_id, int8_t offset) +{ + if(servo_id>=0 && servo_id<MANAGER_MAX_NUM_SERVOS) + { + manager_offset[servo_id]=(int16_t)(offset*8); + ram_data[BIOLOID_MM_SERVO0_OFFSET+servo_id]=offset; + } +} + inline int16_t manager_get_cw_angle_limit(uint8_t servo_id) { return manager_servos[servo_id].cw_angle_limit; @@ -468,8 +483,8 @@ inline int16_t manager_get_ccw_angle_limit(uint8_t servo_id) uint8_t manager_in_range(unsigned short int address, unsigned short int length) { if(ram_in_window(MANAGER_BASE_ADDRESS,MANAGER_MEM_LENGTH,address,length) || - ram_in_window(BIOLOID_MM_PERIOD_L,BIOLOID_MM_BAL_HIP_ROLL_GAIN_H,address,length) || - ram_in_window(BIOLOID_MM_SERVO0_OFFSET,BIOLOID_MM_SERVO31_OFFSET,address,length)) + ram_in_window(BIOLOID_MM_PERIOD_L,10,address,length) || + ram_in_window(BIOLOID_MM_SERVO0_OFFSET,MANAGER_MAX_NUM_SERVOS,address,length)) return 0x01; else return 0x00; @@ -485,6 +500,11 @@ void manager_process_write_cmd(unsigned short int address,unsigned short int len word_value=data[BIOLOID_MM_PERIOD_L-address]+(data[BIOLOID_MM_PERIOD_H-address]<<8); manager_set_period(word_value); } + for(i=BIOLOID_MM_SERVO0_OFFSET,j=0;i<=BIOLOID_MM_SERVO31_OFFSET;i++,j++) + { + if(ram_in_range(i,address,length)) + manager_set_offset(j,(signed char)(data[i-address])); + } for(i=BIOLOID_MM_MODULE_EN0,j=0;i<=BIOLOID_MM_MODULE_EN15;i++,j+=2) { if(ram_in_range(i,address,length))