WIP nicla<->zumo integration

3 files changed, 53 insertions, 14 deletions

diff --git a/nicla/serial_test.py b/nicla/serial_test.py
index c8b84e5..276f6d1 100644
--- a/nicla/serial_test.py
+++ b/nicla/serial_test.py
@@ -13,10 +13,34 @@ def uart_send(byte):
         udelay(1000)
     zumo_rx.value(1)
 
-while True:
-    # uart_send("a")
-    for x in range(8):
-        n = 1 << x
-        uart_send(n)
-        print(f"0x{n:02x}")
+__uart_buffer = bytearray()
+def uart_flush():
+    global __uart_buffer
+    print("UART FLUSH START")
+    for byte in __uart_buffer:
+        print(f"BYTE 0x{byte:02X}")
+        uart_send(byte) # dit is de oplossing
+        udelay(2000)
+        uart_send(byte)
+        udelay(2000)
+        uart_send(byte)
+    __uart_buffer = bytearray()
+
+def tx_irq_handler(pin):
+    if pin is zumo_tx:
+        uart_flush()
+
+zumo_tx.irq(trigger = Pin.IRQ_RISING, handler = tx_irq_handler)
+
+def uart_buffer(i):
+    global __uart_buffer
+    __uart_buffer.append(i)
+
+if __name__ == "__main__":
+    while True: # test commands
+        uart_buffer(0x29)
+        uart_buffer(0x70)
+        delay(1000)
+        uart_buffer(0xff)
+        uart_buffer(0x20)
         delay(1000)
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);
 }