Zi 字媒體
2017-07-25T20:27:27+00:00
C++ PID 控制器實作 數字追逐賽
資料來源: https://gist.github.com/bradley219/5373998
GITHUB: https://github.com/jash-git/CPP_PID_Controll_EX
PIDImpl.h
#ifndef PIDIMPL_H
#define PIDIMPL_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 // PIDIMPL_H
PIDImpl.cpp
#include "PIDImpl.h"
#include
#include
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()
{
}
main.cpp
#include
#include
#include
#include
#include
#include
#include
#include "PIDImpl.h"
using namespace std;
//https://gist.github.com/bradley219/5373998
//PID 控制器實作 數字追逐賽
//https://zh.wikipedia.org/wiki/齐格勒－尼科尔斯方法
int main()
{
//PID pid = PID(0.1, 100, -100, 0.1, 0.01, 0.5);//0.1s=10Hz,outmax=100,outmin=-100,P=0.1,D=0.01,I=0.5
//Ziegler–Nichols方法
//Ku=2,Tu=0.2
//P=2*0.2
//D=2*0.2*0.2/3
//I=2*(2*0.2)/0.2
PID pid = PID(0.1, 100, -100, (0.2*2), ((2*0.2)*0.2/3), (2*(2*0.2)/0.2));
double val = 10.0;
double setpoint = 50.0;
double inc=0.0;
long i=1;
time_t t01 = time(NULL); struct tm tm01 = *localtime(&t01);
printf("start: M-d-d d:d:d\n", tm01.tm_year + 1900, tm01.tm_mon + 1, tm01.tm_mday, tm01.tm_hour, tm01.tm_min, tm01.tm_sec);
while(1)
{
Sleep( 100 );//100ms=0.1s
inc = pid.calculate(setpoint, val);//setpoint(目標[期望]),input(輸入)
printf("i=%d,val:% 7.5f inc:% 7.5f\n",i, val, inc);//顯示小數點五位
if(fabs(val-setpoint)<0.0001)//小數點四位
{
break;
}
val += inc;
i++;
}
//printf("i=%d,val:% 7.5f inc:% 7.5f\n",i, val, inc);//顯示小數點五位 time_t t02 = time(NULL); struct tm tm02 = *localtime(&t02);
printf("end: M-d-d d:d:d\n", tm02.tm_year + 1900, tm02.tm_mon + 1, tm02.tm_mday, tm02.tm_hour, tm02.tm_min, tm02.tm_sec);
return 0;
}
寫了
5860316篇文章，獲得
23313次喜歡