diff options
| -rw-r--r-- | nicla/serial_test.py | 36 | ||||
| -rw-r--r-- | zumo/control.cpp | 10 | ||||
| -rw-r--r-- | zumo/zumo.ino | 21 |
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); } |