pid.cpp

00001 /**
00002  * Copyright 2019 Bradley J. Snyder <snyder.bradleyj@gmail.com>
00003  *
00004  * Permission is hereby granted, free of charge, to any person obtaining a copy
00005  * of this software and associated documentation files (the "Software"), to deal
00006  * in the Software without restriction, including without limitation the rights
00007  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00008  * copies of the Software, and to permit persons to whom the Software is
00009  * furnished to do so, subject to the following conditions:
00010  *
00011  * The above copyright notice and this permission notice shall be included in
00012  * all copies or substantial portions of the Software.
00013  *
00014  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00015  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00016  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00017  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00018  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00019  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00020  * THE SOFTWARE.
00021  */
00022 
00023 #ifndef _PID_SOURCE_
00024 #define _PID_SOURCE_
00025 
00026 #include <iostream>
00027 #include <cmath>
00028 #include "pid.h"
00029 
00030 using namespace std;
00031 
00032 class PIDImpl
00033 {
00034     public:
00035         PIDImpl( double dt, double max, double min, double Kp, double Kd, double Ki );
00036         ~PIDImpl();
00037         double calculate( double setpoint, double pv );
00038 
00039     private:
00040         double _dt;
00041         double _max;
00042         double _min;
00043         double _Kp;
00044         double _Kd;
00045         double _Ki;
00046         double _pre_error;
00047         double _integral;
00048 };
00049 
00050 
00051 PID::PID( double dt, double max, double min, double Kp, double Kd, double Ki )
00052 {
00053     pimpl = new PIDImpl(dt,max,min,Kp,Kd,Ki);
00054 }
00055 double PID::calculate( double setpoint, double pv )
00056 {
00057     return pimpl->calculate(setpoint,pv);
00058 }
00059 PID::~PID() 
00060 {
00061     delete pimpl;
00062 }
00063 
00064 
00065 /**
00066  * Implementation
00067  */
00068 PIDImpl::PIDImpl( double dt, double max, double min, double Kp, double Kd, double Ki ) :
00069     _dt(dt),
00070     _max(max),
00071     _min(min),
00072     _Kp(Kp),
00073     _Kd(Kd),
00074     _Ki(Ki),
00075     _pre_error(0),
00076     _integral(0)
00077 {
00078 }
00079 
00080 double PIDImpl::calculate( double setpoint, double pv )
00081 {
00082     
00083     // Calculate error
00084     double error = setpoint - pv;
00085 
00086     // Proportional term
00087     double Pout = _Kp * error;
00088 
00089     // Integral term
00090     _integral += error * _dt;
00091     double Iout = _Ki * _integral;
00092 
00093     // Derivative term
00094     double derivative = (error - _pre_error) / _dt;
00095     double Dout = _Kd * derivative;
00096 
00097     // Calculate total output
00098     double output = Pout + Iout + Dout;
00099 
00100     // Restrict to max/min
00101     if( abs(output) > _max )
00102         output = output > 0 ? _max : -_max;
00103     else if( abs(output) < _min )
00104         output = output > 0 ? _min : -_min;
00105 
00106     // Save error to previous error
00107     _pre_error = error;
00108 
00109     return output;
00110 }
00111 
00112 PIDImpl::~PIDImpl()
00113 {
00114 }
00115 
00116 #endif