From 69e0de0fc8135bc8f8fff5347a1fb1316c8d6fce Mon Sep 17 00:00:00 2001
From: Stefan Jahn <stefan@gruft.de>
Date: Thu, 29 Dec 2022 21:22:47 +0100
Subject: [PATCH] Improved PID regulator functionality a little, added
 documentation

---
 pid.cc | 60 ++++++++++++++++++++++++++++++++++++++++++++++++++--------
 pid.h  |  4 +++-
 2 files changed, 55 insertions(+), 9 deletions(-)

diff --git a/pid.cc b/pid.cc
index 13a1f62..775169d 100644
--- a/pid.cc
+++ b/pid.cc
@@ -1,5 +1,7 @@
 /*
-  PID regulator.
+  The file implements a simple PID regulator.  The time resolution is
+  1usec.  The update function should be called regularily as often as
+  possible.
  */
 
 #include <stdio.h>
@@ -7,6 +9,11 @@
 #include <sys/time.h>
 #include "pid.h"
 
+/*
+  This this the class constructor.  The min/max values are the minimum
+  and maximum values output by the regulator.  The three regulator
+  parameters must be adjusted by experiments.
+ */
 PID::PID(double min, double max, double kp, double kd, double ki) {
   Min = min;
   Max = max;
@@ -15,13 +22,20 @@ PID::PID(double min, double max, double kp, double kd, double ki) {
   Ki = ki;
   Integral = 0;
   PreviousError = 0;
-  PreviousTime = getTime();
+  PreviousTime = GetTime();
 }
 
+/*
+  Class destructor.
+ */
 PID::~PID() {
 }
 
-int64_t PID::getTime(void) {
+/*
+  A static function to obtain a timestamp for the integrative/derivative
+  parts of the regulator.
+ */
+int64_t PID::GetTime(void) {
 
   struct timeval current_time;
   int64_t t;
@@ -31,8 +45,24 @@ int64_t PID::getTime(void) {
   return t;
 }
 
+/*
+  The function can be used to re-start the PID regulator.  Re-setting
+  the integral and setting the current time as starting point.
+ */
+void PID::Start(void) {
+  Integral = 0;
+  PreviousError = 0;
+  PreviousTime = GetTime();
+}
+
+/*
+  This is the update function of the PID regulator.  Passing the
+  target value of the regulator, the current process value and the
+  timestamp when the value was obtained.
+ */
 double PID::Update(double target, double value, int64_t time) {
-  double error, out, dt;
+  double error, out;
+  int64_t dt;
   double P, I, D;
   double Derivative;
 
@@ -47,14 +77,19 @@ double PID::Update(double target, double value, int64_t time) {
   Integral += error * dt;
   I = Ki * Integral;
 
-  // derivative part
-  Derivative = (error - PreviousError) / dt;
-  D = Kd * Derivative;
+  if (dt > 0) {
+    // derivative part
+    Derivative = (error - PreviousError) / dt;
+    D = Kd * Derivative;
+  }
+  else {
+    D = 0;
+  }
 
   // calculate output of regulator
   out = P + I + D;
   
-  // Min/Max
+  // Min/Max limitations of the regulator output
   if (out > Max) out = Max;
   if (out < Min) out = Min;
 
@@ -64,3 +99,12 @@ double PID::Update(double target, double value, int64_t time) {
 
   return out;
 }
+
+/*
+  This is a shortcut function of the update function without passing
+  the timestamp.  The timestamp will be the time when the fuction is
+  called.
+ */
+double PID::Update(double target, double value) {
+  return Update(target, value, GetTime());
+}
diff --git a/pid.h b/pid.h
index f10048d..d31160f 100644
--- a/pid.h
+++ b/pid.h
@@ -16,8 +16,10 @@ class PID {
   PID(double min, double max, double kp, double kd, double ki);
   ~PID();
 
+  void Start(void);
   double Update(double target, double value, int64_t time);
-  static int64_t getTime(void);
+  double Update(double target, double value);
+  static int64_t GetTime(void);
 };
 
 #endif