From 93c107718fd6e323fa2336db885b26721f241fa4 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Tue, 6 Jun 2023 13:09:38 +0200 Subject: road detection using blobs and perspective correction --- nicla/consts.py | 6 ++++++ nicla/road.py | 56 ++++++++++++++++++++++++++++++++++++++++++++++++++++ nicla/serial_test.py | 46 ------------------------------------------ nicla/uart.py | 46 ++++++++++++++++++++++++++++++++++++++++++ 4 files changed, 108 insertions(+), 46 deletions(-) create mode 100644 nicla/consts.py create mode 100644 nicla/road.py delete mode 100644 nicla/serial_test.py create mode 100644 nicla/uart.py diff --git a/nicla/consts.py b/nicla/consts.py new file mode 100644 index 0000000..b5ccd9a --- /dev/null +++ b/nicla/consts.py @@ -0,0 +1,6 @@ +DUI_CMD_SIGN_START = 0x01 +DUI_CMD_SIGN_END = 0x0f +DUI_CMD_SPEED_START = 0x10 +DUI_CMD_SPEED_END = 0x1f +DUI_CMD_STEER_START = 0x20 +DUI_CMD_STEER_END = 0xff diff --git a/nicla/road.py b/nicla/road.py new file mode 100644 index 0000000..c5ffae3 --- /dev/null +++ b/nicla/road.py @@ -0,0 +1,56 @@ +import sensor, image, time, math +import uart +from consts import * + +sensor.reset() +sensor.set_pixformat(sensor.RGB565) +sensor.set_framesize(sensor.HVGA) +sensor.skip_frames(time = 2000) +clock = time.clock() + +WIDTH = 480 +HEIGHT = 320 + +HORIZON = 120 +STRETCH = 155 +SQUEEZE = 400 + +STEERING_ENTHOUSIASM = 7.0 +ROAD_MIN_BRIGHTNESS = 0xa0 + +points = [(STRETCH, HORIZON), + (WIDTH-1-STRETCH, HORIZON), + (WIDTH-1+SQUEEZE, HEIGHT-1), + (-SQUEEZE, HEIGHT-1)] + +def main(): + img = sensor.snapshot() + img.to_grayscale() + img.replace(vflip=True, hmirror=True) + img.rotation_corr(corners=points) + img.gaussian(3) + + offset_sum = 0.0 + offset_count = 0.0 + + for blob in img.find_blobs([(ROAD_MIN_BRIGHTNESS, 0xff)], pixels_threshold=100): + img.draw_rectangle(blob.rect()) + area_weight = blob.area() + horizontal_pos = (blob.x() + blob.w()/2) / WIDTH + offset_sum += horizontal_pos * area_weight + offset_count += area_weight + + # dit tegen niemand zeggen + if offset_count < 0.01: return + avg = offset_sum / offset_count + avg = avg * 2 - 1 + avg *= STEERING_ENTHOUSIASM + avg = max(-1, min(1, avg)) + + steerByte = int((avg + 1.0) * (DUI_CMD_STEER_END - DUI_CMD_STEER_START) / 2 + DUI_CMD_STEER_START) + uart.uart_buffer(steerByte) + +while(True): + main() + uart.uart_buffer(DUI_CMD_SPEED_END) + clock.tick() diff --git a/nicla/serial_test.py b/nicla/serial_test.py deleted file mode 100644 index 276f6d1..0000000 --- a/nicla/serial_test.py +++ /dev/null @@ -1,46 +0,0 @@ -from pyb import Pin, delay, udelay - -zumo_tx = Pin("PA10", Pin.IN) -zumo_rx = Pin("PA9", Pin.OUT_PP) - -def uart_send(byte): - zumo_rx.value(0) - udelay(1000) - for x in range(8): - bit = (byte & (1 << 7)) >> 7 - byte <<= 1 - zumo_rx.value(bit) - udelay(1000) - zumo_rx.value(1) - -__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/nicla/uart.py b/nicla/uart.py new file mode 100644 index 0000000..276f6d1 --- /dev/null +++ b/nicla/uart.py @@ -0,0 +1,46 @@ +from pyb import Pin, delay, udelay + +zumo_tx = Pin("PA10", Pin.IN) +zumo_rx = Pin("PA9", Pin.OUT_PP) + +def uart_send(byte): + zumo_rx.value(0) + udelay(1000) + for x in range(8): + bit = (byte & (1 << 7)) >> 7 + byte <<= 1 + zumo_rx.value(bit) + udelay(1000) + zumo_rx.value(1) + +__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) -- cgit v1.2.3