1 files changed, 34 insertions, 0 deletions
diff --git a/zumo/zumo.ino b/zumo/zumo.ino
new file mode 100644
index 0000000..f59bd36
--- /dev/null
+++ b/zumo/zumo.ino
@@ -0,0 +1,34 @@
+#include <Zumo32U4.h>
+#include <Arduino.h>
+#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 = 1.f,
+};
+dui_state_t g_dui_current_state = {
+	.steer = 0.f,
+	.speed = 1.f,
+	.current_sign = DUI_SIGN_NONE,
+	.speed_mod = 1.f,
+};
+
+void setup() {
+}
+
+void loop() {
+	unsigned char cmd = 0x00;
+	while ((cmd = uart_read()))
+		handle_cmd(cmd, &g_dui_target_state);
+
+	apply_pid(&g_dui_target_state, &g_dui_current_state);
+
+	apply_state(&g_dui_current_state);
+
+	delay(10);
+}