![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
to compare with V6, do not change.
Dependencies: mbed
Diff: PID_Control.cpp
- Revision:
- 0:01c109e18d49
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID_Control.cpp Fri May 26 12:33:39 2017 +0000 @@ -0,0 +1,77 @@ +/* + * PIDControl.cpp + * + * Created on: 16.04.2017 + * Author: chris + */ + +#include "PID_Control.h" + +/** + * Constructor + */ +PID_Control::PID_Control() : + kp(0), ki(0), kd(0) +{ + eOld = 0.0f; + iSum = 0.0f; +} + +/** + * Destructor + */ +PID_Control::~PID_Control(){ +} + +/** + * Sets the PID values + * @param p proportional gain + * @param i integral gain + * @param d differencial gain + */ +void PID_Control::setPIDValues(float p, float i, float d, float _max, float _min, float _iMax) +{ + kp = p; + ki = i; + kd = d; + + max = _max; + min = _min; + iMax = _iMax; +} + +/** + * Calculate and returns the next value from PID control + * @param e the error + * @param period the period + * @return + */ +float PID_Control::calc(float e, float period) +{ + static float dpart = 0.0f; + float out(0.0f); + + iSum += e; + + //Saturate i part + if (iSum > iMax) + iSum = iMax; + if (iSum < -iMax) + iSum = -iMax; + + out = kp * e; + out += ki * iSum * period; + + dpart = 0.7f * dpart + 0.3f * (e - eOld) * 1.0f / period; //low pass filter + out += kd * dpart; + + // out += kd * (e - eOld) * 1.0f / period; // is affected by noise + + eOld = e; + + if( out > max ) out = max; + else if( out < min) out = min; + + return out; +} +