Cristi Stoican / Mbed 2 deprecated nRF51822_SimpleChat

PID

Dependencies:   BLE_API mbed nRF51822

PID.cpp@1:d3e12393b71d, 2017-01-12 (annotated)

Committer:
stoicancristi
Date:
Thu Jan 12 16:04:37 2017 +0000
Revision:
1:d3e12393b71d
Parent:
0:1f4d5c5491b8 
v2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
stoicancristi 0:1f4d5c5491b8 1 #include "PID.h"
stoicancristi 0:1f4d5c5491b8 2
stoicancristi 1:d3e12393b71d 3 PIDClass::PIDClass(float _Kr,float _Ki, float _Kd,float _SetPoint):Kr(_Kr),Ki(_Ki),Kd(_Kd),SetPoint(_SetPoint)
stoicancristi 0:1f4d5c5491b8 4 {
stoicancristi 1:d3e12393b71d 5 //
stoicancristi 0:1f4d5c5491b8 6 }
stoicancristi 0:1f4d5c5491b8 7
stoicancristi 0:1f4d5c5491b8 8 float PIDClass::ComputeCommand(float inputADC)
stoicancristi 0:1f4d5c5491b8 9 {
stoicancristi 0:1f4d5c5491b8 10 static PID_static param;
stoicancristi 0:1f4d5c5491b8 11 static float inter = Ki*Kd;
stoicancristi 0:1f4d5c5491b8 12 static float Kc = sqrt(inter);
stoicancristi 0:1f4d5c5491b8 13 float outputPWM = 0;
stoicancristi 0:1f4d5c5491b8 14 float error = 0;
stoicancristi 0:1f4d5c5491b8 15
stoicancristi 0:1f4d5c5491b8 16 error = SetPoint - inputADC;
stoicancristi 0:1f4d5c5491b8 17 param.integral = param.integral + Ki*Te*error;
stoicancristi 0:1f4d5c5491b8 18 outputPWM = Kr*error + param.integral + Kd*(error - param.last_error)/Te;
stoicancristi 0:1f4d5c5491b8 19 param.last_error = error;
stoicancristi 0:1f4d5c5491b8 20
stoicancristi 0:1f4d5c5491b8 21 if(outputPWM > SUPERIOR_MARGIN)
stoicancristi 0:1f4d5c5491b8 22 {
stoicancristi 0:1f4d5c5491b8 23 param.integral = param.integral + Kc*(SUPERIOR_MARGIN - outputPWM);
stoicancristi 0:1f4d5c5491b8 24 outputPWM = SUPERIOR_MARGIN;
stoicancristi 0:1f4d5c5491b8 25 }else if(outputPWM < INFERIOR_MARGIN)
stoicancristi 0:1f4d5c5491b8 26 {
stoicancristi 0:1f4d5c5491b8 27 param.integral = param.integral + Kc*(INFERIOR_MARGIN - outputPWM);
stoicancristi 0:1f4d5c5491b8 28 outputPWM = 0;
stoicancristi 0:1f4d5c5491b8 29 }
stoicancristi 0:1f4d5c5491b8 30
stoicancristi 0:1f4d5c5491b8 31 return outputPWM;
stoicancristi 0:1f4d5c5491b8 32
stoicancristi 0:1f4d5c5491b8 33 }