Cristi Stoican / Mbed 2 deprecated nRF51822_SimpleChat

PID

Dependencies:   BLE_API mbed nRF51822

PID.cpp@0:1f4d5c5491b8, 2016-10-24 (annotated)

Committer:
stoicancristi
Date:
Mon Oct 24 16:10:48 2016 +0000
Revision:
0:1f4d5c5491b8
Child:
1:d3e12393b71d
initial commit : implemented PID class;

Who changed what in which revision?

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