4 files changed, 103 insertions, 13 deletions
diff --git a/zumo/main.cpp b/zumo/main.cpp
deleted file mode 100644
index 23d13db..0000000
--- a/zumo/main.cpp
+++ /dev/null
@@ -1,13 +0,0 @@
-#include <Zumo32U4.h>
-#include <Arduino.h>
-#include <Wire.h>
-
-void setup() {
-}
-
-void loop() {
-	ledRed(1);
-	delay(1000);
-	ledRed(0);
-	delay(1000);
-}
diff --git a/zumo/protocol.cpp b/zumo/protocol.cpp
new file mode 100644
index 0000000..a0c72f0
--- /dev/null
+++ b/zumo/protocol.cpp
@@ -0,0 +1,37 @@
+#include <Zumo32U4Motors.h>
+
+#include "protocol.h"
+
+#define DUI_CMD_NULL 0x00
+#define DUI_CMD_SIGN_START 0x01
+#define DUI_CMD_SIGN_END 0x0f
+#define DUI_CMD_STEER_START 0x10
+#define DUI_CMD_STEER_END 0x1f
+#define DUI_CMD_SPEED_START 0x20
+#define DUI_CMD_SPEED_END 0xff
+
+void handle_cmd(unsigned char cmd, dui_state_t *state) {
+	if (cmd == DUI_CMD_NULL) return;
+	else if (DUI_CMD_SIGN_START <= cmd && cmd <= DUI_CMD_SIGN_END) {
+		state->current_sign = (dui_e_sign) (cmd - DUI_CMD_SIGN_START);
+	} else if (DUI_CMD_STEER_START <= cmd && cmd <= DUI_CMD_STEER_END) {
+		state->steer = (float) (cmd - DUI_CMD_STEER_START) / (float) (DUI_CMD_STEER_END - DUI_CMD_STEER_START);
+	} else if (DUI_CMD_SPEED_START <= cmd && cmd <= DUI_CMD_SPEED_END) {
+		state->speed = ((float) (cmd - DUI_CMD_SPEED_START) / (float) (DUI_CMD_SPEED_START - DUI_CMD_SPEED_END) * (float) 2 - (float) 1);
+	}
+}
+
+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;
+
+	Zumo32U4Motors::setLeftSpeed((int16_t) motor_l);
+	Zumo32U4Motors::setRightSpeed((int16_t) motor_r);
+
+	// TODO: print sign on OLED screen
+}
+
+unsigned char uart_read() {
+	return 0x00;
+}
diff --git a/zumo/protocol.h b/zumo/protocol.h
new file mode 100644
index 0000000..662a5ce
--- /dev/null
+++ b/zumo/protocol.h
@@ -0,0 +1,35 @@
+#pragma once
+
+typedef enum {
+	DUI_CMD_NULL,
+	DUI_CMD_SIGN,
+	DUI_CMD_SPEED,
+	DUI_CMD_STEER,
+} dui_e_cmd;
+
+typedef enum {
+	DUI_SIGN_NONE, /** @brief no sign */
+	DUI_SIGN_STOP, /** @brief stop (set speed to 0) */
+	DUI_SIGN_LEFT, /** @brief turn left (set steer to -1) */
+	DUI_SIGN_RIGHT, /** @brief turn right (set steer to +1) */
+	DUI_SIGN_SPEED_LIMIT_LOW, /** @brief slow down (speed limit 0.5) */
+	DUI_SIGN_SPEED_LIMIT_HIGH, /** @brief full speed (speed limit 1.0) */
+	DUI_SIGN_LIGHT_STOP, /** @brief traffic light red (set speed to 0) */
+	DUI_SIGN_LIGHT_FLOOR_IT, /** @brief traffic light orange (set speed to 2 temporarily) */
+	DUI_SIGN_LIGHT_GO, /** @brief traffic light green (keep current speed) */
+} dui_e_sign;
+
+typedef struct {
+	float steer; /** @brief steer value (-1 is left, +1 is right) */
+	float speed; /** @brief speed (0-15) */
+	dui_e_sign current_sign; /** @brief last seen sign */
+	float speed_mod; /** @brief global speed multiplier */
+} dui_state_t;
+
+/** @brief non blocking read byte */
+unsigned char uart_read();
+/** @brief read and apply cmd to state */
+void handle_cmd(unsigned char cmd, dui_state_t *state);
+/** @brief apply state to motors */
+void apply_state(dui_state_t* state);
+
diff --git a/zumo/zumo.ino b/zumo/zumo.ino
new file mode 100644
index 0000000..ed63f6e
--- /dev/null
+++ b/zumo/zumo.ino
@@ -0,0 +1,31 @@
+#include <Zumo32U4.h>
+#include <Arduino.h>
+#include <Wire.h>
+
+#include "protocol.h"
+
+dui_state_t g_dui_target_state = {
+	.steer = 1.0f,
+	.speed = 1.0f,
+	.current_sign = DUI_SIGN_NONE,
+	.speed_mod =96.0,
+};
+dui_state_t g_dui_current_state = {
+	.steer = 0,
+	.speed = 0,
+	.current_sign = DUI_SIGN_NONE,
+	.speed_mod = 1.0,
+};
+
+void setup() {
+}
+
+void loop() {
+	unsigned char cmd = 0;
+	while ((cmd = uart_read()))
+		handle_cmd(cmd, &g_dui_target_state);
+
+	//TODO: PID controllers + sign handlers
+
+	apply_state(&g_dui_target_state);
+}