1 files changed, 39 insertions, 0 deletions
diff --git a/zumo/protocol.cpp b/zumo/protocol.cpp
new file mode 100644
index 0000000..fea8a00
--- /dev/null
+++ b/zumo/protocol.cpp
@@ -0,0 +1,39 @@
+#include <Zumo32U4Motors.h>
+
+#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
+#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) {
+	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);
+
+	// TODO: print sign on OLED screen
+}
+
+unsigned char uart_read() {
+	return 0x00;
+}