C++ PID 控制器實作 數字追逐賽

資料來源: https://gist.github.com/bradley219/5373998

PID原理詳解: https://bit.ly/35qKtRW

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 <iostream>
#include <cmath>
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 <iostream>
#include <cstdio>
#include <cstdlib>
#include <cmath>
#include <time.h>
#include <sys/timeb.h>
#include <windows.h>
#include "PIDImpl.h"


using namespace std;
//https://gist.github.com/bradley219/5373998
//PID 控制器實作 數字追逐賽
//https://zh.wikipedia.org/wiki/%E9%BD%90%E6%A0%BC%E5%8B%92%EF%BC%8D%E5%B0%BC%E7%A7%91%E5%B0%94%E6%96%AF%E6%96%B9%E6%B3%95
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: %4d-%02d-%02d %02d:%02d:%02d\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: %4d-%02d-%02d %02d:%02d:%02d\n", tm02.tm_year + 1900, tm02.tm_mon + 1, tm02.tm_mday, tm02.tm_hour, tm02.tm_min, tm02.tm_sec);
    return 0;
}