PIDController.cpp

00001 #include "PIDController.hpp"
00002 
00003 //PIDController::PIDController() {}
00004 
00005 void PIDController::updateParams(ControllerParams &cp) {
00006     setKp(cp.kp);
00007     setTi(cp.ti);
00008     setTd(cp.td);
00009 }
00010 
00011 float PIDController::calculateCmd() const {
00012     float outputPWM;
00013     
00014     float Proportional = kp*dif(this->ref,this->out);
00015     float Integral;
00016     float Derivative = (kp*td/TE)*dif(this->ref,this->out);
00017     
00018     if(ti == 0) {
00019         outputPWM = Proportional + Derivative;
00020     } else {
00021         Integral = ((kp*TE)/ti)*dif(this->ref,this->out);
00022         outputPWM = Proportional + Integral + Derivative;
00023     }
00024     
00025     if(outputPWM > 1) {
00026         outputPWM = outputPWM - Integral;
00027     } else if(outputPWM < 0) {
00028         outputPWM = 0;
00029     }
00030     
00031     return outputPWM;
00032 }
00033 
00034 void PIDController::setKp(float _kp) {
00035     kp = _kp;   
00036 }
00037 
00038 void PIDController::setTi(float _ti) {
00039     ti = _ti;
00040 }
00041 
00042 void PIDController::setTd(float _td) {
00043     td = _td;
00044 }
00045 
00046 float PIDController::getKp() const {
00047     return kp;
00048 }
00049 
00050 float PIDController::getTi() const {
00051     return ti;
00052 }
00053 
00054 float PIDController::getTd() const {
00055     return td;
00056 }
00057 
00058 PIDController::~PIDController() {}