add PID controller

4 files changed, 58 insertions, 10 deletions

diff --git a/zumo/pid.cpp b/zumo/pid.cpp
new file mode 100644
index 0000000..594d136
--- /dev/null
+++ b/zumo/pid.cpp
@@ -0,0 +1,36 @@
+#include "pid.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) {
+		A0 = P + I*dt + D/dt;
+		A1 = -P - 2*D/dt;
+		A2 = D/dt;
+	}
+
+	// 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;
+	}
+};
+
+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_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
new file mode 100644
index 0000000..2cdbc91
--- /dev/null
+++ b/zumo/pid.h
@@ -0,0 +1,7 @@
+#pragma once
+
+#include "protocol.h"
+
+/** @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/protocol.cpp b/zumo/protocol.cpp
index a0c72f0..fea8a00 100644
--- a/zumo/protocol.cpp
+++ b/zumo/protocol.cpp
@@ -2,6 +2,9 @@
 
 #include "protocol.h"
 
+#define DUI_SPEED_MOD 96.0f
+#define DUI_MOTOR_DIFF 0.6f
+
 #define DUI_CMD_NULL 0x00
 #define DUI_CMD_SIGN_START 0x01
 #define DUI_CMD_SIGN_END 0x0f
@@ -22,9 +25,8 @@ void handle_cmd(unsigned char cmd, dui_state_t *state) {
 }
 
 void apply_state(dui_state_t *state) {
-	const float MAX_MOTOR_DIFF = 0.6f; // 0 to 1
-	float motor_l = 0.5f * state->speed * (+1.f * state->steer * MAX_MOTOR_DIFF - MAX_MOTOR_DIFF + 2) * state->speed_mod;
-	float motor_r = 0.5f * state->speed * (-1.f * state->steer * MAX_MOTOR_DIFF - MAX_MOTOR_DIFF + 2) * state->speed_mod;
+	float motor_l = 0.5f * state->speed * (+1.f * state->steer * DUI_MOTOR_DIFF - DUI_MOTOR_DIFF + 2) * state->speed_mod * DUI_SPEED_MOD;
+	float motor_r = 0.5f * state->speed * (-1.f * state->steer * DUI_MOTOR_DIFF - DUI_MOTOR_DIFF + 2) * state->speed_mod * DUI_SPEED_MOD;
 
 	Zumo32U4Motors::setLeftSpeed((int16_t) motor_l);
 	Zumo32U4Motors::setRightSpeed((int16_t) motor_r);
diff --git a/zumo/zumo.ino b/zumo/zumo.ino
index ed63f6e..f59bd36 100644
--- a/zumo/zumo.ino
+++ b/zumo/zumo.ino
@@ -3,29 +3,32 @@
 #include <Wire.h>
 
 #include "protocol.h"
+#include "pid.h"
 
 dui_state_t g_dui_target_state = {
 	.steer = 1.0f,
 	.speed = 1.0f,
 	.current_sign = DUI_SIGN_NONE,
-	.speed_mod =96.0,
+	.speed_mod = 1.f,
 };
 dui_state_t g_dui_current_state = {
-	.steer = 0,
-	.speed = 0,
+	.steer = 0.f,
+	.speed = 1.f,
 	.current_sign = DUI_SIGN_NONE,
-	.speed_mod = 1.0,
+	.speed_mod = 1.f,
 };
 
 void setup() {
 }
 
 void loop() {
-	unsigned char cmd = 0;
+	unsigned char cmd = 0x00;
 	while ((cmd = uart_read()))
 		handle_cmd(cmd, &g_dui_target_state);
 
-	//TODO: PID controllers + sign handlers
+	apply_pid(&g_dui_target_state, &g_dui_current_state);
 
-	apply_state(&g_dui_target_state);
+	apply_state(&g_dui_current_state);
+
+	delay(10);
 }