From 863a137d1e2f2a9c14e0152c3893d1edb50fe2c8 Mon Sep 17 00:00:00 2001 From: Luiz-Fernando Lavado-Villa <lflavado@tu-te-calmes> Date: Thu, 27 Jan 2022 23:38:40 +0100 Subject: [PATCH] Latest changes from the Hackathon in Lyon --- src/opalib_control_pid.c | 154 ++++++++++++++++++++++++++++++++++++--- src/opalib_control_pid.h | 45 +++++++++++- 2 files changed, 187 insertions(+), 12 deletions(-) diff --git a/src/opalib_control_pid.c b/src/opalib_control_pid.c index 235bb0a..3d87d6a 100644 --- a/src/opalib_control_pid.c +++ b/src/opalib_control_pid.c @@ -49,9 +49,9 @@ ///// // Local variables -static float32_t saturation_interleaved, saturation_leg1, saturation_leg2; // Represent the minimum voltage required for the entry to suit the Vref +static float32_t saturation_interleaved, saturation_leg1, saturation_leg2, saturation_motor; // Represent the minimum voltage required for the entry to suit physical limits -// Interleaved PID variables +// Interleaved PID variables------------------------------------------------------------------------------------------------------ static arm_pid_instance_f32 PID_variables; // PID structure static float32_t error_pid; // PID error static uint32_t pid_period_us; // Period duration of the PID calculation loop in µs (used for Ki calculation) @@ -65,8 +65,16 @@ static float32_t pid_out_windUp = 0.1; // Stores the current pid output aft static float32_t WindUp_sub, WindUp_mult; // Transition variables for arm calculation of the WindUp float32_t Kw = 0.000143; // WindUp Parameter +// PWM variable +static float32_t pwm_duty_cycle = 0.1; // PWM initialization duty cycle value for the interleaved mode + +// Saturation Variables +static float32_t saturation_pid_interleaved_low = 0; +static float32_t saturation_pid_interleaved_high = 1; +//-------------------------------------------------------------------------------------------------------------------------------- -// Leg1 PID variables + +// Leg1 PID variables-------------------------------------------------------------------------------------------------------------- static arm_pid_instance_f32 PID_1_variables; // PID structure static float32_t error_pid_1; // PID error static uint32_t pid_1_period_us; // Period duration of the PID calculation loop in µs (used for Ki calculation) @@ -80,7 +88,16 @@ static float32_t pid_1_out_windUp = 0.1; // Stores the current pid output a static float32_t WindUp_1_sub, WindUp_1_mult; // Transition variables for arm calculation of the WindUp float32_t Kw_1 = 0.000143; // WindUp Parameter -// Leg2 PID variables +// PWM variable +static float32_t pwm_duty_cycle_1 = 0.1; // PWM initialization duty cycle value for leg 1 + +// Saturation Variables +static float32_t saturation_pid_leg1_low = 0; +static float32_t saturation_pid_leg1_high = 1; +//-------------------------------------------------------------------------------------------------------------------------------- + + +// Leg2 PID variables-------------------------------------------------------------------------------------------------------------- static arm_pid_instance_f32 PID_2_variables; // PID structure static float32_t error_pid_2; // PID error static uint32_t pid_2_period_us; // Period duration of the PID calculation loop in µs (used for Ki calculation) @@ -94,11 +111,39 @@ static float32_t pid_2_out_windUp = 0.1; // Stores the current pid output a static float32_t WindUp_2_sub, WindUp_2_mult; // Transition variables for arm calculation of the WindUp float32_t Kw_2 = 0.000143; // WindUp Parameter -// PWM variables -static float32_t pwm_duty_cycle = 0.1; // PWM initialization duty cycle value for the interleaved mode -static float32_t pwm_duty_cycle_1 = 0.1; // PWM initialization duty cycle value for leg 1 +// PWM variable static float32_t pwm_duty_cycle_2 = 0.1; // PWM initialization duty cycle value for leg 2 +// Saturation Variables +static float32_t saturation_pid_leg2_low = 0; +static float32_t saturation_pid_leg2_high = 1; + +//-------------------------------------------------------------------------------------------------------------------------------- + + + +// motor PID variables-------------------------------------------------------------------------------------------------------------- +static arm_pid_instance_f32 PID_motor_variables; // PID structure +static float32_t error_pid_motor; // PID error +static uint32_t pid_motor_period_us; // Period duration of the PID calculation loop in µs (used for Ki calculation) + +static float32_t prev_pid_motor_out = 0.1; // Stores the previous unsaturated output +static float32_t pid_motor_out; // Stores the current pid_output after saturation and anti windUp (the effective duty cycle) +static uint32_t Count_pid_motor_reset; // Counter to reset the PID when calculation is off + +// Anti-Windup variables +static float32_t pid_motor_out_windUp = 0.1; // Stores the current pid output after anti windup and before saturation +static float32_t WindUp_motor_sub, WindUp_motor_mult; // Transition variables for arm calculation of the WindUp +float32_t Kw_motor = 0.000143; // WindUp Parameter + +// PWM variable +static float32_t pwm_duty_cycle_motor = 0.1; // PWM initialization duty cycle value for leg 2 + +// Saturation variables +static float32_t saturation_pid_motor_low = 0; +static float32_t saturation_pid_motor_high = 1; +//-------------------------------------------------------------------------------------------------------------------------------- + ///// // Public Functions @@ -118,7 +163,6 @@ void opalib_control_init_interleaved_pid(float32_t kp, float32_t ki, float32_t k arm_pid_init_f32(&PID_variables, 1); saturation_interleaved = VMAX; - } /** @@ -143,7 +187,7 @@ void opalib_control_init_leg1_pid(float32_t kp, float32_t ki, float32_t kd, uint * This function initializes all the parameters * needed for the PID calculation for the buck topology */ -void opalib_control_init_leg2(float32_t kp, float32_t ki, float32_t kd, uint32_t task_period_us) +void opalib_control_init_leg2_pid(float32_t kp, float32_t ki, float32_t kd, uint32_t task_period_us) { pid_period_us = task_period_us; @@ -157,6 +201,25 @@ void opalib_control_init_leg2(float32_t kp, float32_t ki, float32_t kd, uint32_t } +/** + * This function initializes all the parameters + * needed for the PID calculation for the buck topology + */ +void opalib_control_init_motor_pid(float32_t kp, float32_t ki, float32_t kd, uint32_t motor_control_period_us, float32_t saturation_high, float32_t saturation_low) +{ + pid_motor_period_us = motor_control_period_us; + + float32_t million = 1000000; + PID_motor_variables.Kp = kp; + PID_motor_variables.Ki = ki * (pid_motor_period_us / million); + PID_motor_variables.Kd = kd; + + arm_pid_init_f32(&PID_motor_variables, 1); + saturation_motor = IMAX_INT; + +} + + /** * This function update the voltage reference of the * control task. @@ -177,6 +240,7 @@ void opalib_control_update_saturation_leg1(float32_t new_saturation_value) saturation_leg1 = new_saturation_value; } + /** * This function update the voltage reference of the * control task. @@ -186,6 +250,16 @@ void opalib_control_update_saturation_leg2(float32_t new_saturation_value) saturation_leg2 = new_saturation_value; } +/** + * This function update the voltage reference of the + * control task. + */ + +void opalib_control_update_saturation_motor(float32_t new_saturation_value) +{ + saturation_motor = new_saturation_value; +} + /** * This function does the PID calculation @@ -200,13 +274,18 @@ float32_t opalib_control_interleaved_pid_calculation(float32_t reference, float3 arm_sub_f32(&reference, &measurement, &error_pid, 1); // CALCULATING THE ERROR BASED ON THE REFERENCE pid_out = arm_pid_f32(&PID_variables, error_pid); // PID CALCULATIONS + if(pid_out>saturation_pid_interleaved_high){ + pid_out=saturation_pid_interleaved_high; + }else if(pid_out<saturation_pid_interleaved_low){ + pid_out=saturation_pid_interleaved_low; + } //PID anti WindUp saturation arm_sub_f32(&pwm_duty_cycle, &prev_pid_out, &WindUp_sub, 1); arm_mult_f32(&WindUp_sub, &Kw, &WindUp_mult, 1); arm_add_f32(&WindUp_mult, &pid_out, &pid_out_windUp, 1); PID_variables.state[2] = pid_out_windUp; - pwm_duty_cycle = pid_out_windUp; + pwm_duty_cycle = pid_out_windUp; prev_pid_out = pid_out; } @@ -227,6 +306,12 @@ float32_t opalib_control_leg1_pid_calculation(float32_t reference, float32_t mea pid_1_out = arm_pid_f32(&PID_1_variables, error_pid_1); // PID CALCULATIONS + if(pid_1_out>saturation_pid_leg1_high){ + pid_1_out=saturation_pid_leg1_high; + }else if(pid_1_out<saturation_pid_leg1_low){ + pid_1_out=saturation_pid_leg1_low; + } + //PID anti WindUp saturation arm_sub_f32(&pwm_duty_cycle_1, &prev_pid_1_out, &WindUp_1_sub, 1); arm_mult_f32(&WindUp_1_sub, &Kw_2, &WindUp_1_mult, 1); @@ -255,6 +340,12 @@ float32_t opalib_control_leg2_pid_calculation(float32_t reference, float32_t mea pid_2_out = arm_pid_f32(&PID_2_variables, error_pid_2); // PID CALCULATIONS + if(pid_2_out>saturation_pid_leg2_high){ + pid_2_out=saturation_pid_leg2_high; + }else if(pid_2_out<saturation_pid_leg2_low){ + pid_2_out=saturation_pid_leg2_low; + } + //PID anti WindUp saturation arm_sub_f32(&pwm_duty_cycle_2, &prev_pid_2_out, &WindUp_2_sub, 1); arm_mult_f32(&WindUp_2_sub, &Kw_2, &WindUp_2_mult, 1); @@ -268,6 +359,48 @@ float32_t opalib_control_leg2_pid_calculation(float32_t reference, float32_t mea return pwm_duty_cycle_2; } +/** + * This function does the PID calculation + * The PID has an anti-windup that permits to avoid + * loosing control. + */ + +float32_t opalib_control_motor_pid_calculation(float32_t reference, float32_t measurement){ + + if (measurement <= saturation_motor){ + + arm_sub_f32(&reference, &measurement, &error_pid_motor, 1); // CALCULATING THE ERROR BASED ON THE REFERENCE + pid_motor_out = arm_pid_f32(&PID_motor_variables, error_pid_motor); // PID CALCULATIONS + + if(pid_motor_out>saturation_pid_motor_high){ + pid_motor_out=saturation_pid_motor_high; + }else if(pid_motor_out<saturation_pid_motor_low){ + pid_motor_out=saturation_pid_motor_low;} + + //PID anti WindUp saturation + arm_sub_f32(&pwm_duty_cycle_motor, &prev_pid_motor_out, &WindUp_motor_sub, 1); + arm_mult_f32(&WindUp_motor_sub, &Kw_motor, &WindUp_motor_mult, 1); + arm_add_f32(&WindUp_motor_mult, &pid_motor_out, &pid_motor_out_windUp, 1); + + PID_motor_variables.state[2] = pid_motor_out_windUp; + pwm_duty_cycle_motor = pid_motor_out_windUp; + prev_pid_motor_out = pid_motor_out; + pwm_duty_cycle_motor = pid_motor_out; + } + + return pwm_duty_cycle_motor; +} + + +void opalib_control_motor_pid_kp_update(float32_t new_kp){ + PID_motor_variables.Kp = new_kp; +} + +void opalib_control_motor_pid_ki_update(float32_t new_ki){ + PID_motor_variables.Ki = new_ki; +} + + /** * This function resets the states of the pid */ @@ -276,4 +409,5 @@ void opalib_control_pid_reset_state() arm_pid_reset_f32(&PID_variables); arm_pid_reset_f32(&PID_1_variables); arm_pid_reset_f32(&PID_2_variables); + arm_pid_reset_f32(&PID_motor_variables); } diff --git a/src/opalib_control_pid.h b/src/opalib_control_pid.h index 55b4cf6..83b8c31 100644 --- a/src/opalib_control_pid.h +++ b/src/opalib_control_pid.h @@ -22,8 +22,8 @@ * @author Antoine Boche <antoine.boche@laas.fr> */ -#ifndef OPALIB_CONTROL_PID_H_ -#define OPALIB_CONTROL_PID_H_ +#ifndef OPALIB_CONTROL_H_ +#define OPALIB_CONTROL_H_ #include <arm_math.h> // adds all the CMSIS library @@ -62,6 +62,17 @@ void opalib_control_init_leg1_pid(float32_t kp, float32_t ki, float32_t kd, uint */ void opalib_control_init_leg2_pid(float32_t kp, float32_t ki, float32_t kd, uint32_t task_period_us); + +/** + * @brief This function initialize all the parameters for leg 2 + * + * @param[in] kp Proportional gain for PID calculation + * @param[in] ki Integral gain for PID calculation + * @param[in] kd Derivative gain for PID calculation + * @param[in] task_period_us the period of the control task in micro-seconds + */ +void opalib_control_init_motor_pid(float32_t kp, float32_t ki, float32_t kd, uint32_t motor_control_period_us, float32_t saturation_high, float32_t saturation_low); + /** * @brief This function update the saturation value used to protect both legs in the pid calculation * @@ -119,12 +130,42 @@ float32_t opalib_control_leg1_pid_calculation(float32_t reference, float32_t mea float32_t opalib_control_leg2_pid_calculation(float32_t reference, float32_t measurement); + +/** + * @brief This function calculates the error and calls motor pid to get an updated value for the duty cycle + * + * @param[in] voltage_reference floating point reference value to be tracked + * @param[in] measurement floating point measurement of the value used to calculate the error + * + * @return duty cycle to be sent to HRTIM peripheral + */ +float32_t opalib_control_motor_pid_calculation(float32_t reference, float32_t measurement); + + + +/** + * @brief This function updates the kp value for the pid control of the motor + * + * @param[in] new_kp floating point reference value to be tracked + */ +void opalib_control_motor_pid_kp_update(float32_t new_kp); + +/** + * @brief This function updates the ki value for the pid control of the motor + * + * @param[in] new_ki floating point reference value to be tracked + */ +void opalib_control_motor_pid_ki_update(float32_t new_ki); + + /** * @brief This function reset the pid state */ void opalib_control_pid_reset_state(void); + + #ifdef __cplusplus } #endif -- GitLab