From 4568db72dc3f9694ecae3506fc3de6ffc948b24f Mon Sep 17 00:00:00 2001 From: lonkaars Date: Wed, 24 May 2023 15:51:25 +0200 Subject: WIP zumo<-nicla communication --- zumo/zumo.ino | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) (limited to 'zumo') diff --git a/zumo/zumo.ino b/zumo/zumo.ino index f59bd36..c65c2f9 100644 --- a/zumo/zumo.ino +++ b/zumo/zumo.ino @@ -5,6 +5,9 @@ #include "protocol.h" #include "pid.h" +#define DUI_PINOUT_NICLA_TX 13 +#define DUI_PINOUT_NICLA_RX 14 + dui_state_t g_dui_target_state = { .steer = 1.0f, .speed = 1.0f, @@ -19,6 +22,8 @@ dui_state_t g_dui_current_state = { }; void setup() { + pinMode(DUI_PINOUT_NICLA_TX, OUTPUT); + pinMode(DUI_PINOUT_NICLA_RX, INPUT_PULLUP); } void loop() { @@ -29,6 +34,4 @@ void loop() { apply_pid(&g_dui_target_state, &g_dui_current_state); apply_state(&g_dui_current_state); - - delay(10); } -- cgit v1.2.3 From cef95354c69745f782a95d37f4b0e7bbf02e106a Mon Sep 17 00:00:00 2001 From: lonkaars Date: Wed, 24 May 2023 18:03:05 +0200 Subject: uart working --- nicla/serial_test.py | 19 ++++++++++--------- zumo/.gitignore | 1 - zumo/pidtest.cpp | 18 ------------------ zumo/pidtest.mk | 21 --------------------- zumo/pidtest/.gitignore | 1 + zumo/pidtest/makefile | 21 +++++++++++++++++++++ zumo/pidtest/pidtest.cpp | 18 ++++++++++++++++++ zumo/protocol.cpp | 18 +++++++++++++++++- zumo/protocol.h | 3 +++ zumo/zumo.ino | 3 --- 10 files changed, 70 insertions(+), 53 deletions(-) delete mode 100644 zumo/pidtest.cpp delete mode 100644 zumo/pidtest.mk create mode 100644 zumo/pidtest/.gitignore create mode 100644 zumo/pidtest/makefile create mode 100644 zumo/pidtest/pidtest.cpp (limited to 'zumo') diff --git a/nicla/serial_test.py b/nicla/serial_test.py index bef43a2..c8b84e5 100644 --- a/nicla/serial_test.py +++ b/nicla/serial_test.py @@ -1,21 +1,22 @@ -from pyb import Pin, delay +from pyb import Pin, delay, udelay zumo_tx = Pin("PA10", Pin.IN) zumo_rx = Pin("PA9", Pin.OUT_PP) -def uart_send(s): +def uart_send(byte): zumo_rx.value(0) - byte = ord(s) - print("START BIT") - delay(2) + udelay(1000) for x in range(8): bit = (byte & (1 << 7)) >> 7 byte <<= 1 zumo_rx.value(bit) - print(f"BIT[{x}] = {bit}") - delay(2) - print("STOP BIT") + udelay(1000) zumo_rx.value(1) while True: - uart_send("a") + # uart_send("a") + for x in range(8): + n = 1 << x + uart_send(n) + print(f"0x{n:02x}") + delay(1000) diff --git a/zumo/.gitignore b/zumo/.gitignore index e45f7a2..5761abc 100644 --- a/zumo/.gitignore +++ b/zumo/.gitignore @@ -1,2 +1 @@ *.o -pidtest diff --git a/zumo/pidtest.cpp b/zumo/pidtest.cpp deleted file mode 100644 index b9ce50b..0000000 --- a/zumo/pidtest.cpp +++ /dev/null @@ -1,18 +0,0 @@ -#include -#include - -#include "pid.h" - -int main() { - float P, I, D; - P = -0.02; - I = 0.13; - D = -300; - PID test(P, I, D); - test.reset(0.0); - - fprintf(stderr, "P: %.3f :: I: %.3f :: D: %.3f\n", P, I, D); - for (unsigned int i = 0; i < 100; i++) { - printf("%2.8f\n", test.iter(i < 50 ? 1.0 : 0.0)); - } -} diff --git a/zumo/pidtest.mk b/zumo/pidtest.mk deleted file mode 100644 index 5ffe1e1..0000000 --- a/zumo/pidtest.mk +++ /dev/null @@ -1,21 +0,0 @@ -CPP = g++ -LD = g++ -RM = rm -f -CFLAGS = -LFLAGS = -TARGET = pidtest - -SRCS := pidtest.cpp pid.cpp -OBJS := pidtest.o pid.o - -all: pidtest - -%.o: %.cpp - $(CPP) -c $(CFLAGS) $< -o $@ - -$(TARGET): $(OBJS) - $(LD) $^ $(LFLAGS) -o $@ - -clean: - $(RM) $(TARGET) $(OBJS) - diff --git a/zumo/pidtest/.gitignore b/zumo/pidtest/.gitignore new file mode 100644 index 0000000..41dc46e --- /dev/null +++ b/zumo/pidtest/.gitignore @@ -0,0 +1 @@ +pidtest diff --git a/zumo/pidtest/makefile b/zumo/pidtest/makefile new file mode 100644 index 0000000..832d60b --- /dev/null +++ b/zumo/pidtest/makefile @@ -0,0 +1,21 @@ +CPP = g++ +LD = g++ +RM = rm -f +CFLAGS = +LFLAGS = +TARGET = pidtest + +SRCS := pidtest.cpp ../pid.cpp +OBJS := pidtest.o ../pid.o + +all: pidtest + +%.o: %.cpp + $(CPP) -c $(CFLAGS) $< -o $@ + +$(TARGET): $(OBJS) + $(LD) $^ $(LFLAGS) -o $@ + +clean: + $(RM) $(TARGET) $(OBJS) + diff --git a/zumo/pidtest/pidtest.cpp b/zumo/pidtest/pidtest.cpp new file mode 100644 index 0000000..4d047fb --- /dev/null +++ b/zumo/pidtest/pidtest.cpp @@ -0,0 +1,18 @@ +#include +#include + +#include "../pid.h" + +int main() { + float P, I, D; + P = -0.02; + I = 0.13; + D = -300; + PID test(P, I, D); + test.reset(0.0); + + fprintf(stderr, "P: %.3f :: I: %.3f :: D: %.3f\n", P, I, D); + for (unsigned int i = 0; i < 100; i++) { + printf("%2.8f\n", test.iter(i < 50 ? 1.0 : 0.0)); + } +} diff --git a/zumo/protocol.cpp b/zumo/protocol.cpp index fea8a00..f11827c 100644 --- a/zumo/protocol.cpp +++ b/zumo/protocol.cpp @@ -1,4 +1,6 @@ #include +#include +#include #include "protocol.h" @@ -34,6 +36,20 @@ void apply_state(dui_state_t *state) { // TODO: print sign on OLED screen } +inline bool rx() { return !digitalRead(DUI_PINOUT_NICLA_RX); } + unsigned char uart_read() { - return 0x00; + if (rx() == true) return 0x00; // return immediately if line is idle + + delayMicroseconds(1500); // 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; } diff --git a/zumo/protocol.h b/zumo/protocol.h index 662a5ce..9db7902 100644 --- a/zumo/protocol.h +++ b/zumo/protocol.h @@ -1,5 +1,8 @@ #pragma once +#define DUI_PINOUT_NICLA_TX 13 +#define DUI_PINOUT_NICLA_RX 14 + typedef enum { DUI_CMD_NULL, DUI_CMD_SIGN, diff --git a/zumo/zumo.ino b/zumo/zumo.ino index c65c2f9..e53513b 100644 --- a/zumo/zumo.ino +++ b/zumo/zumo.ino @@ -5,9 +5,6 @@ #include "protocol.h" #include "pid.h" -#define DUI_PINOUT_NICLA_TX 13 -#define DUI_PINOUT_NICLA_RX 14 - dui_state_t g_dui_target_state = { .steer = 1.0f, .speed = 1.0f, -- cgit v1.2.3 From d0a1cc366e1657b4d65bd5c3bef35f4173ef2b15 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Wed, 24 May 2023 20:51:23 +0200 Subject: fix protocol implementation --- zumo/control.cpp | 37 +++++++++++++++++++++++++++++ zumo/control.h | 12 ++++++++++ zumo/protocol.cpp | 48 ++++++-------------------------------- zumo/protocol.h | 7 ------ zumo/protocoltest/.gitignore | 1 + zumo/protocoltest/makefile | 21 +++++++++++++++++ zumo/protocoltest/protocoltest.cpp | 35 +++++++++++++++++++++++++++ 7 files changed, 113 insertions(+), 48 deletions(-) create mode 100644 zumo/control.cpp create mode 100644 zumo/control.h create mode 100644 zumo/protocoltest/.gitignore create mode 100644 zumo/protocoltest/makefile create mode 100644 zumo/protocoltest/protocoltest.cpp (limited to 'zumo') diff --git a/zumo/control.cpp b/zumo/control.cpp new file mode 100644 index 0000000..263c145 --- /dev/null +++ b/zumo/control.cpp @@ -0,0 +1,37 @@ +#include +#include +#include + +#include "protocol.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; + + 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(1500); // 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; +} + diff --git a/zumo/control.h b/zumo/control.h new file mode 100644 index 0000000..12ecbd7 --- /dev/null +++ b/zumo/control.h @@ -0,0 +1,12 @@ +#pragma once + +#include "protocol.h" + +#define DUI_PINOUT_NICLA_TX 13 +#define DUI_PINOUT_NICLA_RX 14 + +/** @brief non blocking read byte */ +unsigned char uart_read(); +/** @brief apply state to motors */ +void apply_state(dui_state_t* state); + diff --git a/zumo/protocol.cpp b/zumo/protocol.cpp index f11827c..ab8397a 100644 --- a/zumo/protocol.cpp +++ b/zumo/protocol.cpp @@ -1,55 +1,21 @@ -#include -#include -#include - #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 +#define DUI_CMD_SPEED_START 0x10 +#define DUI_CMD_SPEED_END 0x1f +#define DUI_CMD_STEER_START 0x20 +#define DUI_CMD_STEER_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); + state->speed = (float) (cmd - DUI_CMD_SPEED_START) / (float) (DUI_CMD_SPEED_END - DUI_CMD_SPEED_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) * (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 -} - -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 - - 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; -} diff --git a/zumo/protocol.h b/zumo/protocol.h index 9db7902..a1f9951 100644 --- a/zumo/protocol.h +++ b/zumo/protocol.h @@ -1,8 +1,5 @@ #pragma once -#define DUI_PINOUT_NICLA_TX 13 -#define DUI_PINOUT_NICLA_RX 14 - typedef enum { DUI_CMD_NULL, DUI_CMD_SIGN, @@ -29,10 +26,6 @@ typedef struct { float speed_mod; /** @brief global speed multiplier */ } dui_state_t; -/** @brief non blocking read byte */ -unsigned char uart_read(); /** @brief read and apply cmd to state */ void handle_cmd(unsigned char cmd, dui_state_t *state); -/** @brief apply state to motors */ -void apply_state(dui_state_t* state); diff --git a/zumo/protocoltest/.gitignore b/zumo/protocoltest/.gitignore new file mode 100644 index 0000000..fb5d35f --- /dev/null +++ b/zumo/protocoltest/.gitignore @@ -0,0 +1 @@ +protocoltest diff --git a/zumo/protocoltest/makefile b/zumo/protocoltest/makefile new file mode 100644 index 0000000..7a69352 --- /dev/null +++ b/zumo/protocoltest/makefile @@ -0,0 +1,21 @@ +CPP = g++ +LD = g++ +RM = rm -f +CFLAGS = +LFLAGS = +TARGET = protocoltest + +SRCS := $(TARGET).cpp ../protocol.cpp +OBJS := $(TARGET).o ../protocol.o + +all: $(TARGET) + +%.o: %.cpp + $(CPP) -c $(CFLAGS) $< -o $@ + +$(TARGET): $(OBJS) + $(LD) $^ $(LFLAGS) -o $@ + +clean: + $(RM) $(TARGET) $(OBJS) + diff --git a/zumo/protocoltest/protocoltest.cpp b/zumo/protocoltest/protocoltest.cpp new file mode 100644 index 0000000..461221e --- /dev/null +++ b/zumo/protocoltest/protocoltest.cpp @@ -0,0 +1,35 @@ +#include +#include + +#include "../protocol.h" + +void print_state(dui_state_t* s) { + printf("{ steer = %1.3f\n speed = %1.3f }\n", s->steer, s->speed); +} + +void test_case(unsigned char cmd, dui_state_t* s) { + printf("\ncmd in = 0x%02x\n", cmd); + handle_cmd(cmd, s); + print_state(s); +} + +int main() { + dui_state_t state = { + .steer = 0.f, + .speed = 1.f, + .current_sign = DUI_SIGN_NONE, + .speed_mod = 1.f, + }; + + test_case(0x00, &state); + test_case(0x01, &state); + test_case(0x02, &state); + test_case(0x10, &state); + test_case(0x15, &state); + test_case(0x1f, &state); + test_case(0x20, &state); + test_case(0x25, &state); + test_case(0x2f, &state); + test_case(0x88, &state); + test_case(0xff, &state); +} -- cgit v1.2.3 From f6cf3917fc7982e1bfe0aef295c5b228199d78d4 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Wed, 24 May 2023 21:56:32 +0200 Subject: WIP nicla<->zumo integration --- nicla/serial_test.py | 36 ++++++++++++++++++++++++++++++------ zumo/control.cpp | 10 ++++++++-- zumo/zumo.ino | 21 +++++++++++++++------ 3 files changed, 53 insertions(+), 14 deletions(-) (limited to 'zumo') 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 #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 #include +#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); } -- cgit v1.2.3