PID.cpp

00001 #include "PID.h"
00002 
00003 PID::PID(float kp, float ki, float kd, float dt)
00004 {
00005     this->kp = kp;
00006     this->ki = ki;
00007     this->kd = kd;
00008     this->dt = dt;
00009     
00010     reset();
00011 }
00012 
00013 void PID::reset()
00014 {
00015     lastErro = 0.0f;
00016     lastSumErro = 0.0f;
00017     _pid = 0.0f;
00018     
00019     integral = 0.0f;
00020     
00021 }
00022 
00023 void PID::setGains(float kp, float ki, float kd)
00024 {
00025     this->kp = kp;
00026     this->ki = ki;
00027     this->kd = kd;
00028 }
00029 
00030 float PID::compute(float setPoint, float currentPoint)
00031 {
00032     float error = setPoint - currentPoint;
00033     float derivative = (error - lastErro)/dt;
00034     integral = (integral + (error*dt));
00035     
00036     float contribuicao = (kp*error) + (ki*integral) + (kd*derivative);
00037     
00038     
00039   
00040     //_pid = contribuicao;
00041             
00042     lastErro = error;
00043     //lastSumErro += error;
00044     
00045     return contribuicao;
00046 }
00047 
00048 float PID::getP()
00049 {
00050     return kp;
00051 }
00052 
00053 float PID::getI()
00054 {
00055     return ki;
00056 }
00057 
00058 float PID::getD()
00059 {
00060     return kd;
00061 }
00062 /*
00063 float PID::getuP()
00064 {
00065     return up;
00066 }
00067 
00068 float PID::getuI()
00069 {
00070     return ui;
00071 }
00072 
00073 float PID::getuD()
00074 {
00075     return ud;
00076 }
00077 */