Commit 89f632b5 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Solved some bugs:

  - the internal action period needs to be transformed from the period in us.
  - removed the call to the action_load_next_step() function when the action finishes by itself.
parent c3c8de0c
......@@ -12,8 +12,8 @@ extern int64_t action_current_angles[PAGE_MAX_NUM_SERVOS];
extern uint8_t action_slope[PAGE_MAX_NUM_SERVOS];
// public functions
void action_init(uint16_t period);
inline void action_set_period(uint16_t period);
void action_init(uint16_t period_us);
inline void action_set_period(uint16_t period_us);
inline uint16_t action_get_period(void);
uint8_t action_load_page(uint8_t page_id);
void action_start_page(void);
......
......@@ -76,7 +76,7 @@ inline uint8_t manager_is_enabled(void);
void manager_enable_balance(void);
void manager_disable_balance(void);
inline uint8_t manager_get_num_servos(void);
inline void manager_set_module(uint8_t servo_id,TModules module);
void manager_set_module(uint8_t servo_id,TModules module);
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);
......
......@@ -33,6 +33,7 @@ int64_t action_pause_time;// fixed point 48|16 format
int64_t action_current_time;// fixed point 48|16 format
int64_t action_section_time;// fixed point 48|16 format
int64_t action_period;
int16_t action_period_us;
// private functions
void action_load_next_step(void)
......@@ -219,7 +220,6 @@ void action_load_next_step(void)
void action_finish(void)
{
// set the internal state machine to the idle state
action_load_next_step();
action_end=0x00;
// clear the status falg for the action module
ram_data[BIOLOID_ACTION_CNTRL]&=(~ACTION_STATUS);
......@@ -233,7 +233,7 @@ void action_finish(void)
}
// public functions
void action_init(uint16_t period)
void action_init(uint16_t period_us)
{
unsigned char i;
......@@ -266,17 +266,19 @@ void action_init(uint16_t period)
action_current_time=0;// fixed point 48|16 format
action_section_time=0;// fixed point 48|16 format
action_period=period;
action_period=(period_us<<16)/1000000;
action_period_us=period_us;
}
inline void action_set_period(uint16_t period)
inline void action_set_period(uint16_t period_us)
{
action_period=period;
action_period=(period_us<<16)/1000000;
action_period_us=period_us;
}
inline uint16_t action_get_period(void)
{
return action_period;
return action_period_us;
}
uint8_t action_load_page(uint8_t page_id)
......
......@@ -105,15 +105,17 @@ void manager_get_target_position(void)
void manager_balance(void)
{
adc_ch_t fb_ch=gyro_get_fb_adc_channel(),lr_ch=gyro_get_lr_adc_channel();
adc_ch_t fb_ch,lr_ch;
int16_t knee_gain,ankle_roll_gain,ankle_pitch_gain,hip_roll_gain;
uint16_t fb_offset,lr_offset;
int16_t fb_value,lr_value;
if(manager_balance_enabled==0x01)// balance is enabled
{
fb_ch=gyro_get_fb_adc_channel();
lr_ch=gyro_get_lr_adc_channel();
// get the balance gains
knee_gain=ram_data[BIOLOID_MM_BAL_KNEE_GAIN_L]+(ram_data[BIOLOID_MM_BAL_KNEE_GAIN_H]*256);
knee_gain=ram_data[BIOLOID_MM_BAL_KNEE_GAIN_L]+(ram_data[BIOLOID_MM_BAL_KNEE_GAIN_H]<<8);
ankle_roll_gain=ram_data[BIOLOID_MM_BAL_ANKLE_ROLL_GAIN_L]+(ram_data[BIOLOID_MM_BAL_ANKLE_ROLL_GAIN_H]<<8);
ankle_pitch_gain=ram_data[BIOLOID_MM_BAL_ANKLE_PITCH_GAIN_L]+(ram_data[BIOLOID_MM_BAL_ANKLE_PITCH_GAIN_H]<<8);
hip_roll_gain=ram_data[BIOLOID_MM_BAL_HIP_ROLL_GAIN_L]+(ram_data[BIOLOID_MM_BAL_HIP_ROLL_GAIN_H]<<8);
......@@ -123,14 +125,14 @@ void manager_balance(void)
fb_value=adc_get_channel_raw(fb_ch)-fb_offset;
lr_value=adc_get_channel_raw(lr_ch)-lr_offset;
// compensate the servo angle values
manager_balance_offset[R_KNEE]=(((fb_value*manager_servos[R_KNEE].max_angle)/manager_servos[R_KNEE].encoder_resolution)*knee_gain)/65536;
manager_balance_offset[R_ANKLE_PITCH]=(((fb_value*manager_servos[R_ANKLE_PITCH].max_angle)/manager_servos[R_ANKLE_PITCH].encoder_resolution)*ankle_pitch_gain)/65536;
manager_balance_offset[L_KNEE]=-(((fb_value*manager_servos[L_KNEE].max_angle)/manager_servos[L_KNEE].encoder_resolution)*knee_gain)/65536;
manager_balance_offset[L_ANKLE_PITCH]=-(((fb_value*manager_servos[L_ANKLE_PITCH].max_angle)/manager_servos[L_ANKLE_PITCH].encoder_resolution)*ankle_pitch_gain)/65536;
manager_balance_offset[R_HIP_ROLL]=(((lr_value*manager_servos[R_HIP_ROLL].max_angle)/manager_servos[R_HIP_ROLL].encoder_resolution)*hip_roll_gain)/65536;
manager_balance_offset[L_HIP_ROLL]=(((lr_value*manager_servos[L_HIP_ROLL].max_angle)/manager_servos[L_HIP_ROLL].encoder_resolution)*hip_roll_gain)/65536;
manager_balance_offset[R_ANKLE_ROLL]=-(((lr_value*manager_servos[R_ANKLE_ROLL].max_angle)/manager_servos[R_ANKLE_ROLL].encoder_resolution)*ankle_roll_gain)/65536;
manager_balance_offset[L_ANKLE_ROLL]=-(((lr_value*manager_servos[L_ANKLE_ROLL].max_angle)/manager_servos[L_ANKLE_ROLL].encoder_resolution)*ankle_roll_gain)/65536;
manager_balance_offset[R_KNEE]=(((fb_value*manager_servos[R_KNEE].max_angle)/manager_servos[R_KNEE].encoder_resolution)*knee_gain)>>16;
manager_balance_offset[R_ANKLE_PITCH]=(((fb_value*manager_servos[R_ANKLE_PITCH].max_angle)/manager_servos[R_ANKLE_PITCH].encoder_resolution)*ankle_pitch_gain)>>16;
manager_balance_offset[L_KNEE]=-(((fb_value*manager_servos[L_KNEE].max_angle)/manager_servos[L_KNEE].encoder_resolution)*knee_gain)>>16;
manager_balance_offset[L_ANKLE_PITCH]=-(((fb_value*manager_servos[L_ANKLE_PITCH].max_angle)/manager_servos[L_ANKLE_PITCH].encoder_resolution)*ankle_pitch_gain)>>16;
manager_balance_offset[R_HIP_ROLL]=(((lr_value*manager_servos[R_HIP_ROLL].max_angle)/manager_servos[R_HIP_ROLL].encoder_resolution)*hip_roll_gain)>>16;
manager_balance_offset[L_HIP_ROLL]=(((lr_value*manager_servos[L_HIP_ROLL].max_angle)/manager_servos[L_HIP_ROLL].encoder_resolution)*hip_roll_gain)>>16;
manager_balance_offset[R_ANKLE_ROLL]=-(((lr_value*manager_servos[R_ANKLE_ROLL].max_angle)/manager_servos[R_ANKLE_ROLL].encoder_resolution)*ankle_roll_gain)>>16;
manager_balance_offset[L_ANKLE_ROLL]=-(((lr_value*manager_servos[L_ANKLE_ROLL].max_angle)/manager_servos[L_ANKLE_ROLL].encoder_resolution)*ankle_roll_gain)>>16;
}
}
......@@ -184,7 +186,7 @@ void manager_init(uint16_t period_us)
dyn_master_scan(&bioloid_dyn_master_servos,&num,servo_ids);
ram_data[BIOLOID_MM_NUM_SERVOS]=num;
manager_num_servos=0;
/* for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
{
if(i==servo_ids[manager_num_servos])
{
......@@ -289,7 +291,7 @@ void manager_init(uint16_t period_us)
manager_servos[i].ccw_angle_limit=0;
}
}
bioloid_dyn_master_servos_disable_power();*/
bioloid_dyn_master_servos_disable_power();
/* configure timer */
ENABLE_MANAGER_TIMER_CLK;
......@@ -298,27 +300,27 @@ void manager_init(uint16_t period_us)
MANAGER_TIM_Handle.Init.Prescaler = 84;
MANAGER_TIM_Handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
MANAGER_TIM_Handle.Init.CounterMode = TIM_COUNTERMODE_UP;
// HAL_NVIC_SetPriority(MANAGER_TIMER_IRQn, 2, 1);
// HAL_NVIC_EnableIRQ(MANAGER_TIMER_IRQn);
HAL_NVIC_SetPriority(MANAGER_TIMER_IRQn, 2, 1);
HAL_NVIC_EnableIRQ(MANAGER_TIMER_IRQn);
/* use the internal clock */
// sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
// HAL_TIM_ConfigClockSource(&MANAGER_TIM_Handle, &sClockSourceConfig);
// HAL_TIM_OC_Init(&MANAGER_TIM_Handle);
sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
HAL_TIM_ConfigClockSource(&MANAGER_TIM_Handle, &sClockSourceConfig);
HAL_TIM_OC_Init(&MANAGER_TIM_Handle);
/* disable master/slave mode */
// sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
// sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
// HAL_TIMEx_MasterConfigSynchronization(&MANAGER_TIM_Handle, &sMasterConfig);
sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
HAL_TIMEx_MasterConfigSynchronization(&MANAGER_TIM_Handle, &sMasterConfig);
/* configure ouptut counter channel 4 */
// manager_motion_period=(period_us*1000000)>>16;
// manager_motion_period_us=period_us;
manager_motion_period=(period_us<<16)/1000000;
manager_motion_period_us=period_us;
/* initialize balance parameters */
// for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
// manager_balance_offset[i]=0;
// manager_balance_enabled=0x00;
for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
manager_balance_offset[i]=0;
manager_balance_enabled=0x00;
/* initialize action module */
// action_init(period_us);
action_init(period_us);
}
uint16_t manager_get_period(void)
......
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