diff options
| -rw-r--r-- | .gitignore | 4 | ||||
| -rw-r--r-- | nicla/consts.py | 6 | ||||
| -rw-r--r-- | nicla/downscale.py | 19 | ||||
| -rw-r--r-- | nicla/end.jpg | bin | 0 -> 10829 bytes | |||
| -rw-r--r-- | nicla/image.jpg | bin | 0 -> 30111 bytes | |||
| -rw-r--r-- | nicla/no_entry.jpg | bin | 0 -> 16864 bytes | |||
| -rw-r--r-- | nicla/road.py | 73 | ||||
| -rw-r--r-- | nicla/speed.jpg | bin | 0 -> 27055 bytes | |||
| -rw-r--r-- | nicla/stop.jpg | bin | 0 -> 35442 bytes | |||
| -rw-r--r-- | nicla/traffic_light.py | 22 | ||||
| -rw-r--r-- | nicla/uart.py (renamed from nicla/serial_test.py) | 5 | ||||
| -rw-r--r-- | zumo/control.cpp | 10 | ||||
| -rw-r--r-- | zumo/pid.cpp | 3 | ||||
| -rw-r--r-- | zumo/protocol.cpp | 16 | ||||
| -rw-r--r-- | zumo/protocol.h | 15 | ||||
| -rw-r--r-- | zumo/strlutest/.gitignore | 1 | ||||
| -rw-r--r-- | zumo/strlutest/makefile | 21 | ||||
| -rw-r--r-- | zumo/strlutest/strlutest.cpp | 10 | ||||
| -rw-r--r-- | zumo/zumo.ino | 6 |
19 files changed, 185 insertions, 26 deletions
@@ -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/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/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/end.jpg b/nicla/end.jpg Binary files differnew file mode 100644 index 0000000..7756273 --- /dev/null +++ b/nicla/end.jpg diff --git a/nicla/image.jpg b/nicla/image.jpg Binary files differnew file mode 100644 index 0000000..51a3c75 --- /dev/null +++ b/nicla/image.jpg diff --git a/nicla/no_entry.jpg b/nicla/no_entry.jpg Binary files differnew file mode 100644 index 0000000..6b1c54b --- /dev/null +++ b/nicla/no_entry.jpg diff --git a/nicla/road.py b/nicla/road.py new file mode 100644 index 0000000..143a9c5 --- /dev/null +++ b/nicla/road.py @@ -0,0 +1,73 @@ +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 = 4000) +clock = time.clock() + +WIDTH = 480 +HEIGHT = 320 +MAX_AREA = WIDTH * HEIGHT / 10 +MIN_AREA = 40 + +HORIZON = 150 +STRETCH = 40 +SQUEEZE = 400 + +STEERING_ENTHOUSIASM = 3.0 +ROAD_MIN_BRIGHTNESS = 0xa0 + +points = [(STRETCH, HORIZON), + (WIDTH-1-STRETCH, HORIZON), + (WIDTH-1+SQUEEZE, HEIGHT-1), + (-SQUEEZE, HEIGHT-1)] + + +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) + + + 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(0x1f) diff --git a/nicla/speed.jpg b/nicla/speed.jpg Binary files differnew file mode 100644 index 0000000..c6cf7a9 --- /dev/null +++ b/nicla/speed.jpg diff --git a/nicla/stop.jpg b/nicla/stop.jpg Binary files differnew file mode 100644 index 0000000..cd35e95 --- /dev/null +++ b/nicla/stop.jpg diff --git a/nicla/traffic_light.py b/nicla/traffic_light.py index 3d81139..9499aea 100644 --- a/nicla/traffic_light.py +++ b/nicla/traffic_light.py @@ -30,12 +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) - img = img.to_grayscale() + +def traf_lights(imgTraffic): + 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 @@ -55,11 +52,20 @@ while(True): 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]) + + if light_status == 1: + return 0x06 + elif light_status == 2: + return 0x07 + elif light_status == 3: + return 0x08 + else: + return 0x01 diff --git a/nicla/serial_test.py b/nicla/uart.py index 276f6d1..da150b0 100644 --- a/nicla/serial_test.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): 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..06e2993 100644 --- a/zumo/protocol.cpp +++ b/zumo/protocol.cpp @@ -1,5 +1,8 @@ -#include "protocol.h" +#include <Zumo32U4.h> +#include "protocol.h" +#include <Arduino.h> +#include <Zumo32U4LCD.h> #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(); + 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) { - 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..ea3e27d 100644 --- a/zumo/protocol.h +++ b/zumo/protocol.h @@ -19,13 +19,24 @@ 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) */ + 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/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 <cstdio> + +#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 1a64381..9e79581 100644 --- a/zumo/zumo.ino +++ b/zumo/zumo.ino @@ -8,13 +8,13 @@ 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,7 +22,7 @@ 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() { |
