-
-
Save pholat/339c99fc9cf6e1765cb3f9a047aa5a74 to your computer and use it in GitHub Desktop.
PID C++ implementation
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#ifndef _PID_SOURCE_ | |
#define _PID_SOURCE_ | |
#include <iostream> | |
#include <cmath> | |
#include "pid.h" | |
using namespace std; | |
class PIDImpl | |
{ | |
public: | |
PIDImpl( double dt, double max, double min, double Kp, double Kd, double Ki ); | |
~PIDImpl(); | |
double calculate( double setpoint, double pv ); | |
private: | |
double _dt; | |
double _max; | |
double _min; | |
double _Kp; | |
double _Kd; | |
double _Ki; | |
double _pre_error; | |
double _integral; | |
}; | |
PID::PID( double dt, double max, double min, double Kp, double Kd, double Ki ) | |
{ | |
pimpl = new PIDImpl(dt,max,min,Kp,Kd,Ki); | |
} | |
double PID::calculate( double setpoint, double pv ) | |
{ | |
return pimpl->calculate(setpoint,pv); | |
} | |
PID::~PID() | |
{ | |
delete pimpl; | |
} | |
/** | |
* Implementation | |
*/ | |
PIDImpl::PIDImpl( double dt, double max, double min, double Kp, double Kd, double Ki ) : | |
_dt(dt), | |
_max(max), | |
_min(min), | |
_Kp(Kp), | |
_Kd(Kd), | |
_Ki(Ki), | |
_pre_error(0), | |
_integral(0) | |
{ | |
} | |
double PIDImpl::calculate( double setpoint, double pv ) | |
{ | |
// Calculate error | |
double error = setpoint - 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; | |
// Calculate total output | |
double output = Pout + Iout + Dout; | |
// Restrict to max/min | |
if( output > _max ) | |
output = _max; | |
else if( output < _min ) | |
output = _min; | |
// Save error to previous error | |
_pre_error = error; | |
return output; | |
} | |
PIDImpl::~PIDImpl() | |
{ | |
} | |
#endif |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#ifndef _PID_H_ | |
#define _PID_H_ | |
class PIDImpl; | |
class PID | |
{ | |
public: | |
// Kp - proportional gain | |
// Ki - Integral gain | |
// Kd - derivative gain | |
// dt - loop interval time | |
// max - maximum value of manipulated variable | |
// min - minimum value of manipulated variable | |
PID( double dt, double max, double min, double Kp, double Kd, double Ki ); | |
// Returns the manipulated variable given a setpoint and current process value | |
double calculate( double setpoint, double pv ); | |
~PID(); | |
private: | |
PIDImpl *pimpl; | |
}; | |
#endif |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#include "pid.h" | |
#include <stdio.h> | |
int main() { | |
PID pid = PID(0.1, 100, -100, 0.1, 0.01, 0.5); | |
double val = 20; | |
for (int i = 0; i < 100; i++) { | |
double inc = pid.calculate(0, val); | |
printf("val:% 7.3f inc:% 7.3f\n", val, inc); | |
val += inc; | |
} | |
return 0; | |
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
To compile library: | |
g++ -c pid.cpp -o pid.o | |
To compile example code: | |
g++ pid_example.cpp pid.o -o pid_example | |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment