// 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);
}
