โปรแกรมของบอร์ด Motion
Dependencies: BEAR_Reciever Motor eeprom iSerial mbed
Fork of DogPID by
Diff: pidcontrol.cpp
- Revision:
- 36:1561b6d61095
- Parent:
- 34:0cf04acfe422
diff -r 7fa769563e61 -r 1561b6d61095 pidcontrol.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/pidcontrol.cpp Wed Feb 03 14:49:46 2016 +0000 @@ -0,0 +1,168 @@ +#include "pidcontrol.h" + +PID::PID() +{ + Kp=1.0f; + Ki=0.0f; + Kd=0.0f; + il=65535.0; + margin = 0.0f; + +} + +PID::PID(float p,float i,float d) +{ + Kp=p; + Ki=i; + Kd=d; + il=65535.0; + margin =0.0f; +} + +void PID::setGoal(float ref) +{ + setpoint = ref; +} + +void PID::setCurrent(float sensor) +{ + input = sensor; +} + +float PID::compute() +{ + + e_n = setpoint - input; + + if((e_i < il) && (e_i > -il)) + { + e_i += e_n; + } + else + { +#ifdef PID_DEBUG + printf("il overflow\n\r"); +#endif + e_i =il; + } + + + output = (Kp*e_n)+(Ki*e_i)+(Kd*(e_n-e_n_1)); + + if(output > 0) + { + if(output < margin) + { + output = 0.0; + } + } + else + { + if(output > -margin) + { + output = 0.0; + } + } + + return output; +} + +void PID::setMargin(float gap) +{ + margin =gap; +} + +float PID::getMargin() +{ + return margin; +} + + +void PID::setIntegalLimit(float limit) +{ + il = limit; +} +float PID::getIntegalLimit() +{ + return il; +} + + +float PID::getErrorNow() +{ + return e_n; +} + +float PID::getErrorLast() +{ + return e_n_1; +} + +float PID::getErrorDiff() +{ + return e_n - e_n_1; +} + +float PID::getErrorIntegal() +{ + return e_i; +} + +void PID::setKp(float p) +{ + if(p < 0.0f) + { +#ifdef PID_DEBUG + printf("Kp = 0.0\n\r"); +#endif + Kp=0.0; + } + else + { + Kp=p; + } +} + +void PID::setKi(float i) +{ + if(i < 0.0f) + { +#ifdef PID_DEBUG + printf("Ki = 0.0\n\r"); +#endif + Ki=0.0; + } + else + { + Ki=i; + } +} +void PID::setKd(float d) +{ + if(d < 0.0f) + { +#ifdef PID_DEBUG + printf("Kd = 0.0\n\r"); +#endif + Kd=0.0; + } + else + { + Kd=d; + } +} + +float PID::getKp() +{ + return Kp; +} + +float PID::getKi() +{ + return Ki; +} + +float PID::getKd() +{ + return Kd; +} \ No newline at end of file