Pascal Sandrez / PIDcontroller

Simple PID controller class with output clamp

Dependents:   NXPCUPcar

PIDcontroller.cpp@2:994ed695c8a5, 2019-02-20 (annotated)

Committer:
Clarkk
Date:
Wed Feb 20 22:15:52 2019 +0000
Revision:
2:994ed695c8a5
Parent:
1:946d71f8afc6 
Added variables initialization

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Clarkk 0:070ecbe87363 1 #include "PIDcontroller.h"
Clarkk 0:070ecbe87363 2
Clarkk 0:070ecbe87363 3
Clarkk 0:070ecbe87363 4 PID::PID()
Clarkk 0:070ecbe87363 5 {
Clarkk 0:070ecbe87363 6 integral = 0;
Clarkk 0:070ecbe87363 7 previous_error = 0;
Clarkk 0:070ecbe87363 8
Clarkk 1:946d71f8afc6 9 outputMin = -1.0;
Clarkk 1:946d71f8afc6 10 outputMax = 1.0;
Clarkk 1:946d71f8afc6 11
Clarkk 1:946d71f8afc6 12 target = 0;
Clarkk 1:946d71f8afc6 13 kp = 1.0;
Clarkk 1:946d71f8afc6 14 ki = 0;
Clarkk 1:946d71f8afc6 15 kd = 0;
Clarkk 1:946d71f8afc6 16
Clarkk 0:070ecbe87363 17 t.start();
Clarkk 0:070ecbe87363 18 t.reset();
Clarkk 0:070ecbe87363 19 }
Clarkk 0:070ecbe87363 20
Clarkk 0:070ecbe87363 21 void PID::reset(void)
Clarkk 0:070ecbe87363 22 {
Clarkk 0:070ecbe87363 23 integral = 0;
Clarkk 0:070ecbe87363 24 previous_error = 0;
Clarkk 0:070ecbe87363 25 t.reset();
Clarkk 0:070ecbe87363 26 }
Clarkk 0:070ecbe87363 27
Clarkk 0:070ecbe87363 28 void PID::setReference(float ref)
Clarkk 0:070ecbe87363 29 {
Clarkk 0:070ecbe87363 30 target = ref;
Clarkk 0:070ecbe87363 31 }
Clarkk 0:070ecbe87363 32
Clarkk 0:070ecbe87363 33 void PID::setCoefficients(float Kp, float Ki, float Kd)
Clarkk 0:070ecbe87363 34 {
Clarkk 0:070ecbe87363 35 kp = Kp;
Clarkk 0:070ecbe87363 36 ki = Ki;
Clarkk 0:070ecbe87363 37 kd = Kd;
Clarkk 0:070ecbe87363 38 }
Clarkk 0:070ecbe87363 39
Clarkk 0:070ecbe87363 40 void PID::setOutputLimits(float min, float max)
Clarkk 0:070ecbe87363 41 {
Clarkk 0:070ecbe87363 42 if(min > max)
Clarkk 0:070ecbe87363 43 return;
Clarkk 0:070ecbe87363 44
Clarkk 0:070ecbe87363 45 outputMin = min;
Clarkk 0:070ecbe87363 46 outputMax = max;
Clarkk 0:070ecbe87363 47 }
Clarkk 0:070ecbe87363 48
Clarkk 0:070ecbe87363 49 float PID::processNewValue(float input)
Clarkk 0:070ecbe87363 50 {
Clarkk 0:070ecbe87363 51 float dt;
Clarkk 2:994ed695c8a5 52 error = target - input;
Clarkk 2:994ed695c8a5 53
Clarkk 2:994ed695c8a5 54 if(kp != 0)
Clarkk 2:994ed695c8a5 55 pout = kp*error;
Clarkk 2:994ed695c8a5 56 else
Clarkk 2:994ed695c8a5 57 pout = 0;
Clarkk 2:994ed695c8a5 58 // TODO: see if there is a need of a clamp on integral term
Clarkk 2:994ed695c8a5 59 if((ki!= 0) || (kd != 0))
Clarkk 2:994ed695c8a5 60 {
Clarkk 2:994ed695c8a5 61 dt = t.read();
Clarkk 2:994ed695c8a5 62 t.reset();
Clarkk 2:994ed695c8a5 63
Clarkk 2:994ed695c8a5 64 if(ki != 0)
Clarkk 2:994ed695c8a5 65 {
Clarkk 2:994ed695c8a5 66 integral = integral + error*dt;
Clarkk 2:994ed695c8a5 67 iout = ki*integral;
Clarkk 2:994ed695c8a5 68 }
Clarkk 2:994ed695c8a5 69 else
Clarkk 2:994ed695c8a5 70 iout = 0;
Clarkk 2:994ed695c8a5 71
Clarkk 2:994ed695c8a5 72 if(kd != 0)
Clarkk 2:994ed695c8a5 73 {
Clarkk 2:994ed695c8a5 74 derivative = (error - previous_error)/dt;
Clarkk 2:994ed695c8a5 75 dout = kd*derivative;
Clarkk 2:994ed695c8a5 76 }
Clarkk 2:994ed695c8a5 77 else
Clarkk 2:994ed695c8a5 78 dout = 0;
Clarkk 2:994ed695c8a5 79 }
Clarkk 2:994ed695c8a5 80 else
Clarkk 2:994ed695c8a5 81 {
Clarkk 2:994ed695c8a5 82 iout = 0;
Clarkk 2:994ed695c8a5 83 dout = 0;
Clarkk 2:994ed695c8a5 84 }
Clarkk 0:070ecbe87363 85 previous_error = error;
Clarkk 0:070ecbe87363 86
Clarkk 0:070ecbe87363 87 return getControllerOutput();
Clarkk 0:070ecbe87363 88 }
Clarkk 0:070ecbe87363 89
Clarkk 0:070ecbe87363 90 float PID::getControllerOutput(void)
Clarkk 0:070ecbe87363 91 {
Clarkk 0:070ecbe87363 92 float output;
Clarkk 0:070ecbe87363 93
Clarkk 2:994ed695c8a5 94 output = pout + iout + dout;
Clarkk 0:070ecbe87363 95
Clarkk 0:070ecbe87363 96 if(output > outputMax)
Clarkk 0:070ecbe87363 97 return outputMax;
Clarkk 0:070ecbe87363 98
Clarkk 0:070ecbe87363 99 if(output < outputMin)
Clarkk 0:070ecbe87363 100 return outputMin;
Clarkk 0:070ecbe87363 101
Clarkk 0:070ecbe87363 102 return output;
Clarkk 0:070ecbe87363 103 }
Clarkk 2:994ed695c8a5 104
Clarkk 2:994ed695c8a5 105 float PID::getIntegralTerm(void)
Clarkk 2:994ed695c8a5 106 {
Clarkk 2:994ed695c8a5 107 return integral;
Clarkk 2:994ed695c8a5 108 }