實現一個簡單的數位 PID, 利用 opencv 程式庫繪出控制曲線

// g++ pid.c `pkg-config --cflags --libs opencv4` && ./a.out

#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>
using namespace cv;
class DPID { // discrete PID
    private:
        double Kp, Ti, Td, dt;// PID parameters
        double et_1 = 0.0;// initial e[t-1]
        double et_2 = 0.0;// initial e[t-2]
        double ut_1 = 0.0;// initial u[t-1]
    public:
        double operator( )(const double target) {            
            auto et = target - ut_1;
            auto err = (1 + dt/Ti + Td/dt)*et - (1 + 2*Td/dt)*et_1 + (Td/dt)*et_2;           
            // auto err = (1 + dt/(2*Ti) + Td/dt)*et - (1 - dt/(2*Ti) + 2*Td/dt)*et_1 + (Td/dt)*et_2;
            auto ut = ut_1 + Kp * err;
            ut_1 = ut < 0 ? 0 : ut > 1000 ? 1000 : ut; // update output
            et_2 = et_1;
            et_1 = et;
            return ut_1;
        }
        DPID(double Ku = 1, double fMax = 1e8)  {   
            dt  = 1.0/(2*fMax);
            Ti  = dt < 1e-9 ? 1e-9 : dt;
            Td = dt/4;
            Kp = Ku;
        }
};
int main( ) {
    const int pixels = 800;
    const double target = 400.0;
    const double yX2 = 2 * target;// 2x target
    auto map2D = [&yX2](Point p) {// 線性座標轉換
        return Point(p.x, yX2 - p.y);
    };
    Mat mat2D(Size(pixels, yX2), CV_8UC3);
    auto figure = [&mat2D]( ) {
        static String name = "DPID";
        imshow(name, mat2D);
        return name.c_str( );
    };
    auto line2D = [&mat2D, &map2D](Point p1, Point p2) {
        static auto green = Scalar(0, 255, 0);
        line(mat2D, map2D(p1), map2D(p2), green);
    };
    Point last;// the last point
    Scalar red(0, 0, 255);
    Scalar black(0, 0, 0);
    // printf("       t,       y\n");// csv
    for(double k=0.1; k<=0.5; k+=0.01) {
        mat2D.setTo(black);
        line(mat2D, Point(0, 0), Point(pixels, 0)  , red, 2, FILLED);
        line(mat2D, Point(0, 0), Point(0, yX2)     , red, 2, FILLED);
        line(mat2D, Point(0, yX2), Point(pixels, yX2), red, 2, FILLED);
        line(mat2D, Point(pixels, 0), Point(pixels, yX2), red, 2, FILLED);
        auto pid = DPID(k);// foreground
        for (int t = 0; t < pixels; t ++) {
            auto y = pid(target);
            // printf("%8d,%8.2f\n", t, y);
            auto point = Point(t, y);
            if (t > 0) line2D(last, point);
            last = point;
        }
        figure( );
        if(waitKey(50) > 0) break;
    }
    return waitKey(0);
}