update pid, work version

.gitattributes

@@ -0,0 +1,2 @@
+# Auto detect text files and perform LF normalization
+* text=auto

PID.c

@@ -1,55 +1,38 @@
 #include "PID.h"
 
-/*test push*/
+void PIDController_Init(PIDController *pid) {
 
-void pidControllerInit(PidController *pid, float Kp, float Ki, float Kd, float tau, float limMin, float limMax,
+	/* Clear controller variables */
-                       float limMinInt, float limMaxInt, float T) {
+	pid->integrator = 0.0f;
-    pid->Kp = Kp;
+	pid->prevError  = 0.0f;
-    pid->Ki = Ki;
-    pid->Kd = Kd;
-    pid->KpDefault = Kp;
-    pid->KiDefault = Ki;
-    pid->KdDefault = Kd;
 
-    pid->tau = tau;
+	pid->differentiator  = 0.0f;
-    pid->limMin = limMin;
+	pid->prevMeasurement = 0.0f;
-    pid->limMax = limMax;
-    pid->limMinInt = limMinInt;
-    pid->limMaxInt = limMaxInt;
-    pid->T = T;
 
-    /* Clear controller variables */
+	pid->out = 0.0f;
-    pid->integrator = 0.0f;
-    pid->prevError = 0.0f;
 
-    pid->differentiator = 0.0f;
-    pid->prevMeasurement = 0.0f;
-
-    pid->out = 0.0f;
-
-    pid->enable = false;
 }
 
-float pidControllerUpdate(PidController *pid, float setpoint, float measurement) {
+float PIDController_Update(PIDController *pid, float setpoint, float measurement) {
 
-    /*
+	/*
-    * Error signal
+	* Error signal
-    */
+	*/
     float error = setpoint - measurement;
 
 
-    /*
+	/*
-    * Proportional
+	* Proportional
-    */
+	*/
     pid->proportional = pid->Kp * error;
 
 
-    /*
+	/*
-    * Integral
+	* Integral
-    */
+	*/
     pid->integrator = pid->integrator + 0.5f * pid->Ki * pid->T * (error + pid->prevError);
 
-    /* Anti-wind-up via integrator clamping */
+	/* Anti-wind-up via integrator clamping */
     if (pid->integrator > pid->limMaxInt) {
 
         pid->integrator = pid->limMaxInt;
@@ -61,30 +44,19 @@ float pidControllerUpdate(PidController *pid, float setpoint, float measurement)
     }
 
 
-    /*
+	/*
-    * Derivative (band-limited differentiator)
+	* Derivative (band-limited differentiator)
-    */
+	*/
-
+		
-    pid->differentiator = -(2.0f * pid->Kd * (measurement -
+    pid->differentiator = -(2.0f * pid->Kd * (measurement - pid->prevMeasurement)	/* Note: derivative on measurement, therefore minus sign in front of equation! */
-                                              pid->prevMeasurement)    /* Note: derivative on measurement, therefore minus sign in front of equation! */
+                        + (2.0f * pid->tau - pid->T) * pid->differentiator)
-                            + (2.0f * pid->tau - pid->T) * pid->differentiator)
+                        / (2.0f * pid->tau + pid->T);
-                          / (2.0f * pid->tau + pid->T);
 
 
-    /*
+	/*
-    * Compute output and apply limits
+	* Compute output and apply limits
-    */
+	*/
-    pid->out = pid->proportional + pid->integrator + pid->differentiator;
+    pid->out = pid->offset + pid->proportional + pid->integrator + pid->differentiator;
-
-    if (pid->proportional > pid->limMax) {
-
-        pid->proportional = pid->limMax;
-
-    } else if (pid->proportional < pid->limMin) {
-
-        pid->proportional = pid->limMin;
-
-    }
 
     if (pid->out > pid->limMax) {
 
@@ -96,10 +68,11 @@ float pidControllerUpdate(PidController *pid, float setpoint, float measurement)
 
     }
 
-    /* Store error and measurement for later use */
+	/* Store error and measurement for later use */
-    pid->prevError = error;
+    pid->prevError       = error;
     pid->prevMeasurement = measurement;
 
-    /* Return controller output */
+	/* Return controller output */
     return pid->out;
+
 }

PID.h

@@ -1,51 +1,43 @@
 #ifndef PID_CONTROLLER_H
 #define PID_CONTROLLER_H
 
-#include <stdbool.h>
-
 typedef struct {
 
-    /* Controller gains */
+	/* Controller gains */
-    float Kp;
+	float Kp;
-    float Ki;
+	float Ki;
-    float Kd;
+	float Kd;
 
-    /* Default controller gains */
+	/* Derivative low-pass filter time constant */
-    float KpDefault;
+	float tau;
-    float KiDefault;
-    float KdDefault;
 
-    /* Derivative low-pass filter time constant */
+	/* Output limits */
-    float tau;
+	float limMin;
+	float limMax;
+	
+	/* Integrator limits */
+	float limMinInt;
+	float limMaxInt;
 
-    /* Output limits */
+	/* Sample time (in seconds) */
-    float limMin;
+	float T;
-    float limMax;
 
-    /* Integrator limits */
+	/* pid coefficient offset for start pid */
-    float limMinInt;
+	float offset;
-    float limMaxInt;
 
-    /* Sample time (in seconds) */
+	/* Controller "memory" */
-    float T;
+	float integrator;
+	float prevError;			/* Required for integrator */
+	float proportional;
+	float differentiator;
+	float prevMeasurement;		/* Required for differentiator */
 
-    bool enable;
+	/* Controller output */
+	float out;
 
-    /* Controller "memory" */
+} PIDController;
-    float proportional;
-    float integrator;
-    float prevError;            /* Required for integrator */
-    float differentiator;
-    float prevMeasurement;        /* Required for differentiator */
 
-    /* Controller output */
+void  PIDController_Init(PIDController *pid);
-    float out;
+float PIDController_Update(PIDController *pid, float setpoint, float measurement);
 
-} PidController;
+#endif
-
-void pidControllerInit(PidController *pid, float Kp, float Ki, float Kd, float tau, float limMin, float limMax,
-                       float limMinInt, float limMaxInt, float T);
-
-float pidControllerUpdate(PidController *pid, float setpoint, float measurement);
-
-#endif //PID_CONTROLLER_H

PID_Test.txt

@@ -0,0 +1,65 @@
+#include <stdio.h>
+#include <stdlib.h>
+
+#include "PID.h"
+
+/* Controller parameters */
+#define PID_KP  2.0f
+#define PID_KI  0.5f
+#define PID_KD  0.25f
+
+#define PID_TAU 0.02f
+
+#define PID_LIM_MIN -10.0f
+#define PID_LIM_MAX  10.0f
+
+#define PID_LIM_MIN_INT -5.0f
+#define PID_LIM_MAX_INT  5.0f
+
+#define SAMPLE_TIME_S 0.01f
+
+/* Maximum run-time of simulation */
+#define SIMULATION_TIME_MAX 4.0f
+
+/* Simulated dynamical system (first order) */
+float TestSystem_Update(float inp);
+
+int main()
+{
+    /* Initialise PID controller */
+    PIDController pid = { PID_KP, PID_KI, PID_KD,
+                          PID_TAU,
+                          PID_LIM_MIN, PID_LIM_MAX,
+			  PID_LIM_MIN_INT, PID_LIM_MAX_INT,
+                          SAMPLE_TIME_S };
+
+    PIDController_Init(&pid);
+
+    /* Simulate response using test system */
+    float setpoint = 1.0f;
+
+    printf("Time (s)\tSystem Output\tControllerOutput\r\n");
+    for (float t = 0.0f; t <= SIMULATION_TIME_MAX; t += SAMPLE_TIME_S) {
+
+        /* Get measurement from system */
+        float measurement = TestSystem_Update(pid.out);
+
+        /* Compute new control signal */
+        PIDController_Update(&pid, setpoint, measurement);
+
+        printf("%f\t%f\t%f\r\n", t, measurement, pid.out);
+
+    }
+
+    return 0;
+}
+
+float TestSystem_Update(float inp) {
+
+    static float output = 0.0f;
+    static const float alpha = 0.02f;
+
+    output = (SAMPLE_TIME_S * inp + output) / (1.0f + alpha * SAMPLE_TIME_S);
+
+    return output;
+}

README.md

@@ -0,0 +1,5 @@
+# PID
+PID controller implementation written in C.
+ 
+Note on 'derivative-on-measurement': Since the 'error signal' effectively going into the differentiator does not depend on the setpoint: e[n] = 0 - measurement, and therefore (e[n] - e[n - 1]) = (0 - measurement) - (0 - prevMeasurement) = -Kd * (measurement - prevMeasurement). (Note the minus sign compared to derivative-on-error!)
+I've included the minus sign in the code, so gains will have the effect as normal.