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