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@43:5123f24e0b2c, 2017-07-07 (annotated)

Committer:
gunarthon
Date:
Fri Jul 07 00:16:14 2017 +0000
Revision:
43:5123f24e0b2c
Parent:
42:b69538bba4f9 
final version

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 40:19d51f6e6800 4 #include "mbed.h"
gunarthon 40:19d51f6e6800 5
gunarthon 40:19d51f6e6800 6
gunarthon 40:19d51f6e6800 7 DigitalOut led2(LED2);
gunarthon 38:b760c09b311c 8 //http://coder-tronics.com/pid-tutorial-c-code-example-pt2/
gunarthon 38:b760c09b311c 9
gunarthon 38:b760c09b311c 10 class Pid
gunarthon 38:b760c09b311c 11 {
gunarthon 38:b760c09b311c 12 private:
gunarthon 38:b760c09b311c 13 double setPoint;
gunarthon 38:b760c09b311c 14 double Kp, Ki, Kd;
gunarthon 38:b760c09b311c 15
gunarthon 38:b760c09b311c 16 double integrator, derivator;
gunarthon 38:b760c09b311c 17 double maxInteg, minInteg;
gunarthon 38:b760c09b311c 18 double maxPwm, minPwm;
gunarthon 40:19d51f6e6800 19 double lastInput;
gunarthon 40:19d51f6e6800 20 double lastPwm;
gunarthon 40:19d51f6e6800 21 double middlePoint;
gunarthon 38:b760c09b311c 22
gunarthon 38:b760c09b311c 23 public:
gunarthon 38:b760c09b311c 24 Pid(double Kp, double Ki, double Kd)
gunarthon 38:b760c09b311c 25 {
gunarthon 38:b760c09b311c 26 this->Kp = Kp;
gunarthon 38:b760c09b311c 27 this->Ki = Ki;
gunarthon 38:b760c09b311c 28 this->Kd = Kd;
gunarthon 40:19d51f6e6800 29 this->middlePoint = 0.08;
gunarthon 38:b760c09b311c 30 integrator = 0;
gunarthon 38:b760c09b311c 31 derivator = 0;
gunarthon 38:b760c09b311c 32
gunarthon 40:19d51f6e6800 33 maxInteg = 1.0;
gunarthon 40:19d51f6e6800 34 minInteg = -1.0;
gunarthon 38:b760c09b311c 35
gunarthon 38:b760c09b311c 36 maxPwm = 1;
gunarthon 38:b760c09b311c 37 minPwm = 0.05;
gunarthon 40:19d51f6e6800 38 lastInput = 0.5;
gunarthon 40:19d51f6e6800 39 lastPwm = 0.5;
gunarthon 38:b760c09b311c 40 }
gunarthon 40:19d51f6e6800 41 void addKp(double value) {Kp += value;}
gunarthon 40:19d51f6e6800 42 void addKi(double value) {Ki += value;}
gunarthon 40:19d51f6e6800 43 void addKd(double value) {Kd += value;}
gunarthon 40:19d51f6e6800 44 double getKp() {return Kp;}
gunarthon 40:19d51f6e6800 45 double getKi() {return Ki;}
gunarthon 40:19d51f6e6800 46 double getKd() {return Kd;}
gunarthon 40:19d51f6e6800 47 void setParameters(double Kp, double Ki, double Kd)
gunarthon 38:b760c09b311c 48 {
gunarthon 40:19d51f6e6800 49 this->Kp = Kp;
gunarthon 40:19d51f6e6800 50 this->Ki = Ki;
gunarthon 40:19d51f6e6800 51 this->Kd = Kd;
gunarthon 38:b760c09b311c 52 }
gunarthon 40:19d51f6e6800 53 //desired value
gunarthon 40:19d51f6e6800 54 void setSetPoint(double setPoint) {this->setPoint = setPoint;}
gunarthon 40:19d51f6e6800 55 double getSetPoint() {return setPoint;}
gunarthon 40:19d51f6e6800 56 double getLastInput() {return lastInput;}
gunarthon 40:19d51f6e6800 57 double getLastPwm() {return lastPwm;}
gunarthon 38:b760c09b311c 58
gunarthon 38:b760c09b311c 59 //values from 0-1
gunarthon 38:b760c09b311c 60 double getPwm(double input)
gunarthon 38:b760c09b311c 61 {
gunarthon 40:19d51f6e6800 62 lastInput = input;
gunarthon 38:b760c09b311c 63 double errorValue = setPoint - input;
gunarthon 38:b760c09b311c 64
gunarthon 38:b760c09b311c 65 //P term
gunarthon 38:b760c09b311c 66 double P = Kp * errorValue;
gunarthon 38:b760c09b311c 67
gunarthon 38:b760c09b311c 68 //I term
gunarthon 38:b760c09b311c 69 integrator += errorValue;
gunarthon 38:b760c09b311c 70
gunarthon 42:b69538bba4f9 71 double I = (Ki/6000.0) * integrator;
gunarthon 38:b760c09b311c 72
gunarthon 40:19d51f6e6800 73 if(I > maxInteg || I < minInteg)
gunarthon 42:b69538bba4f9 74 led2.write(1);
gunarthon 42:b69538bba4f9 75 else led2.write(0);
gunarthon 40:19d51f6e6800 76
gunarthon 40:19d51f6e6800 77 I = I>maxInteg? maxInteg : I;
gunarthon 40:19d51f6e6800 78 I = I<minInteg? minInteg : I;
gunarthon 38:b760c09b311c 79
gunarthon 38:b760c09b311c 80 //D term
gunarthon 38:b760c09b311c 81 double D = Kd * (derivator - errorValue);
gunarthon 38:b760c09b311c 82 derivator = errorValue;
gunarthon 38:b760c09b311c 83
gunarthon 38:b760c09b311c 84 //output
gunarthon 40:19d51f6e6800 85 double pwm = P + I + D + middlePoint;
gunarthon 38:b760c09b311c 86 pwm = pwm>maxPwm? maxPwm : pwm;
gunarthon 38:b760c09b311c 87 pwm = pwm<minPwm? minPwm : pwm;
gunarthon 38:b760c09b311c 88
gunarthon 40:19d51f6e6800 89 lastPwm = pwm;
gunarthon 38:b760c09b311c 90 return pwm;
gunarthon 38:b760c09b311c 91 }
gunarthon 38:b760c09b311c 92
gunarthon 38:b760c09b311c 93 };
gunarthon 38:b760c09b311c 94 #endif