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 From 7118317a18e3024b341d14e72a9e741a7b14b2e5 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Tue, 6 Jun 2023 13:48:57 +0200 Subject: don't print uart messages constantly and fix for newly bent camera --- nicla/road.py | 7 ++++--- nicla/uart.py | 5 +++-- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/nicla/road.py b/nicla/road.py index c5ffae3..ceabc3b 100644 --- a/nicla/road.py +++ b/nicla/road.py @@ -11,11 +11,11 @@ clock = time.clock() WIDTH = 480 HEIGHT = 320 -HORIZON = 120 -STRETCH = 155 +HORIZON = 150 +STRETCH = 40 SQUEEZE = 400 -STEERING_ENTHOUSIASM = 7.0 +STEERING_ENTHOUSIASM = 3.0 ROAD_MIN_BRIGHTNESS = 0xa0 points = [(STRETCH, HORIZON), @@ -47,6 +47,7 @@ def main(): avg *= STEERING_ENTHOUSIASM avg = max(-1, min(1, avg)) + print(avg) steerByte = int((avg + 1.0) * (DUI_CMD_STEER_END - DUI_CMD_STEER_START) / 2 + DUI_CMD_STEER_START) uart.uart_buffer(steerByte) diff --git a/nicla/uart.py b/nicla/uart.py index 276f6d1..da150b0 100644 --- a/nicla/uart.py +++ b/nicla/uart.py @@ -16,14 +16,15 @@ def uart_send(byte): __uart_buffer = bytearray() def uart_flush(): global __uart_buffer - print("UART FLUSH START") + # print("UART FLUSH START") for byte in __uart_buffer: - print(f"BYTE 0x{byte:02X}") + # print(f"BYTE 0x{byte:02X}") uart_send(byte) # dit is de oplossing udelay(2000) uart_send(byte) udelay(2000) uart_send(byte) + udelay(2000) __uart_buffer = bytearray() def tx_irq_handler(pin): -- cgit v1.2.3 From bdf59e4c3680987591bdf0234220922e5e41fc5a Mon Sep 17 00:00:00 2001 From: heavydemon21 Date: Tue, 6 Jun 2023 13:53:10 +0200 Subject: first integration --- nicla/road.py | 13 ++++++++++--- nicla/traffic_light.py | 9 ++++----- 2 files changed, 14 insertions(+), 8 deletions(-) diff --git a/nicla/road.py b/nicla/road.py index c5ffae3..8d60dff 100644 --- a/nicla/road.py +++ b/nicla/road.py @@ -1,5 +1,6 @@ import sensor, image, time, math import uart +import traffic_light from consts import * sensor.reset() @@ -23,8 +24,8 @@ points = [(STRETCH, HORIZON), (WIDTH-1+SQUEEZE, HEIGHT-1), (-SQUEEZE, HEIGHT-1)] -def main(): - img = sensor.snapshot() +def drive(driveImg): + img = driveImg.copy() img.to_grayscale() img.replace(vflip=True, hmirror=True) img.rotation_corr(corners=points) @@ -49,8 +50,14 @@ def main(): steerByte = int((avg + 1.0) * (DUI_CMD_STEER_END - DUI_CMD_STEER_START) / 2 + DUI_CMD_STEER_START) uart.uart_buffer(steerByte) + sensor.dealloc_extra_fb() while(True): - main() + img = sensor.snapshot() + data = traffic_light.traf_lights(img) + if data is not None: + print(data) + uart.uart_buffer(data) + drive(img) uart.uart_buffer(DUI_CMD_SPEED_END) clock.tick() diff --git a/nicla/traffic_light.py b/nicla/traffic_light.py index 3d81139..517e2c3 100644 --- a/nicla/traffic_light.py +++ b/nicla/traffic_light.py @@ -30,11 +30,9 @@ def rgb2hsv(rgb): 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) + +def traf_lights(img) + original = img.copy() img = img.to_grayscale() for blob in img.find_blobs([(0, 60)], pixels_threshold=100): aspect = blob.h() / blob.w() @@ -63,3 +61,4 @@ while(True): 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]) + sensor.dealloc_extra_fb() -- cgit v1.2.3 From d18a886418bf191ef427ec9276239ea2779e8439 Mon Sep 17 00:00:00 2001 From: heavydemon21 Date: Tue, 6 Jun 2023 14:35:41 +0200 Subject: final testjes --- nicla/road.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nicla/road.py b/nicla/road.py index adc79a4..02867b8 100644 --- a/nicla/road.py +++ b/nicla/road.py @@ -60,5 +60,5 @@ while(True): print(data) uart.uart_buffer(data) drive(img) - uart.uart_buffer(DUI_CMD_SPEED_END) + #uart.uart_buffer(DUI_CMD_SPEED_END) clock.tick() -- cgit v1.2.3 From da7cc5409372caeac64d46346877b5440e4a4383 Mon Sep 17 00:00:00 2001 From: heavydemon21 Date: Tue, 6 Jun 2023 19:11:42 +0200 Subject: Update traffic_light.py --- nicla/traffic_light.py | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/nicla/traffic_light.py b/nicla/traffic_light.py index 517e2c3..7399523 100644 --- a/nicla/traffic_light.py +++ b/nicla/traffic_light.py @@ -31,9 +31,9 @@ def rgb2hsv(rgb): return (h, s, v) -def traf_lights(img) - original = img.copy() - img = img.to_grayscale() +def traf_lights(imgTraffic): + original = imgTraffic.copy() + img = imgTraffic.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 @@ -53,12 +53,21 @@ def traf_lights(img) 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,)) + #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]) + #print(("", "rood", "geel", "groen")[light_status]) sensor.dealloc_extra_fb() + + if light_status == 1: + return 0x06 + elif light_status == 2: + return 0x07 + elif light_status == 3: + return 0x08 + else: + return 0x01 -- cgit v1.2.3 From 051f0284c9895282205632087a2de6d163073cee Mon Sep 17 00:00:00 2001 From: NielsCoding <48092678+heavydemon21@users.noreply.github.com> Date: Tue, 6 Jun 2023 20:30:44 +0200 Subject: WIP final integration --- nicla/.Trashes/._501 | Bin 0 -> 4096 bytes nicla/.Trashes/501/._image.png | Bin 0 -> 4096 bytes nicla/.Trashes/501/._temp.mjpeg | 0 nicla/.Trashes/501/image.png | Bin 0 -> 20357 bytes nicla/.Trashes/501/temp.mjpeg | Bin 0 -> 4096 bytes nicla/._end.jpg | Bin 0 -> 4096 bytes nicla/._image.jpg | Bin 0 -> 4096 bytes nicla/._no_entry.jpg | Bin 0 -> 4096 bytes nicla/._speed.jpg | Bin 0 -> 4096 bytes nicla/._stop.jpg | Bin 0 -> 4096 bytes nicla/._temp.orb | Bin 0 -> 4096 bytes nicla/.fseventsd/fseventsd-uuid | 1 + nicla/ERROR.LOG | 1 + nicla/README.txt | 17 +++++++++++ nicla/end.jpg | Bin 0 -> 10829 bytes nicla/image.jpg | Bin 0 -> 30111 bytes nicla/kpts.orb | Bin 0 -> 2696 bytes nicla/no_entry.jpg | Bin 0 -> 16864 bytes nicla/road.py | 32 ++++++++++++-------- nicla/signs_detect.py | 64 ++++++++++++++++++++++++++++++++++++++++ nicla/speed.jpg | Bin 0 -> 27055 bytes nicla/speed.orb | Bin 0 -> 4096 bytes nicla/stop.jpg | Bin 0 -> 35442 bytes nicla/temp.orb | Bin 0 -> 6308 bytes 24 files changed, 103 insertions(+), 12 deletions(-) create mode 100644 nicla/.Trashes/._501 create mode 100644 nicla/.Trashes/501/._image.png create mode 100644 nicla/.Trashes/501/._temp.mjpeg create mode 100644 nicla/.Trashes/501/image.png create mode 100644 nicla/.Trashes/501/temp.mjpeg create mode 100644 nicla/._end.jpg create mode 100644 nicla/._image.jpg create mode 100644 nicla/._no_entry.jpg create mode 100644 nicla/._speed.jpg create mode 100644 nicla/._stop.jpg create mode 100644 nicla/._temp.orb create mode 100644 nicla/.fseventsd/fseventsd-uuid create mode 100644 nicla/ERROR.LOG create mode 100644 nicla/README.txt create mode 100644 nicla/end.jpg create mode 100644 nicla/image.jpg create mode 100644 nicla/kpts.orb create mode 100644 nicla/no_entry.jpg create mode 100644 nicla/signs_detect.py create mode 100644 nicla/speed.jpg create mode 100644 nicla/speed.orb create mode 100644 nicla/stop.jpg create mode 100644 nicla/temp.orb diff --git a/nicla/.Trashes/._501 b/nicla/.Trashes/._501 new file mode 100644 index 0000000..338bd7b Binary files /dev/null and b/nicla/.Trashes/._501 differ diff --git a/nicla/.Trashes/501/._image.png b/nicla/.Trashes/501/._image.png new file mode 100644 index 0000000..02cf786 Binary files /dev/null and b/nicla/.Trashes/501/._image.png differ diff --git a/nicla/.Trashes/501/._temp.mjpeg b/nicla/.Trashes/501/._temp.mjpeg new file mode 100644 index 0000000..e69de29 diff --git a/nicla/.Trashes/501/image.png b/nicla/.Trashes/501/image.png new file mode 100644 index 0000000..d8bc7f5 Binary files /dev/null and b/nicla/.Trashes/501/image.png differ diff --git a/nicla/.Trashes/501/temp.mjpeg b/nicla/.Trashes/501/temp.mjpeg new file mode 100644 index 0000000..577a50a Binary files /dev/null and b/nicla/.Trashes/501/temp.mjpeg differ diff --git a/nicla/._end.jpg b/nicla/._end.jpg new file mode 100644 index 0000000..c26df8c Binary files /dev/null and b/nicla/._end.jpg differ diff --git a/nicla/._image.jpg b/nicla/._image.jpg new file mode 100644 index 0000000..4520888 Binary files /dev/null and b/nicla/._image.jpg differ diff --git a/nicla/._no_entry.jpg b/nicla/._no_entry.jpg new file mode 100644 index 0000000..a22af57 Binary files /dev/null and b/nicla/._no_entry.jpg differ diff --git a/nicla/._speed.jpg b/nicla/._speed.jpg new file mode 100644 index 0000000..f3a3faf Binary files /dev/null and b/nicla/._speed.jpg differ diff --git a/nicla/._stop.jpg b/nicla/._stop.jpg new file mode 100644 index 0000000..4a47a5b Binary files /dev/null and b/nicla/._stop.jpg differ diff --git a/nicla/._temp.orb b/nicla/._temp.orb new file mode 100644 index 0000000..acfd770 Binary files /dev/null and b/nicla/._temp.orb differ diff --git a/nicla/.fseventsd/fseventsd-uuid b/nicla/.fseventsd/fseventsd-uuid new file mode 100644 index 0000000..d3c0376 --- /dev/null +++ b/nicla/.fseventsd/fseventsd-uuid @@ -0,0 +1 @@ +6FDC5716-0B86-4159-ACC7-80164FCE6FE6 \ No newline at end of file diff --git a/nicla/ERROR.LOG b/nicla/ERROR.LOG new file mode 100644 index 0000000..ba1a6e3 --- /dev/null +++ b/nicla/ERROR.LOG @@ -0,0 +1 @@ +FATAL ERROR: diff --git a/nicla/README.txt b/nicla/README.txt new file mode 100644 index 0000000..9fdd185 --- /dev/null +++ b/nicla/README.txt @@ -0,0 +1,17 @@ +Thank you for supporting Arduino and the OpenMV project! + +To download the OpenMV IDE, please visit: +https://openmv.io/pages/download + +For tutorials and documentation, please visit: +https://docs.arduino.cc/ +http://docs.openmv.io/ + +For technical OpenMV support and projects, please visit the forums: +http://forums.openmv.io/ + +For Arduino related issues, please visit the Arduino help center: +https://support.arduino.cc/ + +Please use Github to report bugs and issues: +https://github.com/openmv/openmv diff --git a/nicla/end.jpg b/nicla/end.jpg new file mode 100644 index 0000000..7756273 Binary files /dev/null and b/nicla/end.jpg differ diff --git a/nicla/image.jpg b/nicla/image.jpg new file mode 100644 index 0000000..51a3c75 Binary files /dev/null and b/nicla/image.jpg differ diff --git a/nicla/kpts.orb b/nicla/kpts.orb new file mode 100644 index 0000000..0c3e1e7 Binary files /dev/null and b/nicla/kpts.orb differ diff --git a/nicla/no_entry.jpg b/nicla/no_entry.jpg new file mode 100644 index 0000000..6b1c54b Binary files /dev/null and b/nicla/no_entry.jpg differ diff --git a/nicla/road.py b/nicla/road.py index 02867b8..27101ea 100644 --- a/nicla/road.py +++ b/nicla/road.py @@ -1,12 +1,13 @@ import sensor, image, time, math import uart +import signs_detect import traffic_light from consts import * sensor.reset() sensor.set_pixformat(sensor.RGB565) sensor.set_framesize(sensor.HVGA) -sensor.skip_frames(time = 2000) +sensor.skip_frames(time = 4000) clock = time.clock() WIDTH = 480 @@ -33,14 +34,12 @@ def drive(driveImg): 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 @@ -52,13 +51,22 @@ def drive(driveImg): steerByte = int((avg + 1.0) * (DUI_CMD_STEER_END - DUI_CMD_STEER_START) / 2 + DUI_CMD_STEER_START) uart.uart_buffer(steerByte) sensor.dealloc_extra_fb() - +count = 0 while(True): - img = sensor.snapshot() - data = traffic_light.traf_lights(img) - if data is not None: - print(data) - uart.uart_buffer(data) - drive(img) - #uart.uart_buffer(DUI_CMD_SPEED_END) - clock.tick() + if count == 0: + count = count + 1 + speed = signs_detect.init_kpts("speed") + #stop = signs_detect.init_kpts("stop") + #car = signs_detect.init_kpts("image") + else: + img = sensor.snapshot() + data = traffic_light.traf_lights(img) + if data is not None: + uart.uart_buffer(data) + + #data_sign = signs_detect.sign_detection(img) + #if data_sign is not None: + #uart.uart_buffer(data_sign) + + drive(img) + #uart.uart_buffer(DUI_CMD_SPEED_END) diff --git a/nicla/signs_detect.py b/nicla/signs_detect.py new file mode 100644 index 0000000..775ac15 --- /dev/null +++ b/nicla/signs_detect.py @@ -0,0 +1,64 @@ +import sensor, image + +kpts_threshold = 20 +kpts_corner = image.CORNER_FAST + +def init_kpts(str): + temp_img = image.Image(f"./{str}.jpg",copy_to_fb=True) + temp_img.to_grayscale() + kpts = temp_img.find_keypoints(max_keypoints=128, threshold=kpts_threshold, corner_detector=kpts_corner, scale_factor=1.2) + temp_img.dealloc_extra_fb() + return kpts + +def match_kpts(kpts0, kpts1): + if kpts0 is not None and kpts1 is not None: + match = image.match_descriptor(kpts0, kpts1, threshold=70) + #print("matched:%d dt:%d"%(match.count(), match.theta())) + if match.count() > 0: + print(match.count()) + return match.count() > 1 + else: + return 0 + +def read_red_sign(val, img, kpts): + if match_kpts(kpts, stop): + img.draw_rectangle(val.rect()) + #img.draw_cross(match.cx(), match.cy(), size=10) + #print("stop") + return 0x01 + if match_kpts(kpts, speed): + img.draw_rectangle(val.rect()) + #print("speed") + return 0x02 + if match_kpts(kpts, car): + img.draw_rectangle(val.rect()) + #print("car") + return 0x03 + +#def read_red_sign(val, img, kpts): + +def sign_detection(img_sign): + img = img_sign.copy() + ######## Detect signs + blobs_r = img.find_blobs([(0, 100, 25, 63, -128, 127)]) + blobs_b = img.find_blobs([(0, 29, 11, -128, -31, -5)]) + #print(f"old: { len(blobs_r) + len(blobs_b) }") + + blobs_r[:] = [b for b in blobs_r if (b.convexity() < 0.7 and b.area() > 64)] + blobs_b[:] = [b for b in blobs_b if (b.convexity() < 0.7 and b.area() > 64)] + #print(f"new: { len(blobs_r) + len(blobs_b) }") + + + ######## Read signs + img = img.to_grayscale() + sign_buffer = 0x00 + if(len(blobs_r) > 0 or len(blobs_b) > 0): + kpts_img = img.find_keypoints(max_keypoints=255, threshold=kpts_threshold, corner_detector=kpts_corner) + + for index, b in enumerate(blobs_r): + sign_buffer = read_red_sign(b, img, kpts_img) + + for index, b in enumerate(blobs_b): + sign_buffer = read_blu_sign(b, img, kpts_img) + sensor.dealloc_extra_fb() + return sign_buffer diff --git a/nicla/speed.jpg b/nicla/speed.jpg new file mode 100644 index 0000000..c6cf7a9 Binary files /dev/null and b/nicla/speed.jpg differ diff --git a/nicla/speed.orb b/nicla/speed.orb new file mode 100644 index 0000000..5c48d10 Binary files /dev/null and b/nicla/speed.orb differ diff --git a/nicla/stop.jpg b/nicla/stop.jpg new file mode 100644 index 0000000..cd35e95 Binary files /dev/null and b/nicla/stop.jpg differ diff --git a/nicla/temp.orb b/nicla/temp.orb new file mode 100644 index 0000000..ec41eff Binary files /dev/null and b/nicla/temp.orb differ -- cgit v1.2.3 From c537c7ad294107b1804975c4e7ed0dc9886990cf Mon Sep 17 00:00:00 2001 From: lonkaars Date: Wed, 7 Jun 2023 10:22:02 +0200 Subject: better road detection --- nicla/downscale.py | 19 +++++++++++++++++++ nicla/road.py | 4 +++- 2 files changed, 22 insertions(+), 1 deletion(-) create mode 100644 nicla/downscale.py diff --git a/nicla/downscale.py b/nicla/downscale.py new file mode 100644 index 0000000..738b4bb --- /dev/null +++ b/nicla/downscale.py @@ -0,0 +1,19 @@ +import sensor, image, time, math + +sensor.reset() +sensor.set_pixformat(sensor.RGB565) +sensor.set_framesize(sensor.HVGA) +sensor.skip_frames(time = 2000) +sensor.set_vflip(True) +sensor.set_hmirror(True) +clock = time.clock() + +def main(): + img = sensor.snapshot() + img.to_grayscale() + img.scale(copy_to_fb=True, x_size=100) + +if __name__ == "__main__": + while(True): + main() + clock.tick() diff --git a/nicla/road.py b/nicla/road.py index ceabc3b..20863cc 100644 --- a/nicla/road.py +++ b/nicla/road.py @@ -10,6 +10,8 @@ clock = time.clock() WIDTH = 480 HEIGHT = 320 +MAX_AREA = WIDTH * HEIGHT / 10 +MIN_AREA = 40 HORIZON = 150 STRETCH = 40 @@ -35,7 +37,7 @@ def main(): for blob in img.find_blobs([(ROAD_MIN_BRIGHTNESS, 0xff)], pixels_threshold=100): img.draw_rectangle(blob.rect()) - area_weight = blob.area() + area_weight = MIN_AREA + min(MAX_AREA, blob.w() * blob.h()) # limit max area_weight so small blobs still have impact horizontal_pos = (blob.x() + blob.w()/2) / WIDTH offset_sum += horizontal_pos * area_weight offset_count += area_weight -- cgit v1.2.3 From b62bcc004fc0dd3d53fbbaf02d3af516b7cb47e0 Mon Sep 17 00:00:00 2001 From: heavydemon21 Date: Wed, 7 Jun 2023 11:21:19 +0200 Subject: integration --- nicla/road.py | 26 +++++++++++++------------- nicla/signs_detect.py | 9 +++------ nicla/traffic_light.py | 2 -- 3 files changed, 16 insertions(+), 21 deletions(-) diff --git a/nicla/road.py b/nicla/road.py index 27101ea..5e5e1de 100644 --- a/nicla/road.py +++ b/nicla/road.py @@ -26,7 +26,6 @@ points = [(STRETCH, HORIZON), (-SQUEEZE, HEIGHT-1)] def drive(driveImg): - img = driveImg.copy() img.to_grayscale() img.replace(vflip=True, hmirror=True) img.rotation_corr(corners=points) @@ -50,23 +49,24 @@ def drive(driveImg): print(avg) steerByte = int((avg + 1.0) * (DUI_CMD_STEER_END - DUI_CMD_STEER_START) / 2 + DUI_CMD_STEER_START) uart.uart_buffer(steerByte) - sensor.dealloc_extra_fb() -count = 0 + + +speed = signs_detect.init_kpts("speed") +stop = signs_detect.init_kpts("stop") +car = signs_detect.init_kpts("image") while(True): - if count == 0: - count = count + 1 - speed = signs_detect.init_kpts("speed") - #stop = signs_detect.init_kpts("stop") - #car = signs_detect.init_kpts("image") - else: + img = sensor.snapshot() data = traffic_light.traf_lights(img) if data is not None: uart.uart_buffer(data) - #data_sign = signs_detect.sign_detection(img) - #if data_sign is not None: - #uart.uart_buffer(data_sign) - drive(img) + sign_img = sensor.snapshot() + data_sign = signs_detect.sign_detection(sign_img) + if data_sign is not None: + uart.uart_buffer(data_sign) + + drive_img = sensor.snapshot() + drive(drive_img) #uart.uart_buffer(DUI_CMD_SPEED_END) diff --git a/nicla/signs_detect.py b/nicla/signs_detect.py index 775ac15..786972d 100644 --- a/nicla/signs_detect.py +++ b/nicla/signs_detect.py @@ -7,7 +7,6 @@ def init_kpts(str): temp_img = image.Image(f"./{str}.jpg",copy_to_fb=True) temp_img.to_grayscale() kpts = temp_img.find_keypoints(max_keypoints=128, threshold=kpts_threshold, corner_detector=kpts_corner, scale_factor=1.2) - temp_img.dealloc_extra_fb() return kpts def match_kpts(kpts0, kpts1): @@ -37,8 +36,7 @@ def read_red_sign(val, img, kpts): #def read_red_sign(val, img, kpts): -def sign_detection(img_sign): - img = img_sign.copy() +def sign_detection(img): ######## Detect signs blobs_r = img.find_blobs([(0, 100, 25, 63, -128, 127)]) blobs_b = img.find_blobs([(0, 29, 11, -128, -31, -5)]) @@ -58,7 +56,6 @@ def sign_detection(img_sign): for index, b in enumerate(blobs_r): sign_buffer = read_red_sign(b, img, kpts_img) - for index, b in enumerate(blobs_b): - sign_buffer = read_blu_sign(b, img, kpts_img) - sensor.dealloc_extra_fb() + #for index, b in enumerate(blobs_b): + #sign_buffer = read_blu_sign(b, img, kpts_img) return sign_buffer diff --git a/nicla/traffic_light.py b/nicla/traffic_light.py index 7399523..9499aea 100644 --- a/nicla/traffic_light.py +++ b/nicla/traffic_light.py @@ -32,7 +32,6 @@ def rgb2hsv(rgb): def traf_lights(imgTraffic): - original = imgTraffic.copy() img = imgTraffic.to_grayscale() for blob in img.find_blobs([(0, 60)], pixels_threshold=100): aspect = blob.h() / blob.w() @@ -61,7 +60,6 @@ def traf_lights(imgTraffic): 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]) - sensor.dealloc_extra_fb() if light_status == 1: return 0x06 -- cgit v1.2.3 From e943ed3992044c601c3eb036a07ff9f68db52c8b Mon Sep 17 00:00:00 2001 From: heavydemon21 Date: Wed, 7 Jun 2023 13:54:17 +0200 Subject: zumo update --- nicla/road.py | 67 +++++++++++++++++++++++++-------------------------- nicla/signs_detect.py | 2 +- zumo/control.cpp | 10 +++----- zumo/pid.cpp | 3 +-- zumo/protocol.cpp | 16 +++++++++--- zumo/protocol.h | 3 +-- zumo/zumo.ino | 12 ++++++--- 7 files changed, 62 insertions(+), 51 deletions(-) diff --git a/nicla/road.py b/nicla/road.py index 1b06e78..143a9c5 100644 --- a/nicla/road.py +++ b/nicla/road.py @@ -27,41 +27,40 @@ points = [(STRETCH, HORIZON), (WIDTH-1+SQUEEZE, HEIGHT-1), (-SQUEEZE, HEIGHT-1)] -def drive(driveImg): - 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 = MIN_AREA + min(MAX_AREA, blob.w() * blob.h()) # limit max area_weight so small blobs still have impact - 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)) - - print(avg) - steerByte = int((avg + 1.0) * (DUI_CMD_STEER_END - DUI_CMD_STEER_START) / 2 + DUI_CMD_STEER_START) - uart.uart_buffer(steerByte) - - -speed = signs_detect.init_kpts("speed") -stop = signs_detect.init_kpts("stop") -car = signs_detect.init_kpts("image") + +def drive(img): + 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 = MIN_AREA + min(MAX_AREA, blob.w() * blob.h()) # limit max area_weight so small blobs still have impact + 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): - img = sensor.snapshot() - data = traffic_light.traf_lights(img) - if data is not None: - uart.uart_buffer(data) + #img = sensor.snapshot() + #data = traffic_light.traf_lights(img) + #if data is not None: + #uart.uart_buffer(data) sign_img = sensor.snapshot() @@ -71,4 +70,4 @@ while(True): drive_img = sensor.snapshot() drive(drive_img) - #uart.uart_buffer(DUI_CMD_SPEED_END) + uart.uart_buffer(0x1f) diff --git a/nicla/signs_detect.py b/nicla/signs_detect.py index 786972d..57b6c3e 100644 --- a/nicla/signs_detect.py +++ b/nicla/signs_detect.py @@ -15,7 +15,7 @@ def match_kpts(kpts0, kpts1): #print("matched:%d dt:%d"%(match.count(), match.theta())) if match.count() > 0: print(match.count()) - return match.count() > 1 + return match.count() > 0 else: return 0 diff --git a/zumo/control.cpp b/zumo/control.cpp index 778969a..71e7537 100644 --- a/zumo/control.cpp +++ b/zumo/control.cpp @@ -6,11 +6,11 @@ #include "control.h" #define DUI_SPEED_MOD 96.0f -#define DUI_MOTOR_DIFF 0.6f +#define DUI_MOTOR_DIFF 0.62f 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; + 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(" "); @@ -18,8 +18,7 @@ void apply_state(dui_state_t *state) { Serial.println(""); - Zumo32U4Motors::setLeftSpeed((int16_t) motor_l); - Zumo32U4Motors::setRightSpeed((int16_t) motor_r); + Zumo32U4Motors::setSpeeds((int16_t) motor_l,(int16_t) motor_r); // TODO: print sign on OLED screen } @@ -38,6 +37,5 @@ unsigned char uart_read() { } delayMicroseconds(1000); // wait out stop bit - return byte; } diff --git a/zumo/pid.cpp b/zumo/pid.cpp index 7c1cbda..17588e3 100644 --- a/zumo/pid.cpp +++ b/zumo/pid.cpp @@ -45,8 +45,7 @@ PID speed_pid = PID(); PID steer_pid = PID(); PID speed_mod_pid = PID(); void apply_pid(dui_state_t* target, dui_state_t* current) { - current->speed = speed_pid.iter(target->speed); + current->Speed = speed_pid.iter(target->Speed); current->steer = steer_pid.iter(target->steer); current->speed_mod = speed_mod_pid.iter(target->speed_mod); } - diff --git a/zumo/protocol.cpp b/zumo/protocol.cpp index ab8397a..d176c24 100644 --- a/zumo/protocol.cpp +++ b/zumo/protocol.cpp @@ -1,5 +1,8 @@ -#include "protocol.h" +#include +#include "protocol.h" +#include +#include #define DUI_CMD_NULL 0x00 #define DUI_CMD_SIGN_START 0x01 #define DUI_CMD_SIGN_END 0x0f @@ -7,15 +10,22 @@ #define DUI_CMD_SPEED_END 0x1f #define DUI_CMD_STEER_START 0x20 #define DUI_CMD_STEER_END 0xff +Zumo32U4OLED display; void handle_cmd(unsigned char cmd, dui_state_t *state) { + Serial.println(cmd,HEX); + 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); + display.clear(); + display.print(state->current_sign+1); + display.gotoXY(0, 1); } else if (DUI_CMD_SPEED_START <= cmd && cmd <= DUI_CMD_SPEED_END) { - state->speed = (float) (cmd - DUI_CMD_SPEED_START) / (float) (DUI_CMD_SPEED_END - DUI_CMD_SPEED_START); + Serial.print(" Hallo: " ); + state->Speed = (float) (cmd - DUI_CMD_SPEED_START) / (float) (DUI_CMD_SPEED_END - DUI_CMD_SPEED_START); + Serial.println(state->Speed); } 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; } } - diff --git a/zumo/protocol.h b/zumo/protocol.h index a1f9951..5eb2343 100644 --- a/zumo/protocol.h +++ b/zumo/protocol.h @@ -21,11 +21,10 @@ typedef enum { typedef struct { float steer; /** @brief steer value (-1 is left, +1 is right) */ - float speed; /** @brief speed (0-15) */ + float Speed; /** @brief speed (0-15) */ dui_e_sign current_sign; /** @brief last seen sign */ float speed_mod; /** @brief global speed multiplier */ } dui_state_t; /** @brief read and apply cmd to state */ void handle_cmd(unsigned char cmd, dui_state_t *state); - diff --git a/zumo/zumo.ino b/zumo/zumo.ino index 1a64381..a4f4d6b 100644 --- a/zumo/zumo.ino +++ b/zumo/zumo.ino @@ -2,19 +2,20 @@ #include #include + #include "control.h" #include "protocol.h" #include "pid.h" dui_state_t g_dui_target_state = { .steer = 0.0f, - .speed = 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 = 0.f, + .Speed = 0.f, .current_sign = DUI_SIGN_NONE, .speed_mod = 1.f, }; @@ -22,10 +23,13 @@ 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); + Serial.begin(11500); + } void loop() { + + static unsigned char cmd_old = 0x00; for (unsigned int i = 0; i < 1000; i++) { digitalWrite(DUI_PINOUT_NICLA_TX, LOW); @@ -40,4 +44,6 @@ void loop() { g_dui_current_state.current_sign = g_dui_target_state.current_sign; apply_state(&g_dui_current_state); + + } -- cgit v1.2.3 From 74527f9b5a51eca49734a50a5b78e26e2d824776 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Wed, 7 Jun 2023 14:00:14 +0200 Subject: remove bloat --- .gitignore | 4 ++++ nicla/.Trashes/._501 | Bin 4096 -> 0 bytes nicla/.Trashes/501/._image.png | Bin 4096 -> 0 bytes nicla/.Trashes/501/._temp.mjpeg | 0 nicla/.Trashes/501/image.png | Bin 20357 -> 0 bytes nicla/.Trashes/501/temp.mjpeg | Bin 4096 -> 0 bytes nicla/._end.jpg | Bin 4096 -> 0 bytes nicla/._image.jpg | Bin 4096 -> 0 bytes nicla/._no_entry.jpg | Bin 4096 -> 0 bytes nicla/._speed.jpg | Bin 4096 -> 0 bytes nicla/._stop.jpg | Bin 4096 -> 0 bytes nicla/._temp.orb | Bin 4096 -> 0 bytes nicla/.fseventsd/fseventsd-uuid | 1 - nicla/ERROR.LOG | 1 - nicla/README.txt | 17 ----------------- nicla/kpts.orb | Bin 2696 -> 0 bytes nicla/speed.orb | Bin 4096 -> 0 bytes nicla/temp.orb | Bin 6308 -> 0 bytes 18 files changed, 4 insertions(+), 19 deletions(-) delete mode 100644 nicla/.Trashes/._501 delete mode 100644 nicla/.Trashes/501/._image.png delete mode 100644 nicla/.Trashes/501/._temp.mjpeg delete mode 100644 nicla/.Trashes/501/image.png delete mode 100644 nicla/.Trashes/501/temp.mjpeg delete mode 100644 nicla/._end.jpg delete mode 100644 nicla/._image.jpg delete mode 100644 nicla/._no_entry.jpg delete mode 100644 nicla/._speed.jpg delete mode 100644 nicla/._stop.jpg delete mode 100644 nicla/._temp.orb delete mode 100644 nicla/.fseventsd/fseventsd-uuid delete mode 100644 nicla/ERROR.LOG delete mode 100644 nicla/README.txt delete mode 100644 nicla/kpts.orb delete mode 100644 nicla/speed.orb delete mode 100644 nicla/temp.orb diff --git a/.gitignore b/.gitignore index 3f4b7ab..e6072de 100644 --- a/.gitignore +++ b/.gitignore @@ -2,3 +2,7 @@ assets/LSD_straightLines_Pic_0.png assets/LSD_straightLines_Pic_1.png assets/hough_straightLines_Pic_0.png assets/hough_straightLines_Pic_1.png + +nicla/.* +nicla/ERROR.LOG +nicla/README.txt diff --git a/nicla/.Trashes/._501 b/nicla/.Trashes/._501 deleted file mode 100644 index 338bd7b..0000000 Binary files a/nicla/.Trashes/._501 and /dev/null differ diff --git a/nicla/.Trashes/501/._image.png b/nicla/.Trashes/501/._image.png deleted file mode 100644 index 02cf786..0000000 Binary files a/nicla/.Trashes/501/._image.png and /dev/null differ diff --git a/nicla/.Trashes/501/._temp.mjpeg b/nicla/.Trashes/501/._temp.mjpeg deleted file mode 100644 index e69de29..0000000 diff --git a/nicla/.Trashes/501/image.png b/nicla/.Trashes/501/image.png deleted file mode 100644 index d8bc7f5..0000000 Binary files a/nicla/.Trashes/501/image.png and /dev/null differ diff --git a/nicla/.Trashes/501/temp.mjpeg b/nicla/.Trashes/501/temp.mjpeg deleted file mode 100644 index 577a50a..0000000 Binary files a/nicla/.Trashes/501/temp.mjpeg and /dev/null differ diff --git a/nicla/._end.jpg b/nicla/._end.jpg deleted file mode 100644 index c26df8c..0000000 Binary files a/nicla/._end.jpg and /dev/null differ diff --git a/nicla/._image.jpg b/nicla/._image.jpg deleted file mode 100644 index 4520888..0000000 Binary files a/nicla/._image.jpg and /dev/null differ diff --git a/nicla/._no_entry.jpg b/nicla/._no_entry.jpg deleted file mode 100644 index a22af57..0000000 Binary files a/nicla/._no_entry.jpg and /dev/null differ diff --git a/nicla/._speed.jpg b/nicla/._speed.jpg deleted file mode 100644 index f3a3faf..0000000 Binary files a/nicla/._speed.jpg and /dev/null differ diff --git a/nicla/._stop.jpg b/nicla/._stop.jpg deleted file mode 100644 index 4a47a5b..0000000 Binary files a/nicla/._stop.jpg and /dev/null differ diff --git a/nicla/._temp.orb b/nicla/._temp.orb deleted file mode 100644 index acfd770..0000000 Binary files a/nicla/._temp.orb and /dev/null differ diff --git a/nicla/.fseventsd/fseventsd-uuid b/nicla/.fseventsd/fseventsd-uuid deleted file mode 100644 index d3c0376..0000000 --- a/nicla/.fseventsd/fseventsd-uuid +++ /dev/null @@ -1 +0,0 @@ -6FDC5716-0B86-4159-ACC7-80164FCE6FE6 \ No newline at end of file diff --git a/nicla/ERROR.LOG b/nicla/ERROR.LOG deleted file mode 100644 index ba1a6e3..0000000 --- a/nicla/ERROR.LOG +++ /dev/null @@ -1 +0,0 @@ -FATAL ERROR: diff --git a/nicla/README.txt b/nicla/README.txt deleted file mode 100644 index 9fdd185..0000000 --- a/nicla/README.txt +++ /dev/null @@ -1,17 +0,0 @@ -Thank you for supporting Arduino and the OpenMV project! - -To download the OpenMV IDE, please visit: -https://openmv.io/pages/download - -For tutorials and documentation, please visit: -https://docs.arduino.cc/ -http://docs.openmv.io/ - -For technical OpenMV support and projects, please visit the forums: -http://forums.openmv.io/ - -For Arduino related issues, please visit the Arduino help center: -https://support.arduino.cc/ - -Please use Github to report bugs and issues: -https://github.com/openmv/openmv diff --git a/nicla/kpts.orb b/nicla/kpts.orb deleted file mode 100644 index 0c3e1e7..0000000 Binary files a/nicla/kpts.orb and /dev/null differ diff --git a/nicla/speed.orb b/nicla/speed.orb deleted file mode 100644 index 5c48d10..0000000 Binary files a/nicla/speed.orb and /dev/null differ diff --git a/nicla/temp.orb b/nicla/temp.orb deleted file mode 100644 index ec41eff..0000000 Binary files a/nicla/temp.orb and /dev/null differ -- cgit v1.2.3 From 2903e61cbe2eeff3121f67da516ea195999f0bba Mon Sep 17 00:00:00 2001 From: lonkaars Date: Wed, 7 Jun 2023 14:13:04 +0200 Subject: string lookup for display --- zumo/protocol.cpp | 12 ++++++------ zumo/protocol.h | 12 ++++++++++++ zumo/strlutest/.gitignore | 1 + zumo/strlutest/makefile | 21 +++++++++++++++++++++ zumo/strlutest/strlutest.cpp | 10 ++++++++++ zumo/zumo.ino | 6 ------ 6 files changed, 50 insertions(+), 12 deletions(-) create mode 100644 zumo/strlutest/.gitignore create mode 100644 zumo/strlutest/makefile create mode 100644 zumo/strlutest/strlutest.cpp diff --git a/zumo/protocol.cpp b/zumo/protocol.cpp index d176c24..06e2993 100644 --- a/zumo/protocol.cpp +++ b/zumo/protocol.cpp @@ -13,18 +13,18 @@ Zumo32U4OLED display; void handle_cmd(unsigned char cmd, dui_state_t *state) { - Serial.println(cmd,HEX); + Serial.println(cmd, HEX); 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); - display.clear(); - display.print(state->current_sign+1); - display.gotoXY(0, 1); + display.clear(); + if (DUI_SIGN_LOOKUP[state->current_sign] == nullptr) return; + display.print(DUI_SIGN_LOOKUP[state->current_sign]); } else if (DUI_CMD_SPEED_START <= cmd && cmd <= DUI_CMD_SPEED_END) { - Serial.print(" Hallo: " ); + Serial.print(" Hallo: " ); state->Speed = (float) (cmd - DUI_CMD_SPEED_START) / (float) (DUI_CMD_SPEED_END - DUI_CMD_SPEED_START); - Serial.println(state->Speed); + Serial.println(state->Speed); } 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; } diff --git a/zumo/protocol.h b/zumo/protocol.h index 5eb2343..ea3e27d 100644 --- a/zumo/protocol.h +++ b/zumo/protocol.h @@ -19,6 +19,18 @@ typedef enum { DUI_SIGN_LIGHT_GO, /** @brief traffic light green (keep current speed) */ } dui_e_sign; +const char* const DUI_SIGN_LOOKUP[16] = { + [DUI_SIGN_NONE] = "", + [DUI_SIGN_STOP] = "bord\nstop", + [DUI_SIGN_LEFT] = "bord\nlinks", + [DUI_SIGN_RIGHT] = "bord\nrechts", + [DUI_SIGN_SPEED_LIMIT_LOW] = "snelheid\nlaag", + [DUI_SIGN_SPEED_LIMIT_HIGH] = "snelheid\nhoog", + [DUI_SIGN_LIGHT_STOP] = "stop.l.\nrood", + [DUI_SIGN_LIGHT_FLOOR_IT] = "stop.l.\ngeel", + [DUI_SIGN_LIGHT_GO] = "stop.l.\ngroen", +}; + typedef struct { float steer; /** @brief steer value (-1 is left, +1 is right) */ float Speed; /** @brief speed (0-15) */ diff --git a/zumo/strlutest/.gitignore b/zumo/strlutest/.gitignore new file mode 100644 index 0000000..15010c5 --- /dev/null +++ b/zumo/strlutest/.gitignore @@ -0,0 +1 @@ +strlutest diff --git a/zumo/strlutest/makefile b/zumo/strlutest/makefile new file mode 100644 index 0000000..f05e0ba --- /dev/null +++ b/zumo/strlutest/makefile @@ -0,0 +1,21 @@ +CPP = g++ +LD = g++ +RM = rm -f +CFLAGS = +LFLAGS = +TARGET = strlutest + +SRCS := strlutest.cpp +OBJS := strlutest.o + +all: strlutest + +%.o: %.cpp + $(CPP) -c $(CFLAGS) $< -o $@ + +$(TARGET): $(OBJS) + $(LD) $^ $(LFLAGS) -o $@ + +clean: + $(RM) $(TARGET) $(OBJS) + diff --git a/zumo/strlutest/strlutest.cpp b/zumo/strlutest/strlutest.cpp new file mode 100644 index 0000000..72ec591 --- /dev/null +++ b/zumo/strlutest/strlutest.cpp @@ -0,0 +1,10 @@ +#include + +#include "../protocol.h" + +int main() { + for (unsigned int i = 0; i < 15; i++) { + if (DUI_SIGN_LOOKUP[i] == NULL) continue; + printf("%d: %s\n", i, DUI_SIGN_LOOKUP[i]); + } +} diff --git a/zumo/zumo.ino b/zumo/zumo.ino index a4f4d6b..9e79581 100644 --- a/zumo/zumo.ino +++ b/zumo/zumo.ino @@ -2,7 +2,6 @@ #include #include - #include "control.h" #include "protocol.h" #include "pid.h" @@ -24,12 +23,9 @@ void setup() { pinMode(DUI_PINOUT_NICLA_TX, OUTPUT); pinMode(DUI_PINOUT_NICLA_RX, INPUT_PULLUP); Serial.begin(11500); - } void loop() { - - static unsigned char cmd_old = 0x00; for (unsigned int i = 0; i < 1000; i++) { digitalWrite(DUI_PINOUT_NICLA_TX, LOW); @@ -44,6 +40,4 @@ void loop() { g_dui_current_state.current_sign = g_dui_target_state.current_sign; apply_state(&g_dui_current_state); - - } -- cgit v1.2.3