Team repo

Dependencies:   mbed QEI

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers pid.cpp Source File

pid.cpp

00001 #ifndef _PID_SOURCE_
00002 #define _PID_SOURCE_
00003 
00004 #include <iostream>
00005 #include <cmath>
00006 #include "pid.h"
00007 
00008 using namespace std;
00009 
00010 PID::PID( float dt, float max, float min, float kp, float ki, float kd )
00011 {
00012    dt_ = dt;
00013    max_ = max;
00014    min_ = min;
00015    kp_ = kp;
00016    ki_ = ki;
00017    kd_ = kd;
00018    pre_error_ = 0;
00019    integral_ = 0;
00020    
00021 }
00022 
00023 float PID::calculate(float setpoint, float pv)
00024 {
00025     // Calculate time changed since last PID calculation
00026 //    dt_ = current_time - dt_;
00027        
00028     // Calculate error
00029     float error = setpoint - pv;
00030 
00031     // Proportional term
00032     float Pout = kp_ * error;
00033 
00034     // Integral term
00035     integral_ += error * dt_;
00036     float Iout = ki_ * integral_;
00037 
00038     // Derivative term
00039     float derivative = (error - pre_error_) / dt_;
00040     float Dout = kd_ * derivative;
00041 
00042     // Calculate total output
00043     float output = Pout + Iout + Dout;
00044 
00045     // Restrict to max/min
00046 
00047     if( output > max_ ){
00048         output = max_;
00049     }
00050     else if( output < min_ ){
00051         output = min_;
00052     }
00053 
00054 
00055     // Save error to previous error
00056     pre_error_ = error;
00057 
00058     return output;
00059 }
00060 
00061 void PID::testError(float setpoint, float pv){
00062         float error = setpoint - pv;
00063         printf("Error: %f\n", error);
00064         printf("Change in Time: %f\n", dt_);
00065 }
00066 
00067 PID::~PID() {}
00068 
00069 
00070 /**
00071  * Implementation
00072  */
00073 /*
00074 PIDImpl::PIDImpl( float dt, float max, float min, float Kp, float Kd, float Ki ) :
00075     _dt(dt),
00076     _max(max),
00077     _min(min),
00078     _Kp(Kp),
00079     _Kd(Kd),
00080     _Ki(Ki),
00081     _pre_error(0),
00082     _integral(0)
00083 {
00084 }
00085 
00086 float PIDImpl::calculate( float setpoint, float pv )
00087 {
00088     
00089     // Calculate error
00090     float error = setpoint - pv;
00091 
00092     // Proportional term
00093     float Pout = _Kp * error;
00094 
00095     // Integral term
00096     _integral += error * _dt;
00097     float Iout = _Ki * _integral;
00098 
00099     // Derivative term
00100     float derivative = (error - _pre_error) / _dt;
00101     float Dout = _Kd * derivative;
00102 
00103     // Calculate total output
00104     float output = Pout + Iout + Dout;
00105 
00106     // Restrict to max/min
00107     if( output > _max )
00108         output = _max;
00109     else if( output < _min )
00110         output = _min;
00111 
00112     // Save error to previous error
00113     _pre_error = error;
00114 
00115     return output;
00116 }
00117 
00118 PIDImpl::~PIDImpl()
00119 {
00120 }
00121 */
00122 #endif