Diff: PIDController.cpp

Revision:

0:b5906c81772b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PIDController.cpp	Sun Feb 05 16:31:58 2017 +0000
@@ -0,0 +1,58 @@
+#include "PIDController.hpp"
+
+//PIDController::PIDController() {}
+
+void PIDController::updateParams(ControllerParams &cp) {
+    setKp(cp.kp);
+    setTi(cp.ti);
+    setTd(cp.td);
+}
+
+float PIDController::calculateCmd() const {
+    float outputPWM;
+    
+    float Proportional = kp*dif(this->ref,this->out);
+    float Integral;
+    float Derivative = (kp*td/TE)*dif(this->ref,this->out);
+    
+    if(ti == 0) {
+        outputPWM = Proportional + Derivative;
+    } else {
+        Integral = ((kp*TE)/ti)*dif(this->ref,this->out);
+        outputPWM = Proportional + Integral + Derivative;
+    }
+    
+    if(outputPWM > 1) {
+        outputPWM = outputPWM - Integral;
+    } else if(outputPWM < 0) {
+        outputPWM = 0;
+    }
+    
+    return outputPWM;
+}
+
+void PIDController::setKp(float _kp) {
+    kp = _kp;   
+}
+
+void PIDController::setTi(float _ti) {
+    ti = _ti;
+}
+
+void PIDController::setTd(float _td) {
+    td = _td;
+}
+
+float PIDController::getKp() const {
+    return kp;
+}
+
+float PIDController::getTi() const {
+    return ti;
+}
+
+float PIDController::getTd() const {
+    return td;
+}
+
+PIDController::~PIDController() {}