From ff7a914055cf851c994ee037342a331902f84a0c Mon Sep 17 00:00:00 2001
From: lonkaars <loek@pipeframe.xyz>
Date: Fri, 19 May 2023 13:34:31 +0200
Subject: add pidtest

---
 zumo/.gitignore  |  1 +
 zumo/pid.cpp     | 44 +++++++++++++++++++++-----------------------
 zumo/pid.h       | 13 +++++++++++++
 zumo/pidtest.cpp | 35 +++++++++++++++++++++++++++++++++++
 zumo/pidtest.mk  | 21 +++++++++++++++++++++
 5 files changed, 91 insertions(+), 23 deletions(-)
 create mode 100644 zumo/pidtest.cpp
 create mode 100644 zumo/pidtest.mk

diff --git a/zumo/.gitignore b/zumo/.gitignore
index d3e1535..888e905 100644
--- a/zumo/.gitignore
+++ b/zumo/.gitignore
@@ -3,3 +3,4 @@ main
 *.hex
 compile_commands.json
 .cache
+pidtest
diff --git a/zumo/pid.cpp b/zumo/pid.cpp
index 594d136..9bef08f 100644
--- a/zumo/pid.cpp
+++ b/zumo/pid.cpp
@@ -1,35 +1,33 @@
 #include "pid.h"
 
-class PID {
-private:
-	float A0, A1, A2;
-	float error[3] = {0};
-	float dt = 0.010;
-	float output = 0;
+PID::PID(float P, float I, float D) {
+	A0 = P + I*dt + D/dt;
+	A1 = -P - 2*D/dt;
+	A2 = D/dt;
+}
 
-public:
-	PID(float P, float I, float D) {
-		A0 = P + I*dt + D/dt;
-		A1 = -P - 2*D/dt;
-		A2 = D/dt;
-	}
+// https://en.wikipedia.org/wiki/PID_controller#Pseudocode
+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];
+	return output;
+}
 
-	// https://en.wikipedia.org/wiki/PID_controller#Pseudocode
-	float iter(float current, float target) {
-		error[2] = error[1];
-		error[1] = error[0];
-		error[0] = target - current;
-		output = output + A0 * error[0] + A1 * error[1] + A2 * error[2];
-		return output;
-	}
-};
+void PID::reset(float value) {
+	error[0] = 0.0;
+	error[1] = 0.0;
+	error[2] = 0.0;
+	output = value;
+}
 
 PID speed_pid(0.1, 0.0, 0.0); // TODO: tune these (garbage values)
 PID steer_pid(0.1, 0.0, 0.0);
 PID speed_mod_pid(1, 1, 1);
 void apply_pid(dui_state_t* target, dui_state_t* current) {
-	current->speed = speed_pid.iter(current->speed, target->speed);
-	current->steer = steer_pid.iter(current->steer, target->steer);
+	current->speed = speed_pid.iter(target->speed);
+	current->steer = steer_pid.iter(target->steer);
 	// current->speed_mod = speed_mod_pid.iter(current->speed_mod, target->speed_mod);
 	current->speed_mod = target->speed_mod;
 }
diff --git a/zumo/pid.h b/zumo/pid.h
index 2cdbc91..fbcd063 100644
--- a/zumo/pid.h
+++ b/zumo/pid.h
@@ -2,6 +2,19 @@
 
 #include "protocol.h"
 
+class PID {
+private:
+	float A0, A1, A2;
+	float error[3] = {0};
+	float dt = 0.010;
+	float output = 0;
+
+public:
+	PID(float P, float I, float D);
+	float iter(float target);
+	void reset(float value);
+};
+
 /** @brief edit `current` to be closer to `target` using PID controllers */
 void apply_pid(dui_state_t* target, dui_state_t* current);
 
diff --git a/zumo/pidtest.cpp b/zumo/pidtest.cpp
new file mode 100644
index 0000000..f5198bb
--- /dev/null
+++ b/zumo/pidtest.cpp
@@ -0,0 +1,35 @@
+#include <cstdio>
+#include <random>
+
+#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);
+
+		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);
+}
diff --git a/zumo/pidtest.mk b/zumo/pidtest.mk
new file mode 100644
index 0000000..5ffe1e1
--- /dev/null
+++ b/zumo/pidtest.mk
@@ -0,0 +1,21 @@
+CPP = g++
+LD = g++
+RM = rm -f
+CFLAGS =
+LFLAGS =
+TARGET = pidtest
+
+SRCS := pidtest.cpp pid.cpp
+OBJS := pidtest.o pid.o
+
+all: pidtest
+
+%.o: %.cpp
+	$(CPP) -c $(CFLAGS) $< -o $@
+
+$(TARGET): $(OBJS)
+	$(LD) $^ $(LFLAGS) -o $@
+
+clean:
+	$(RM) $(TARGET) $(OBJS)
+
-- 
cgit v1.2.3