PID
Dependencies: BLE_API mbed nRF51822
Diff: PID.cpp
- Revision:
- 0:1f4d5c5491b8
- Child:
- 1:d3e12393b71d
diff -r 000000000000 -r 1f4d5c5491b8 PID.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID.cpp Mon Oct 24 16:10:48 2016 +0000 @@ -0,0 +1,36 @@ +#include "PID.h" + +PIDClass::PIDClass(int _Kr,int _Ki, int _Kd,int _SetPoint):Kr(_Kr),Ki(_Ki),Kd(_Kd),SetPoint(_SetPoint) +{ + /*Kr = _Kr; + Ki = _Ki; + Kd = _Kd;*/ +} + + +float PIDClass::ComputeCommand(float inputADC) +{ + static PID_static param; + static float inter = Ki*Kd; + static float Kc = sqrt(inter); + float outputPWM = 0; + float error = 0; + + error = SetPoint - inputADC; + param.integral = param.integral + Ki*Te*error; + outputPWM = Kr*error + param.integral + Kd*(error - param.last_error)/Te; + param.last_error = error; + + if(outputPWM > SUPERIOR_MARGIN) + { + param.integral = param.integral + Kc*(SUPERIOR_MARGIN - outputPWM); + outputPWM = SUPERIOR_MARGIN; + }else if(outputPWM < INFERIOR_MARGIN) + { + param.integral = param.integral + Kc*(INFERIOR_MARGIN - outputPWM); + outputPWM = 0; + } + + return outputPWM; + +}