diff --git a/include/action.h b/include/action.h index 693ccc167a525e600e8c1f30b103b23f593bf394..97315bede8b2236116dcbf23006d7760f7a061e7 100644 --- a/include/action.h +++ b/include/action.h @@ -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); diff --git a/include/motion_manager.h b/include/motion_manager.h index e6a1616b2776b95096e80f7e77504eba4cf237dc..2885584c858f9b7864c86b5c6640fb6bc2c75bdf 100644 --- a/include/motion_manager.h +++ b/include/motion_manager.h @@ -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); diff --git a/src/action.c b/src/action.c index 72f2d0ef97d396dd24e12ece3e2a4f7bf16732f9..a6e06989d829322b805c6359393269a6b1af3303 100644 --- a/src/action.c +++ b/src/action.c @@ -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) diff --git a/src/motion_manager.c b/src/motion_manager.c index 4370b172836180863b4663148d4a8c9f85a293f6..8a172e0def975f19b1e07078d6fc3b250fead857 100644 --- a/src/motion_manager.c +++ b/src/motion_manager.c @@ -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)