1 files changed, 43 insertions, 0 deletions
diff --git a/zumo/control.cpp b/zumo/control.cpp
new file mode 100644
index 0000000..778969a
--- /dev/null
+++ b/zumo/control.cpp
@@ -0,0 +1,43 @@
+#include <Zumo32U4Motors.h>
+#include <Arduino.h>
+#include <Wire.h>
+
+#include "protocol.h"
+#include "control.h"
+
+#define DUI_SPEED_MOD 96.0f
+#define DUI_MOTOR_DIFF 0.6f
+
+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;
+
+	Serial.print(motor_l);
+	Serial.print(" ");
+	Serial.print(motor_r);
+
+	Serial.println("");
+
+	Zumo32U4Motors::setLeftSpeed((int16_t) motor_l);
+	Zumo32U4Motors::setRightSpeed((int16_t) motor_r);
+
+	// TODO: print sign on OLED screen
+}
+
+inline bool rx() { return !digitalRead(DUI_PINOUT_NICLA_RX); }
+
+unsigned char uart_read() {
+	if (rx() == true) return 0x00; // return immediately if line is idle
+
+	delayMicroseconds(1200); // wait out start bit
+
+	unsigned char byte = 0x00;
+	for (unsigned int i = 0; i < 8; i++) {
+		byte = (byte << 1) | rx();
+		delayMicroseconds(1000);
+	}
+
+	delayMicroseconds(1000); // wait out stop bit
+
+	return byte;
+}