Gunar Kroeger / Mbed OS PID_floating_ball

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   TextLCD

Fork of mbed-os-example-mbed5-blinky by mbed-os-examples

pid.h@38:b760c09b311c, 2017-06-15 (annotated)

Committer:
gunarthon
Date:
Thu Jun 15 23:19:38 2017 +0000
Revision:
38:b760c09b311c
Child:
40:19d51f6e6800
initial commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gunarthon 38:b760c09b311c 1 #ifndef _PID_H_
gunarthon 38:b760c09b311c 2 #define _PID_H_
gunarthon 38:b760c09b311c 3
gunarthon 38:b760c09b311c 4 //http://coder-tronics.com/pid-tutorial-c-code-example-pt2/
gunarthon 38:b760c09b311c 5
gunarthon 38:b760c09b311c 6 class Pid
gunarthon 38:b760c09b311c 7 {
gunarthon 38:b760c09b311c 8 private:
gunarthon 38:b760c09b311c 9 double setPoint;
gunarthon 38:b760c09b311c 10 double Kp, Ki, Kd;
gunarthon 38:b760c09b311c 11
gunarthon 38:b760c09b311c 12 double integrator, derivator;
gunarthon 38:b760c09b311c 13 double maxInteg, minInteg;
gunarthon 38:b760c09b311c 14 double maxPwm, minPwm;
gunarthon 38:b760c09b311c 15
gunarthon 38:b760c09b311c 16 public:
gunarthon 38:b760c09b311c 17 Pid(double Kp, double Ki, double Kd)
gunarthon 38:b760c09b311c 18 {
gunarthon 38:b760c09b311c 19 this->Kp = Kp;
gunarthon 38:b760c09b311c 20 this->Ki = Ki;
gunarthon 38:b760c09b311c 21 this->Kd = Kd;
gunarthon 38:b760c09b311c 22
gunarthon 38:b760c09b311c 23 integrator = 0;
gunarthon 38:b760c09b311c 24 derivator = 0;
gunarthon 38:b760c09b311c 25
gunarthon 38:b760c09b311c 26 maxInteg = 3;
gunarthon 38:b760c09b311c 27 minInteg = -3;
gunarthon 38:b760c09b311c 28
gunarthon 38:b760c09b311c 29 maxPwm = 1;
gunarthon 38:b760c09b311c 30 minPwm = 0.05;
gunarthon 38:b760c09b311c 31 }
gunarthon 38:b760c09b311c 32
gunarthon 38:b760c09b311c 33 //desired value
gunarthon 38:b760c09b311c 34 void setSetPoint(double setPoint)
gunarthon 38:b760c09b311c 35 {
gunarthon 38:b760c09b311c 36 this->setPoint = setPoint;
gunarthon 38:b760c09b311c 37 }
gunarthon 38:b760c09b311c 38 double getSetPoint()
gunarthon 38:b760c09b311c 39 {
gunarthon 38:b760c09b311c 40 return setPoint;
gunarthon 38:b760c09b311c 41 }
gunarthon 38:b760c09b311c 42
gunarthon 38:b760c09b311c 43 //values from 0-1
gunarthon 38:b760c09b311c 44 double getPwm(double input)
gunarthon 38:b760c09b311c 45 {
gunarthon 38:b760c09b311c 46 double errorValue = setPoint - input;
gunarthon 38:b760c09b311c 47
gunarthon 38:b760c09b311c 48 //P term
gunarthon 38:b760c09b311c 49 double P = Kp * errorValue;
gunarthon 38:b760c09b311c 50
gunarthon 38:b760c09b311c 51 //I term
gunarthon 38:b760c09b311c 52 integrator += errorValue;
gunarthon 38:b760c09b311c 53
gunarthon 38:b760c09b311c 54 integrator = integrator>maxInteg? maxInteg : integrator;
gunarthon 38:b760c09b311c 55 integrator = integrator<minInteg? minInteg : integrator;
gunarthon 38:b760c09b311c 56
gunarthon 38:b760c09b311c 57 double I = Ki * integrator;
gunarthon 38:b760c09b311c 58
gunarthon 38:b760c09b311c 59 //D term
gunarthon 38:b760c09b311c 60 double D = Kd * (derivator - errorValue);
gunarthon 38:b760c09b311c 61 derivator = errorValue;
gunarthon 38:b760c09b311c 62
gunarthon 38:b760c09b311c 63 //output
gunarthon 38:b760c09b311c 64 double pwm = P + I + D;
gunarthon 38:b760c09b311c 65 pwm = pwm>maxPwm? maxPwm : pwm;
gunarthon 38:b760c09b311c 66 pwm = pwm<minPwm? minPwm : pwm;
gunarthon 38:b760c09b311c 67
gunarthon 38:b760c09b311c 68 return pwm;
gunarthon 38:b760c09b311c 69 }
gunarthon 38:b760c09b311c 70
gunarthon 38:b760c09b311c 71 };
gunarthon 38:b760c09b311c 72 #endif