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

pidstd.h

#pragma once
#include <integrator.h>
template<typename T>
class PIDStd {
	T Kp;
	T Ti;
	T Td;
	T N;
	T Ts;
	T x;
	T y;
	DescreteTimeIntegrator<T> IF;
	DescreteTimeIntegrator<T> DF;
public:
	PIDStd(T kp, T ti, T td, T n, T ts, T initial=T()) : Kp(kp), Ti(ti), Td(td), N(n), IF(1, ts), DF(1, ts) {
		y = initial;
		IF.reset(y);
		DF.reset(0);
	}
	T update(T input) {
		x = y;
		y = Kp * ( x + 1 / Ti * IF.update(x) + Td / (Td / N + DF.update(x)) );
		return y;
	}
	void reset(T initial=T()) {
		y = initial;
		IF.reset(initial);
		DF.reset(0);
	}
};