add PID controller

1 files changed, 10 insertions, 7 deletions

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);
 }