Discrete-Time PID https://jp.mathworks.com/help/control/ug/discrete-time-proportional-integral-derivative-pid-controller.html

pid.h

#pragma once
#include <integrator.h>
template<typename T>
class PID {
	T Kp;
	T Ki;
	T Kd;
	T Tf;
	T Ts;
	T x;
	T y;
	DescreteTimeIntegrator<T> IF;
	DescreteTimeIntegrator<T> DF;
public:
	PID(T kp, T ki, T kd, T tf, T ts, T initial=T()) : Kp(kp), Ki(ki), Kd(kd), Tf(tf), IF(1, ts), DF(1, ts) {
		y = initial;
		IF.reset(y);
		DF.reset(0);
	}
	T update(T input) {
		x = y;
		y = Kp * x + Ki * IF.update(x) + Kd / (Tf + DF.update(x));
		return y;
	}
	void reset(T initial=T()) {
		y = initial;
		IF.reset(initial);
		DF.reset(0);
	}
};