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 --- nicla/serial_test.py | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) create mode 100644 nicla/serial_test.py (limited to 'nicla') diff --git a/nicla/serial_test.py b/nicla/serial_test.py new file mode 100644 index 0000000..bef43a2 --- /dev/null +++ b/nicla/serial_test.py @@ -0,0 +1,21 @@ +from pyb import Pin, delay + +zumo_tx = Pin("PA10", Pin.IN) +zumo_rx = Pin("PA9", Pin.OUT_PP) + +def uart_send(s): + zumo_rx.value(0) + byte = ord(s) + print("START BIT") + delay(2) + 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") + zumo_rx.value(1) + +while True: + uart_send("a") -- 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 'nicla') 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 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 'nicla') 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 From 0f764db3c3595e863a4949c67592451c7d65a2cf Mon Sep 17 00:00:00 2001 From: lonkaars Date: Mon, 5 Jun 2023 11:55:09 +0200 Subject: shaky traffic light detection in micropython --- nicla/traffic_light.py | 65 ++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 65 insertions(+) create mode 100644 nicla/traffic_light.py (limited to 'nicla') diff --git a/nicla/traffic_light.py b/nicla/traffic_light.py new file mode 100644 index 0000000..3d81139 --- /dev/null +++ b/nicla/traffic_light.py @@ -0,0 +1,65 @@ +import sensor, image, time, math + +sensor.reset() +sensor.set_pixformat(sensor.RGB565) +sensor.set_framesize(sensor.QVGA) +sensor.skip_frames(time = 2000) +clock = time.clock() + +""" +returns hsv tuple for rgb input tuple +""" +def rgb2hsv(rgb): + r, g, b = rgb + maxc = max(r, g, b) + minc = min(r, g, b) + rangec = (maxc-minc) + v = maxc + if minc == maxc: + return 0.0, 0.0, v + s = rangec / maxc + rc = (maxc-r) / rangec + gc = (maxc-g) / rangec + bc = (maxc-b) / rangec + if r == maxc: + h = bc-gc + elif g == maxc: + h = 2.0+rc-bc + else: + h = 4.0+gc-rc + h = (h/6.0) % 1.0 + return (h, s, v) + +while(True): + clock.tick() + img = sensor.snapshot() + ## todo: downsample img + original = img.copy(copy_to_fb=True) + img = img.to_grayscale() + for blob in img.find_blobs([(0, 60)], pixels_threshold=100): + aspect = blob.h() / blob.w() + if abs(aspect - 2.2) > 0.5: continue + lights = ( + (round(blob.x() + blob.w() / 2), round(blob.y() + 0.8 * blob.h())), + (round(blob.x() + blob.w() / 2), round(blob.y() + 0.5 * blob.h())), + (round(blob.x() + blob.w() / 2), round(blob.y() + 0.2 * blob.h())), + ) + + light_status = 0 + for i, light in enumerate(lights): + r, g, b = original.get_pixel(light[0], light[1]) + h, s, v = rgb2hsv(((r/255),(g/255),(b/255),)) + if s < 0.65: continue + # if v < 0.3: continue + if i == 0 and abs(h - 0.50) < 0.45: continue + if i == 1 and abs(h - 0.05) > 0.1: continue + if i == 2 and abs(h - 0.40) > 0.1: continue + light_status = i + 1 + print((h,s,v,)) + break + if light_status == 0: + continue + + img.draw_rectangle(blob.rect()) + img.draw_circle(lights[light_status-1][0], lights[light_status-1][1], 2) + print(("", "rood", "geel", "groen")[light_status]) -- cgit v1.2.3