Commit 3bfc7923 authored by Antoine Boche's avatar Antoine Boche
Browse files

Addition of antiwindup and negative error

Addition of an anti-windup at the end of the pid calculation to
avoid PID divergence.
Correction of the negative error calculation

Tested in V1.1.2 board 2
Buck mode
Converged in 20ms
12V step response
24V Vhigh
parent 2c7aa1ea
......@@ -42,18 +42,18 @@
// ADC convertions variable
// ADC1
static float32_t raw_values_adc1[4];
static float32_t raw_values_offset_removed_adc1[4];
static float32_t converted_values_adc1[4];
static float32_t raw_values_adc1[4]={0};
static float32_t raw_values_offset_removed_adc1[4]={0};
static float32_t converted_values_adc1[4]={0};
static float32_t gain_adc1[4] = {0.0125, 0.0107, 0.0125, 0};
static float32_t offset_adc1[4] = {2030.88, 2029.065, 2017.68, 0};
// ADC2
static float32_t raw_values_adc2[3];
static float32_t raw_values_offset_removed_adc2[3];
static float32_t converted_values_adc2[3];
static float32_t gain_adc2[3] = {0.0355, 0.0355, 0.0672};
static float32_t offset_adc2[3] = {2012.11, 2011.74, 3.022};
static float32_t raw_values_adc2[3]={0};
static float32_t raw_values_offset_removed_adc2[3]={0};
static float32_t converted_values_adc2[3]={0};
static float32_t gain_adc2[3] = {0.0462, 0.0355, 0.0668};
static float32_t offset_adc2[3] = {94.04, 2011.74, -4.1708};
static float32_t gain_adc1_triple_linearization[3] = {0.0439, 0.0261, 0.0453};
static float32_t offset_adc2_triple_linearization[3] = {2036.583, 1834.98, 2160.97};
......@@ -61,6 +61,54 @@ static float32_t offset_adc2_triple_linearization[3] = {2036.583, 1834.98, 2160.
/////
// Private Functions
/**
* These functions update the paramters of the variables
*/
void v1_low_parameters_set(float32_t gain, float32_t offset)
{
gain_adc2[0] = gain;
offset_adc2[0] = offset;
}
void v2_low_parameters_set(float32_t gain, float32_t offset)
{
gain_adc2[1] = gain;
offset_adc2[1] = offset;
}
void v_high_parameters_set(float32_t gain, float32_t offset)
{
gain_adc2[2] = gain;
offset_adc2[2] = offset;
}
void i1_low_parameters_set(float32_t gain, float32_t offset)
{
gain_adc1[0] = gain;
offset_adc1[0] = offset;
}
void i2_low_parameters_set(float32_t gain, float32_t offset)
{
gain_adc1[1] = gain;
offset_adc1[1] = offset;
}
void i_high_parameters_set(float32_t gain, float32_t offset)
{
gain_adc1[2] = gain;
offset_adc1[2] = offset;
}
void temp_parameters_set(float32_t gain, float32_t offset)
{
gain_adc1[3] = gain;
offset_adc1[3] = offset;
}
/**
* This function select the right conversion equation
* depending of the voltage measured to solve the non linear response
......@@ -85,6 +133,8 @@ void triple_linerarization(float32_t value_adc, int8_t adc_channel)
}
}
/////
// Public Functions
......@@ -94,31 +144,53 @@ void triple_linerarization(float32_t value_adc, int8_t adc_channel)
*/
void data_conversion()
{
if( (data_dispatch_get_values_available_in_adc1_channel(0)!=0) &&
(data_dispatch_get_values_available_in_adc1_channel(1)!=0) &&
(data_dispatch_get_values_available_in_adc1_channel(2)!=0) &&
(data_dispatch_get_values_available_in_adc2_channel(0)!=0) &&
(data_dispatch_get_values_available_in_adc2_channel(1)!=0) &&
(data_dispatch_get_values_available_in_adc2_channel(2)!=0) )
if(
// (data_dispatch_get_values_available_in_adc1_channel(0)!=0) &&
// (data_dispatch_get_values_available_in_adc1_channel(1)!=0) &&
// (data_dispatch_get_values_available_in_adc1_channel(2)!=0) &&
(data_dispatch_get_values_available_in_adc2_channel(0)!=0) //&&
// (data_dispatch_get_values_available_in_adc2_channel(1)!=0) &&
// (data_dispatch_get_values_available_in_adc2_channel(2)!=0)
)
{
// Data gathering from the buffer and conversion to float for PID calculation
raw_values_adc1[0] = (float32_t)data_dispatch_get_next_value_from_adc1_channel(0);
raw_values_adc1[1] = (float32_t)data_dispatch_get_next_value_from_adc1_channel(1);
raw_values_adc1[2] = (float32_t)data_dispatch_get_next_value_from_adc1_channel(2);
// raw_values_adc1[0] = (float32_t)data_dispatch_get_next_value_from_adc1_channel(0);
// raw_values_adc1[1] = (float32_t)data_dispatch_get_next_value_from_adc1_channel(1);
// raw_values_adc1[2] = (float32_t)data_dispatch_get_next_value_from_adc1_channel(2);
raw_values_adc2[0] = (float32_t)data_dispatch_get_next_value_from_adc2_channel(0);
raw_values_adc2[1] = (float32_t)data_dispatch_get_next_value_from_adc2_channel(1);
raw_values_adc2[2] = (float32_t)data_dispatch_get_next_value_from_adc2_channel(2);
// raw_values_adc2[1] = (float32_t)data_dispatch_get_next_value_from_adc2_channel(1);
// raw_values_adc2[2] = (float32_t)data_dispatch_get_next_value_from_adc2_channel(2);
arm_mult_f32(&raw_values_adc2[0], &gain_adc2[0], &raw_values_offset_removed_adc2[0], 1);
arm_sub_f32(&raw_values_offset_removed_adc2[0], &offset_adc2[0], &converted_values_adc2[0], 1);
}
if(
// (data_dispatch_get_values_available_in_adc1_channel(0)!=0) &&
// (data_dispatch_get_values_available_in_adc1_channel(1)!=0) &&
// (data_dispatch_get_values_available_in_adc1_channel(2)!=0) &&
//(data_dispatch_get_values_available_in_adc2_channel(0)!=0) //&&
// (data_dispatch_get_values_available_in_adc2_channel(1)!=0) &&
(data_dispatch_get_values_available_in_adc2_channel(2)!=0)
)
{
// Data gathering from the buffer and conversion to float for PID calculation
raw_values_adc2[2] = (float32_t)data_dispatch_get_next_value_from_adc2_channel(2);
// Update of the linearization depending of the value
triple_linerarization(raw_values_adc2[0], 0);
triple_linerarization(raw_values_adc2[1], 1);
//triple_linerarization(raw_values_adc2[0], 0);
//triple_linerarization(raw_values_adc2[1], 1);
// Conversion of the raw ADC value in volt and ampere
arm_sub_f32(raw_values_adc1, offset_adc1, raw_values_offset_removed_adc1, 4);
arm_sub_f32(raw_values_adc2, offset_adc2, raw_values_offset_removed_adc2, 3);
arm_mult_f32(raw_values_offset_removed_adc1, gain_adc1, converted_values_adc1, 4);
arm_mult_f32(raw_values_offset_removed_adc2, gain_adc2, converted_values_adc2, 3);
// arm_sub_f32(raw_values_adc1, offset_adc1, raw_values_offset_removed_adc1, 4);
//arm_sub_f32(&raw_values_adc2[0], &offset_adc2[0], &raw_values_offset_removed_adc2[0], 1);
// arm_mult_f32(raw_values_offset_removed_adc1, gain_adc1, converted_values_adc1, 4);
//arm_mult_f32(&raw_values_offset_removed_adc2[0], &gain_adc2[0], &converted_values_adc2[0], 1);
arm_mult_f32(&raw_values_adc2[2], &gain_adc2[2], &raw_values_offset_removed_adc2[2], 1);
arm_sub_f32(&raw_values_offset_removed_adc2[2], &offset_adc2[2], &converted_values_adc2[2], 1);
}
}
/**
......
......@@ -44,6 +44,24 @@ void data_conversion();
*/
float32_t get_value_converted(uint8_t adc_number, uint8_t adc_channel);
void v1_low_parameters_set(float32_t gain, float32_t offset);
void v2_low_parameters_set(float32_t gain, float32_t offset);
void v_high_parameters_set(float32_t gain, float32_t offset);
void i1_low_parameters_set(float32_t gain, float32_t offset);
void i2_low_parameters_set(float32_t gain, float32_t offset);
void i_high_parameters_set(float32_t gain, float32_t offset);
void temp_parameters_set(float32_t gain, float32_t offset);
#ifdef __cplusplus
}
#endif
......
......@@ -55,9 +55,13 @@
// PID variables
static float32_t Vref;
static float32_t V1_value;
static float32_t V1_value, Vhigh_value;
static float32_t error_pid;
static arm_pid_instance_f32 PID_variables;
static float32_t pid_out_windUp = 0.1; // stores the current pid output after anti windup and before saturation
static float32_t prev_pid_out = 0.1; // stores the previous unsaturated output
static float32_t pid_out; // stores the current pid_output after saturation and anti windUp (the effective duty cycle)
float32_t Kw = 0.000143; // WindUp Parameter
// PWM variables
static float32_t pwm_duty_cycle = 0.1; // PWM initialization value
......@@ -119,28 +123,59 @@ void pid_calculation_and_pwm_update()
// PID CALCUALTIONS
V1_value = get_value_converted(2 , 0);
arm_sub_f32(&V1_value, &Vref, &error_pid, 1); // CALCULATING THE ERROR BASED ON THE REFERENCE
pwm_duty_cycle = arm_pid_f32(&PID_variables, 0.01*error_pid); // PID CALCULATIONS
// TESTING PWM VALUE TO AVOID OVERFLOW AND PWM UPDATE//
if (pwm_duty_cycle > HIGH_DUTY) // SATURATION CONDITIONS TO AVOID DIVERGENCE.
Vhigh_value = get_value_converted(2 , 2);
arm_sub_f32(&Vref, &V1_value, &error_pid, 1); // CALCULATING THE ERROR BASED ON THE REFERENCE
if (Vhigh_value > Vref)
{
pwm_pulse_width = pwm_high_pulse_width;
leg_set(TIMA, pwm_pulse_width, 0);
leg_set(TIMB, pwm_pulse_width, pwm_phase_shift);
pid_out = arm_pid_f32(&PID_variables, error_pid); // PID CALCULATIONS
//PID anti WindUp saturation
pid_out_windUp = pid_out + Kw * (pwm_duty_cycle - prev_pid_out);
PID_variables.state[2] = pid_out_windUp;
pwm_duty_cycle = pid_out_windUp;
prev_pid_out = pid_out;
// TESTING PWM VALUE TO AVOID OVERFLOW AND PWM UPDATE//
if (pwm_duty_cycle > HIGH_DUTY) // SATURATION CONDITIONS TO AVOID DIVERGENCE.
{
pwm_duty_cycle = HIGH_DUTY;
pwm_pulse_width = pwm_high_pulse_width;
leg_set(TIMA, pwm_pulse_width, 0);
leg_set(TIMB, pwm_pulse_width, pwm_phase_shift);
}
else if (pwm_duty_cycle < LOW_DUTY) // SATURATION CONDITIONS TO AVOID DIVERGENCE.
{
pwm_duty_cycle = LOW_DUTY;
pwm_pulse_width = pwm_low_pulse_width;
leg_set(TIMA, pwm_pulse_width, 0);
leg_set(TIMB, pwm_pulse_width, pwm_phase_shift);
}
else
{
pwm_pulse_width = (pwm_duty_cycle * pwm_period);
leg_set(TIMA, pwm_pulse_width, 0);
leg_set(TIMB, pwm_pulse_width, pwm_phase_shift);
}
}
else if (pwm_duty_cycle < LOW_DUTY) // SATURATION CONDITIONS TO AVOID DIVERGENCE.
{
pwm_pulse_width = pwm_low_pulse_width;
leg_set(TIMA, pwm_pulse_width, 0);
leg_set(TIMB, pwm_pulse_width, pwm_phase_shift);
}
else
{
pwm_pulse_width = (pwm_duty_cycle * pwm_period);
leg_set(TIMA, pwm_pulse_width, 0);
leg_set(TIMB, pwm_pulse_width, pwm_phase_shift);
}
arm_pid_reset_q31(&PID_variables);
pwm_duty_cycle = LOW_DUTY;
leg_set(TIMA, pwm_low_pulse_width, 0);
leg_set(TIMB, pwm_low_pulse_width, pwm_phase_shift);
}
}
void test()
{
data_conversion();
pwm_pulse_width = (0.5 * pwm_period);
leg_set(TIMA, pwm_pulse_width, 0);
leg_set(TIMB, pwm_pulse_width, pwm_phase_shift);
}
\ No newline at end of file
......@@ -59,6 +59,7 @@ void pid_init_boost(float32_t vref, float32_t kp, float32_t ki, float32_t kd);
*/
void pid_calculation_and_pwm_update();
void test();
#ifdef __cplusplus
}
......
Markdown is supported
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