2 files changed, 23 insertions, 8 deletions

diff --git a/zumo/control.cpp b/zumo/control.cpp
index 263c145..778969a 100644
--- a/zumo/control.cpp
+++ b/zumo/control.cpp
@@ -3,6 +3,7 @@
 #include <Wire.h>
 
 #include "protocol.h"
+#include "control.h"
 
 #define DUI_SPEED_MOD 96.0f
 #define DUI_MOTOR_DIFF 0.6f
@@ -11,6 +12,12 @@ 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);
 
@@ -22,7 +29,7 @@ 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(1500); // wait out start bit
+	delayMicroseconds(1200); // wait out start bit
 
 	unsigned char byte = 0x00;
 	for (unsigned int i = 0; i < 8; i++) {
@@ -34,4 +41,3 @@ unsigned char uart_read() {
 
 	return byte;
 }
-
diff --git a/zumo/zumo.ino b/zumo/zumo.ino
index e53513b..1a64381 100644
--- a/zumo/zumo.ino
+++ b/zumo/zumo.ino
@@ -2,18 +2,19 @@
 #include <Arduino.h>
 #include <Wire.h>
 
+#include "control.h"
 #include "protocol.h"
 #include "pid.h"
 
 dui_state_t g_dui_target_state = {
-	.steer = 1.0f,
-	.speed = 1.0f,
+	.steer = 0.0f,
+	.speed = 0.0f,
 	.current_sign = DUI_SIGN_NONE,
 	.speed_mod = 1.f,
 };
 dui_state_t g_dui_current_state = {
 	.steer = 0.f,
-	.speed = 1.f,
+	.speed = 0.f,
 	.current_sign = DUI_SIGN_NONE,
 	.speed_mod = 1.f,
 };
@@ -21,14 +22,22 @@ dui_state_t g_dui_current_state = {
 void setup() {
 	pinMode(DUI_PINOUT_NICLA_TX, OUTPUT);
 	pinMode(DUI_PINOUT_NICLA_RX, INPUT_PULLUP);
+	Serial.begin(115200);
 }
 
 void loop() {
-	unsigned char cmd = 0x00;
-	while ((cmd = uart_read()))
-		handle_cmd(cmd, &g_dui_target_state);
+	static unsigned char cmd_old = 0x00;
+	for (unsigned int i = 0; i < 1000; i++) {
+		digitalWrite(DUI_PINOUT_NICLA_TX, LOW);
+		unsigned char cmd = uart_read();
+		if (cmd == 0x00) continue;
+		if (cmd == cmd_old) handle_cmd(cmd, &g_dui_target_state);
+		cmd_old = cmd;
+	}
+	digitalWrite(DUI_PINOUT_NICLA_TX, HIGH);
 
 	apply_pid(&g_dui_target_state, &g_dui_current_state);
+	g_dui_current_state.current_sign = g_dui_target_state.current_sign;
 
 	apply_state(&g_dui_current_state);
 }