before test
Dependencies: BEAR_Protocol_Edited BufferedSerial Debug MaxSonar PID Process QEI UI iSerial mbed
Fork of clean_V1 by
Diff: pidcontrol.cpp
- Revision:
- 2:f873deba2305
- Parent:
- 1:45f1573d65a1
- Child:
- 3:edaab92dbd2f
--- a/pidcontrol.cpp Mon Mar 21 20:21:12 2016 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,168 +0,0 @@ -#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; -}