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