Merge branch 'lonkaars:master' into master

1 files changed, 21 insertions, 0 deletions

diff --git a/zumo/pid.h b/zumo/pid.h
new file mode 100644
index 0000000..beb73ac
--- /dev/null
+++ b/zumo/pid.h
@@ -0,0 +1,21 @@
+#pragma once
+
+#include "protocol.h"
+
+class PID {
+private:
+	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 = -0.02, float I = 0.13, float D = -300.0);
+	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);
+