Skip to content
Snippets Groups Projects
Commit 863a137d authored by Luiz-Fernando Lavado-Villa's avatar Luiz-Fernando Lavado-Villa
Browse files

Latest changes from the Hackathon in Lyon

parent 58adb4cd
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/opalib_control_pid.c
+ 144
− 10
View file @ 863a137d
...@@ -49,9 +49,9 @@ ...@@ -49,9 +49,9 @@
///// /////
// Local variables // 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 arm_pid_instance_f32 PID_variables; // PID structure
static float32_t error_pid; // PID error 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) 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 ...@@ -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 static float32_t WindUp_sub, WindUp_mult; // Transition variables for arm calculation of the WindUp
float32_t Kw = 0.000143; // WindUp Parameter 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 arm_pid_instance_f32 PID_1_variables; // PID structure
static float32_t error_pid_1; // PID error 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) 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 ...@@ -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 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 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 arm_pid_instance_f32 PID_2_variables; // PID structure
static float32_t error_pid_2; // PID error 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) 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 ...@@ -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 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 float32_t Kw_2 = 0.000143; // WindUp Parameter
// PWM variables // PWM variable
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
static float32_t pwm_duty_cycle_2 = 0.1; // PWM initialization duty cycle value for leg 2 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 // Public Functions
...@@ -118,7 +163,6 @@ void opalib_control_init_interleaved_pid(float32_t kp, float32_t ki, float32_t k ...@@ -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); arm_pid_init_f32(&PID_variables, 1);
saturation_interleaved = VMAX; saturation_interleaved = VMAX;
} }
/** /**
...@@ -143,7 +187,7 @@ void opalib_control_init_leg1_pid(float32_t kp, float32_t ki, float32_t kd, uint ...@@ -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 * This function initializes all the parameters
* needed for the PID calculation for the buck topology * 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; 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 ...@@ -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 * This function update the voltage reference of the
* control task. * control task.
...@@ -177,6 +240,7 @@ void opalib_control_update_saturation_leg1(float32_t new_saturation_value) ...@@ -177,6 +240,7 @@ void opalib_control_update_saturation_leg1(float32_t new_saturation_value)
saturation_leg1 = new_saturation_value; saturation_leg1 = new_saturation_value;
} }
/** /**
* This function update the voltage reference of the * This function update the voltage reference of the
* control task. * control task.
...@@ -186,6 +250,16 @@ void opalib_control_update_saturation_leg2(float32_t new_saturation_value) ...@@ -186,6 +250,16 @@ void opalib_control_update_saturation_leg2(float32_t new_saturation_value)
saturation_leg2 = 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 * This function does the PID calculation
...@@ -200,13 +274,18 @@ float32_t opalib_control_interleaved_pid_calculation(float32_t reference, float3 ...@@ -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 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 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 //PID anti WindUp saturation
arm_sub_f32(&pwm_duty_cycle, &prev_pid_out, &WindUp_sub, 1); arm_sub_f32(&pwm_duty_cycle, &prev_pid_out, &WindUp_sub, 1);
arm_mult_f32(&WindUp_sub, &Kw, &WindUp_mult, 1); arm_mult_f32(&WindUp_sub, &Kw, &WindUp_mult, 1);
arm_add_f32(&WindUp_mult, &pid_out, &pid_out_windUp, 1); arm_add_f32(&WindUp_mult, &pid_out, &pid_out_windUp, 1);
PID_variables.state[2] = pid_out_windUp; PID_variables.state[2] = pid_out_windUp;
pwm_duty_cycle = pid_out_windUp; pwm_duty_cycle = pid_out_windUp;
prev_pid_out = pid_out; prev_pid_out = pid_out;
} }
...@@ -227,6 +306,12 @@ float32_t opalib_control_leg1_pid_calculation(float32_t reference, float32_t mea ...@@ -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 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 //PID anti WindUp saturation
arm_sub_f32(&pwm_duty_cycle_1, &prev_pid_1_out, &WindUp_1_sub, 1); 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); 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 ...@@ -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 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 //PID anti WindUp saturation
arm_sub_f32(&pwm_duty_cycle_2, &prev_pid_2_out, &WindUp_2_sub, 1); 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); 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 ...@@ -268,6 +359,48 @@ float32_t opalib_control_leg2_pid_calculation(float32_t reference, float32_t mea
return pwm_duty_cycle_2; 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 * This function resets the states of the pid
*/ */
...@@ -276,4 +409,5 @@ void opalib_control_pid_reset_state() ...@@ -276,4 +409,5 @@ void opalib_control_pid_reset_state()
arm_pid_reset_f32(&PID_variables); arm_pid_reset_f32(&PID_variables);
arm_pid_reset_f32(&PID_1_variables); arm_pid_reset_f32(&PID_1_variables);
arm_pid_reset_f32(&PID_2_variables); arm_pid_reset_f32(&PID_2_variables);
arm_pid_reset_f32(&PID_motor_variables);
} }
src/opalib_control_pid.h
+ 43
− 2
View file @ 863a137d
...@@ -22,8 +22,8 @@ ...@@ -22,8 +22,8 @@
* @author Antoine Boche <antoine.boche@laas.fr> * @author Antoine Boche <antoine.boche@laas.fr>
*/ */
#ifndef OPALIB_CONTROL_PID_H_ #ifndef OPALIB_CONTROL_H_
#define OPALIB_CONTROL_PID_H_ #define OPALIB_CONTROL_H_
#include <arm_math.h> // adds all the CMSIS library #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 ...@@ -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); 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 * @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 ...@@ -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); 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 * @brief This function reset the pid state
*/ */
void opalib_control_pid_reset_state(void); void opalib_control_pid_reset_state(void);
#ifdef __cplusplus #ifdef __cplusplus
} }
#endif #endif
......
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