Diff: PID/pid.cpp

Revision:

23:5238b046119b

Parent:

22:c18f04d1dc49

--- a/PID/pid.cpp	Fri Nov 18 02:47:59 2016 +0000
+++ b/PID/pid.cpp	Fri Nov 18 11:55:33 2016 +0000
@@ -7,43 +7,71 @@
 
 using namespace std;
 
-class PIDImpl
+PID::PID( float dt, float max, float min, float kp, float ki, float kd )
 {
-    public:
-        PIDImpl( double dt, double max, double min, double Kp, double Kd, double Ki );
-        ~PIDImpl();
-        double calculate( double setpoint, double pv );
+   dt_ = dt;
+   max_ = max;
+   min_ = min;
+   kp_ = kp;
+   ki_ = ki;
+   kd_ = kd;
+   pre_error_ = 0;
+   integral_ = 0;
+   
+}
+
+float PID::calculate(float setpoint, float pv)
+{
+    // Calculate time changed since last PID calculation
+//    dt_ = current_time - dt_;
+       
+    // Calculate error
+    float error = setpoint - pv;
 
-    private:
-        double _dt;
-        double _max;
-        double _min;
-        double _Kp;
-        double _Kd;
-        double _Ki;
-        double _pre_error;
-        double _integral;
-};
+    // Proportional term
+    float Pout = kp_ * error;
+
+    // Integral term
+    integral_ += error * dt_;
+    float Iout = ki_ * integral_;
+
+    // Derivative term
+    float derivative = (error - pre_error_) / dt_;
+    float Dout = kd_ * derivative;
+
+    // Calculate total output
+    float output = Pout + Iout + Dout;
+
+    // Restrict to max/min
+
+    if( output > max_ ){
+        output = max_;
+    }
+    else if( output < min_ ){
+        output = min_;
+    }
 
 
-PID::PID( double dt, double max, double min, double Kp, double Kd, double Ki )
-{
-    pimpl = new PIDImpl(dt,max,min,Kp,Kd,Ki);
+    // Save error to previous error
+    pre_error_ = error;
+
+    return output;
 }
-double PID::calculate( double setpoint, double pv )
-{
-    return pimpl->calculate(setpoint,pv);
+
+void PID::testError(float setpoint, float pv){
+        float error = setpoint - pv;
+        printf("Error: %f\n", error);
+        printf("Change in Time: %f\n", dt_);
 }
-PID::~PID() 
-{
-    delete pimpl;
-}
+
+PID::~PID() {}
 
 
 /**
  * Implementation
  */
-PIDImpl::PIDImpl( double dt, double max, double min, double Kp, double Kd, double Ki ) :
+/*
+PIDImpl::PIDImpl( float dt, float max, float min, float Kp, float Kd, float Ki ) :
     _dt(dt),
     _max(max),
     _min(min),
@@ -55,25 +83,25 @@
 {
 }
 
-double PIDImpl::calculate( double setpoint, double pv )
+float PIDImpl::calculate( float setpoint, float pv )
 {
     
     // Calculate error
-    double error = setpoint - pv;
+    float error = setpoint - pv;
 
     // Proportional term
-    double Pout = _Kp * error;
+    float Pout = _Kp * error;
 
     // Integral term
     _integral += error * _dt;
-    double Iout = _Ki * _integral;
+    float Iout = _Ki * _integral;
 
     // Derivative term
-    double derivative = (error - _pre_error) / _dt;
-    double Dout = _Kd * derivative;
+    float derivative = (error - _pre_error) / _dt;
+    float Dout = _Kd * derivative;
 
     // Calculate total output
-    double output = Pout + Iout + Dout;
+    float output = Pout + Iout + Dout;
 
     // Restrict to max/min
     if( output > _max )
@@ -90,5 +118,5 @@
 PIDImpl::~PIDImpl()
 {
 }
-
+*/
 #endif
\ No newline at end of file