pid tuned

3 files changed, 37 insertions, 34 deletions

diff --git a/zumo/pid.cpp b/zumo/pid.cpp
index 9bef08f..fabe1da 100644
--- a/zumo/pid.cpp
+++ b/zumo/pid.cpp
@@ -1,9 +1,14 @@
 #include "pid.h"
 
 PID::PID(float P, float I, float D) {
-	A0 = P + I*dt + D/dt;
-	A1 = -P - 2*D/dt;
-	A2 = D/dt;
+	A0 = P + I * dt;
+	A1 = -P;
+	A0d = D / dt;
+	A1d = -2 * D / dt;
+	A2d = D / dt;
+	tau = D / (P * N);
+	alpha = dt / (2 * tau);
+	reset(0.0);
 }
 
 // https://en.wikipedia.org/wiki/PID_controller#Pseudocode
@@ -11,7 +16,17 @@ float PID::iter(float target) {
 	error[2] = error[1];
 	error[1] = error[0];
 	error[0] = target - output;
-	output = output + A0 * error[0] + A1 * error[1] + A2 * error[2];
+
+	output = output + A0 * error[0] + A1 * error[1];
+
+	d1 = d0;
+	d0 = A0d * error[0] + A1d * error[1] + A2d * error[2];
+	fd1 = fd0;
+	fd0 = ((alpha) / (alpha + 1)) * (d0 + d1) - ((alpha - 1) / (alpha + 1)) * fd1;
+	output = output + fd0;
+
+	if (output < -1) output = -1;
+	if (output > 1) output = 1;
 	return output;
 }
 
@@ -19,6 +34,10 @@ void PID::reset(float value) {
 	error[0] = 0.0;
 	error[1] = 0.0;
 	error[2] = 0.0;
+	d0 = 0;
+	d1 = 0;
+	fd0 = 0;
+	fd1 = 0;
 	output = value;
 }
 
diff --git a/zumo/pid.h b/zumo/pid.h
index fbcd063..ef733d6 100644
--- a/zumo/pid.h
+++ b/zumo/pid.h
@@ -4,10 +4,11 @@
 
 class PID {
 private:
-	float A0, A1, A2;
-	float error[3] = {0};
-	float dt = 0.010;
-	float output = 0;
+	float A0, A1, A0d, A1d, A2d, tau, alpha, d0, d1, fd0, fd1;
+	float error[3];
+	float output;
+	const float dt = 1.0;
+	const float N = 10.0;
 
 public:
 	PID(float P, float I, float D);
diff --git a/zumo/pidtest.cpp b/zumo/pidtest.cpp
index f5198bb..b9ce50b 100644
--- a/zumo/pidtest.cpp
+++ b/zumo/pidtest.cpp
@@ -3,33 +3,16 @@
 
 #include "pid.h"
 
-std::random_device rd;
-std::mt19937 rng(rd());
-std::uniform_real_distribution<float> uni(0,13);
-
-auto random_integer = uni(rng);
-
 int main() {
 	float P, I, D;
-	do {
-		// P = uni(rng);
-		// I = uni(rng);
-		// D = uni(rng);
-		P = 10;
-		I = 0.1;
-		D = 10;
-		PID test(P, I, D);
-		test.reset(0.0);
+	P = -0.02;
+	I = 0.13;
+	D = -300;
+	PID test(P, I, D);
+	test.reset(0.0);
 
-		float val = 0;
-		for (unsigned int i = 0; i < 100; i++) val = test.iter(1.0);
-		// if (val > 0.999 && val < 1.001) {
-			fprintf(stderr, "P: %.3f :: I: %.3f :: D: %.3f\n", P, I, D);
-			test.reset(0.0);
-			for (unsigned int i = 0; i < 100; i++) {
-				printf("%2.8f\n", test.iter(1.0));
-			}
-			exit(0);
-		// }
-	} while (false);
+	fprintf(stderr, "P: %.3f :: I: %.3f :: D: %.3f\n", P, I, D);
+	for (unsigned int i = 0; i < 100; i++) {
+		printf("%2.8f\n", test.iter(i < 50 ? 1.0 : 0.0));
+	}
 }