v07 pid and negative voltage limit change

now the voltage limits are from -5 to +12V for the output and the pid loop is now implemented, but not tested yet

Core/Application/MAIN_MainApplication.c

@@ -57,7 +57,7 @@
 //=================================================================================================
 
 /* Software Version */
-#define SW_VERSION                  6
+#define SW_VERSION                  			7
 
 #define MSG_QUEUE_SIZE							8
 

Core/Application/PECO_PeltierController.c

@@ -56,6 +56,8 @@
 
 #define EVENT_FLAGS_ALL (EVENT_TIMER_UPDATE)
 
+#define MAX_POWER_PELTIER 100
+
 //=================================================================================================
 // Section:       MACROS
 // Description:   Definition of local macros (visible by this module only).
@@ -202,8 +204,14 @@ VOID vTask(PVOID arg)
   UNUSED(arg);
   U32 u32Flags;
 
-  FLOAT last_error = 0;
-  FLOAT integral = 0;
+  FLOAT error_prev = 0;
+  FLOAT Tc_prev = 0;
+  FLOAT i_prev = 0;
+  FLOAT d_prev = 0;
+
+  FLOAT tau = 0.4f;
+
+
   FLOAT dT = REFRESH / 1000.0f; // 0.2s
 
   while (TRUE)
@@ -223,60 +231,80 @@ VOID vTask(PVOID arg)
         {
           // TODO: check the measurements, everything ok?
 
+          FLOAT power = VARH_flGetVariableData(VARH_ePeltier_P);
+          if(power > MAX_POWER_PELTIER){
+        	  PECO_Enable(FALSE);
+          }
+
 
           // PID Regelung
 
-          FLOAT kp = VARH_flGetVariableData(VARH_ePID_kp);
-          FLOAT ki = VARH_flGetVariableData(VARH_ePID_ki);
-          FLOAT kd = VARH_flGetVariableData(VARH_ePID_kd);
-          FLOAT max = VARH_flGetVariableData(VARH_ePID_Max);
-          FLOAT min = VARH_flGetVariableData(VARH_ePID_Min);
+          FLOAT Kp = VARH_flGetVariableData(VARH_ePID_kp);
+          FLOAT Ki = VARH_flGetVariableData(VARH_ePID_ki);
+          FLOAT Kd = VARH_flGetVariableData(VARH_ePID_kd);
+          FLOAT Umax = VARH_flGetVariableData(VARH_ePID_Max);
+          FLOAT Umin = VARH_flGetVariableData(VARH_ePID_Min);
 
-          FLOAT Tnow = VARH_flGetVariableData(VARH_eTemp_Module);
-          FLOAT Tset = VARH_flGetVariableData(VARH_ePID_Temp);
+          FLOAT Tc = VARH_flGetVariableData(VARH_eTemp_Module);
+          FLOAT Ts = VARH_flGetVariableData(VARH_ePID_Temp);
 
-          // -error
-          FLOAT error = Tnow - Tset;
+          // calculate error
+          FLOAT error = Tc - Ts;
 
           // proportional term
-          FLOAT P = kp * error;
+          FLOAT P = Kp * error;
 
           // integral term
-          integral += error * dT;
-          FLOAT I = ki * integral;
+          FLOAT I = i_prev + 0.5f * Ki * dT * (error + error_prev);
+
+          // compute integral limits (anti-windup via dynamic integrator clamping)
+          FLOAT limMinInt = 0.0f;
+          FLOAT limMaxInt = 0.0f;
+
+          if(Umax > P) limMaxInt = Umax - P;
+          if(Umin < P) limMinInt = Umin - P;
+
+          // clamp integrator
+          if(I > limMaxInt) I = limMaxInt;
+          else if (I < limMinInt) I = limMinInt;
 
           // derivative term
-          FLOAT D = kd * (error - last_error) / dT;
-          last_error = error;
+          FLOAT D = -(2.0f * Kd * (Tc - Tc_prev)	/* Note: derivative on measurement, therefore minus sign in front of equation! */
+			+ (2.0f * tau - dT) * d_prev)
+			/ (2.0f * tau + dT);
 
           // total
-          FLOAT output = P + I + D;
+          FLOAT Uout = P + I + D;
 
           // limitter
-          if (output > max)
-          {
-            output = max;
-            integral = 0;
-          }
-          else if (output < min)
-          {
-            output = min;
-            integral = 0;
-          }
+          if (Uout > Umax) Uout = Umax;
+          else if (Uout < Umin) Uout = Umin;
 
-          boSetPeltierVoltage(output); // set the output
+          Tc_prev = Tc;
+          error_prev = error;
+          d_prev = D;
+          i_prev = I;
+
+          boSetPeltierVoltage(Uout); // set the output
         }
         else if (VARH_u32GetVariableData(VARH_eMode) == PECO_eConst_Voltage)
         {
           boSetPeltierVoltage(VARH_flGetVariableData(VARH_eControlVoltage)); // set the output
-          last_error = 0;
-          integral = 0;
+
+          // reset PID Vars
+          error_prev = 0;
+          Tc_prev = 0;
+          i_prev = 0;
+          d_prev = 0;
         }
       }
       else
       {
-        last_error = 0;
-        integral = 0;
+    	// reset PID vars
+    	error_prev = 0;
+		Tc_prev = 0;
+		i_prev = 0;
+		d_prev = 0;
       }
     }
   }
@@ -294,8 +322,8 @@ BOOL boSetPeltierVoltage(FLOAT Voltage)
 
   if (Voltage > 12)
     Voltage = 12;
-  if (Voltage < -3)
-    Voltage = -3;
+  if (Voltage < -5)
+    Voltage = -5;
 
   boOK &= ANPO_boSetVoltage(Voltage);
 

Core/Application/VARH_VariableHandler.c

@@ -80,7 +80,7 @@ LOCAL CONST VARH_StVarInfo m_astVarInfo[VARH_eNumberOfVariables] =
 { 
   { VARH_FLAGINFO_NONE, (VARH_UVariable)(U32)1, (VARH_UVariable)(U32)0, (VARH_UVariable)(U32)1}, // VARH_eMode
 
-  { VARH_FLAGINFO_FLOAT, (VARH_UVariable)0.0f, (VARH_UVariable)-2.0f, (VARH_UVariable)12.0f}, // VARH_eControlVoltage
+  { VARH_FLAGINFO_FLOAT, (VARH_UVariable)0.0f, (VARH_UVariable)-5.0f, (VARH_UVariable)12.0f}, // VARH_eControlVoltage
 
   { VARH_FLAGINFO_FLASH | VARH_FLAGINFO_FLOAT, (VARH_UVariable)0.0f, (VARH_UVariable)0.0f, (VARH_UVariable)100.0f }, // VARH_ePID_kp
   { VARH_FLAGINFO_FLASH | VARH_FLAGINFO_FLOAT, (VARH_UVariable)0.0f, (VARH_UVariable)0.0f, (VARH_UVariable)100.0f }, // VARH_ePID_ki

Core/Src/main.c

@@ -691,6 +691,8 @@ void vDefaultTask(void *argument)
 void Error_Handler(void)
 {
   /* USER CODE BEGIN Error_Handler_Debug */
+
+
   /* User can add his own implementation to report the HAL error return state */
   __disable_irq();
   while (1)