Diff: pid.cpp

Revision:

0:3fbb3fa4ff64

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pid.cpp	Fri Apr 12 06:58:18 2019 +0000
@@ -0,0 +1,72 @@
+/* 
+ * File:   pid.cpp
+ * Author: User
+ * 
+ * Created on 27 februari 2019, 10:28
+ */
+#include <iostream>
+#include <cmath>
+#include "pid.h"
+
+using namespace std;
+
+control::PID::PID( double dt, double Kp, double Kd, double Ki, double Fc )
+{
+    pimpl = new PIDImpl(dt,Kp,Kd,Ki,Fc);
+}
+double control::PID::calculate( double reference, double pv )
+{
+    return pimpl->calculate(reference,pv);
+}
+control::PID::~PID() 
+{
+    delete pimpl;
+}
+
+/**
+ * Implementation
+ */
+control::PIDImpl::PIDImpl( double dt, double Kp, double Kd, double Ki, double Fc ) :
+    _dt(dt),
+    _Kp(Kp),
+    _Kd(Kd),
+    _Ki(Ki),
+    _Fc(Fc),
+    _pre_error(0),
+    _integral(0)
+{
+}
+/*The input for the calculate function will be the reference and the 'previous 
+ * value (pv)' that comes in reality from the sensors. This data is stored in 
+ * DataStore so we should add these values. This is different for all the PID 
+ * controllers   
+*/
+double control::PIDImpl::calculate( double reference, double pv )
+{
+    
+    // Calculate error
+    double error = reference - pv;
+
+    // Proportional term
+    double Pout = _Kp * error;
+
+    // Integral term
+    _integral += error * _dt;
+    double Iout = _Ki * _integral;
+
+    // Derivative term
+    //double derivative = (error - _pre_error) / _dt;
+    //double Dout = _Kd * derivative;
+    double Dout = _Kd * (_Fc/(1+_Fc* _integral));
+    // Calculate total output
+    double output = Pout + Iout + Dout;
+
+    // Save error to previous error
+    _pre_error = error;
+
+    return output;
+}
+
+control::PIDImpl::~PIDImpl()
+{
+}