Skip to content

Instantly share code, notes, and snippets.

@pholat
Forked from bradley219/.gitignore
Created March 1, 2017 22:52
Show Gist options
  • Save pholat/339c99fc9cf6e1765cb3f9a047aa5a74 to your computer and use it in GitHub Desktop.
Save pholat/339c99fc9cf6e1765cb3f9a047aa5a74 to your computer and use it in GitHub Desktop.
PID C++ implementation
#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
#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
#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;
}
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