Commit 62d5566f authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added support to change the servos offsets in the firmware.

parent b545b8a2
......@@ -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
......
......@@ -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))
......
Supports Markdown
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