From 84f2253f9893dea387f0603603a8d59d81fe2b53 Mon Sep 17 00:00:00 2001 From: Carl Ljungholm Date: Tue, 20 Feb 2024 11:13:38 +0100 Subject: [PATCH] Limit m_speed_pid_set_rpm to achievable RPM. If m_speed_pid_set_rpm is allowed to exceed foc_openloop_rpm before index is found, then there will be a jump in torque when index is found. If m_speed_pid_set_rpm is allowed to exceed [l_min_erpm, l_max_erpm] then there is a wind up effect since it will take time for m_speed_pid_set_rpm to return to valid range due to ramp limit. --- motor/foc_math.c | 6 +++++- motor/foc_math.h | 2 +- motor/mcpwm_foc.c | 9 +++++---- 3 files changed, 11 insertions(+), 6 deletions(-) diff --git a/motor/foc_math.c b/motor/foc_math.c index 64204a93..e3b33439 100644 --- a/motor/foc_math.c +++ b/motor/foc_math.c @@ -444,7 +444,7 @@ void foc_run_pid_control_pos(bool index_found, float dt, motor_all_state_t *moto } } -void foc_run_pid_control_speed(float dt, motor_all_state_t *motor) { +void foc_run_pid_control_speed(bool index_found, float dt, motor_all_state_t *motor) { mc_configuration *conf_now = motor->m_conf; float p_term; float d_term; @@ -459,6 +459,10 @@ void foc_run_pid_control_speed(float dt, motor_all_state_t *motor) { if (conf_now->s_pid_ramp_erpms_s > 0.0) { utils_step_towards((float*)&motor->m_speed_pid_set_rpm, motor->m_speed_command_rpm, conf_now->s_pid_ramp_erpms_s * dt); + if (!index_found) { + utils_truncate_number_abs(&motor->m_speed_pid_set_rpm, conf_now->foc_openloop_rpm); + } + utils_truncate_number(&motor->m_speed_pid_set_rpm, conf_now->l_min_erpm, conf_now->l_max_erpm); } float rpm = 0.0; diff --git a/motor/foc_math.h b/motor/foc_math.h index aa906eba..ac8574d9 100644 --- a/motor/foc_math.h +++ b/motor/foc_math.h @@ -243,7 +243,7 @@ void foc_pll_run(float phase, float dt, float *phase_var, void foc_svm(float alpha, float beta, uint32_t PWMFullDutyCycle, uint32_t* tAout, uint32_t* tBout, uint32_t* tCout, uint32_t *svm_sector); void foc_run_pid_control_pos(bool index_found, float dt, motor_all_state_t *motor); -void foc_run_pid_control_speed(float dt, motor_all_state_t *motor); +void foc_run_pid_control_speed(bool index_found, float dt, motor_all_state_t *motor); float foc_correct_encoder(float obs_angle, float enc_angle, float speed, float sl_erpm, motor_all_state_t *motor); float foc_correct_hall(float angle, float dt, motor_all_state_t *motor, int hall_val); void foc_run_fw(motor_all_state_t *motor, float dt); diff --git a/motor/mcpwm_foc.c b/motor/mcpwm_foc.c index cf8b75fd..73e1b638 100644 --- a/motor/mcpwm_foc.c +++ b/motor/mcpwm_foc.c @@ -4169,11 +4169,12 @@ static THD_FUNCTION(pid_thread, arg) { float dt = timer_seconds_elapsed_since(last_time); last_time = timer_time_now(); - foc_run_pid_control_pos(encoder_index_found(), dt, (motor_all_state_t*)&m_motor_1); - foc_run_pid_control_speed(dt, (motor_all_state_t*)&m_motor_1); + bool index_found = encoder_index_found(); + foc_run_pid_control_pos(index_found, dt, (motor_all_state_t*)&m_motor_1); + foc_run_pid_control_speed(index_found, dt, (motor_all_state_t*)&m_motor_1); #ifdef HW_HAS_DUAL_MOTORS - foc_run_pid_control_pos(encoder_index_found(), dt, (motor_all_state_t*)&m_motor_2); - foc_run_pid_control_speed(dt, (motor_all_state_t*)&m_motor_2); + foc_run_pid_control_pos(index_found, dt, (motor_all_state_t*)&m_motor_2); + foc_run_pid_control_speed(index_found, dt, (motor_all_state_t*)&m_motor_2); #endif } }