From 6a5c6f8001361622fb61899d4ecbc5f5657b6fec Mon Sep 17 00:00:00 2001
From: Martin Dittrich <103326240+martind69@users.noreply.github.com>
Date: Sat, 9 Apr 2022 18:24:09 +0200
Subject: [PATCH] Update PIDController.cpp

Added Failsafe to return unchanged calculation to prevent divsion by 0 what results in NaN
Added Adaptive Ki factor to stabilize the controller at higher sensor values
---
 src/PIDController.cpp | 24 ++++++++++++++++++++----
 1 file changed, 20 insertions(+), 4 deletions(-)

diff --git a/src/PIDController.cpp b/src/PIDController.cpp
index 2d9523e..94b16e2 100644
--- a/src/PIDController.cpp
+++ b/src/PIDController.cpp
@@ -26,8 +26,10 @@ PIDController::PIDController () {
 void PIDController::begin () {
   Kp = 1;
   Ki = 1;
+  aKi = 1;
   Kd = 1;
   divisor = 10;
+  aKiDivisor = 1000;
   doLimit = false;
   init = true;
 }
@@ -64,6 +66,11 @@ void PIDController::minimize (double newMinimize) {
   divisor = newMinimize;
 }
 
+void PIDController::adaptiveRetune (double newFactor) {
+  aKiDivisor = newValue;
+  doAdaption = true;
+}
+
 // Getters
 double PIDController::getOutput () {
   return output;
@@ -78,19 +85,28 @@ double PIDController::compute (double sensor, String graph, String verbose) {
   if (!init) return 0;
 
   // Calculate time difference since last time executed
-  unsigned long now = millis();
+  unsigned long now = micros();
   double timeChange = (double)(now - lastTime);
+  
+  // Failsafe, return unchanged to prevent divide by 0 what results in NaN
+  if (timeChange < 1.0) return output;
 
   // Calculate error (P, I and D)
-  double error = setPoint - sensor;
+  error = setPoint - sensor;
+  
+  // Calculate adaptive Ki
+  if (doAdaption) {
+	aKi = setPoint / aKiDivisor;
+  }
+  
   errSum += error * timeChange;
   if (doLimit) {
     errSum = constrain(errSum, minOut * 1.1, maxOut * 1.1); 
   }
-  double dErr = (error - lastErr) / timeChange;
+  dErr = (error - lastErr) / timeChange;
 
   // Calculate the new output by adding all three elements together
-  double newOutput = (Kp * error + Ki * errSum + Kd * dErr) / divisor;
+  double newOutput = (Kp * error + Ki * aKi * errSum + Kd * dErr) / divisor;
 
   // If limit is specifyed, limit the output
   if (doLimit) {