merge dev into master

19 files changed, 185 insertions, 26 deletions

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/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
new file mode 100644
index 0000000..7756273
--- /dev/null
+++ b/nicla/end.jpg
diff --git a/nicla/image.jpg b/nicla/image.jpg
new file mode 100644
index 0000000..51a3c75
--- /dev/null
+++ b/nicla/image.jpg
diff --git a/nicla/no_entry.jpg b/nicla/no_entry.jpg
new 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
new file mode 100644
index 0000000..c6cf7a9
--- /dev/null
+++ b/nicla/speed.jpg
diff --git a/nicla/stop.jpg b/nicla/stop.jpg
new 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() {