Refactor PID control to simplify the code and modifications

Each PID uses its internal dt, so may be various contexts with different rate.
PID has max dt, so no need to reset explicitly.

flix/pid.h

@@ -9,40 +9,44 @@
 
 class PID {
 public:
-	float p = 0;
-	float i = 0;
-	float d = 0;
-	float windup = 0;
+	float p, i, d;
+	float windup;
+	float dtMax;
 
 	float derivative = 0;
 	float integral = 0;
 
 	LowPassFilter<float> lpf; // low pass filter for derivative term
 
-	PID(float p, float i, float d, float windup = 0, float dAlpha = 1) : p(p), i(i), d(d), windup(windup), lpf(dAlpha) {};
+	PID(float p, float i, float d, float windup = 0, float dAlpha = 1, float dtMax = 0.1) :
+		p(p), i(i), d(d), windup(windup), lpf(dAlpha), dtMax(dtMax) {}
 
-	float update(float error, float dt) {
-		integral += error * dt;
+	float update(float error) {
+		float dt = t - prevTime;
 
-		if (isfinite(prevError) && dt > 0) {
-			// calculate derivative if both dt and prevError are valid
-			derivative = (error - prevError) / dt;
-
-			// apply low pass filter to derivative
-			derivative = lpf.update(derivative);
+		if (dt > 0 && dt < dtMax) {
+			integral += error * dt;
+			derivative = lpf.update((error - prevError) / dt); // compute derivative and apply low-pass filter
+		} else {
+			integral = 0;
+			derivative = 0;
 		}
 
 		prevError = error;
+		prevTime = t;
 
 		return p * error + constrain(i * integral, -windup, windup) + d * derivative; // PID
 	}
 
 	void reset() {
 		prevError = NAN;
+		prevTime = NAN;
 		integral = 0;
 		derivative = 0;
+		lpf.reset();
 	}
 
 private:
 	float prevError = NAN;
+	float prevTime = NAN;
 };