almost done, circular buffer added in road.

3 files changed, 61 insertions, 25 deletions

diff --git a/nicla/road.py b/nicla/road.py
index 143a9c5..7edd871 100644
--- a/nicla/road.py
+++ b/nicla/road.py
@@ -27,6 +27,28 @@ points = [(STRETCH, HORIZON),
           (WIDTH-1+SQUEEZE, HEIGHT-1),
           (-SQUEEZE, HEIGHT-1)]
 
+class CircularBuffer:
+    def __init__(self, size):
+        self.buffer = [None] * size
+        self.size = size
+        self.index = 0
+        self.counter = 0
+        self.output_value = None
+
+    def add(self, value):
+        self.buffer[self.index] = value
+        self.index = (self.index + 1) % self.size
+
+        if self.counter > 0 and self.buffer[self.index] == self.output_value:
+            self.counter += 1
+        else:
+            self.output_value = value
+            self.counter = 1
+
+        if self.counter == self.size * 2 // 3:
+            return self.output_value
+
+        return None
 
 def drive(img):
     img.to_grayscale()
@@ -54,20 +76,21 @@ def drive(img):
     uart.uart_buffer(steerByte)
 
 
+traffic_buffer = CircularBuffer(2)
+sign_buffer = CircularBuffer(3)
 
 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_buffer.add(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)
+      data = sign_buffer.add(signs_detect.sign_detection(sign_img))
+      if data is not None:
+        uart.uart_buffer(data)
 
-      drive_img = sensor.snapshot()
-      drive(drive_img)
-      uart.uart_buffer(0x1f)
+      #drive_img = sensor.snapshot()
+      #drive(drive_img)
+      #uart.uart_buffer(0x1f)
diff --git a/nicla/signs_detect.py b/nicla/signs_detect.py
index 57b6c3e..17d47c3 100644
--- a/nicla/signs_detect.py
+++ b/nicla/signs_detect.py
@@ -9,32 +9,40 @@ def init_kpts(str):
     kpts = temp_img.find_keypoints(max_keypoints=128, threshold=kpts_threshold, corner_detector=kpts_corner, scale_factor=1.2)
     return kpts
 
+speed = init_kpts("speed")
+stop = init_kpts("stop")
+car = init_kpts("image")
+
 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() > 0
+            #print(match.count())
+            return match.count() > 0
     else:
         return 0
 
 def read_red_sign(val, img, kpts):
+    data = 0x00
     if match_kpts(kpts, stop):
-        img.draw_rectangle(val.rect())
+        #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())
+        data = 0x01
+    elif match_kpts(kpts, speed):
+        #img.draw_rectangle(val.rect())
         #print("speed")
-        return 0x02
-    if match_kpts(kpts, car):
-        img.draw_rectangle(val.rect())
+        data = 0x02
+    elif match_kpts(kpts, car):
+        #img.draw_rectangle(val.rect())
         #print("car")
-        return 0x03
+        data = 0x03
+
+    return data
 
-#def read_red_sign(val, img, kpts):
+def read_blu_sign(val, img, kpts):
+    return 0x02
 
 def sign_detection(img):
     ######## Detect signs
@@ -55,7 +63,11 @@ def sign_detection(img):
 
         for index, b in enumerate(blobs_r):
             sign_buffer = read_red_sign(b, img, kpts_img)
+            if sign_buffer != 0x00:
+                break
 
         #for index, b in enumerate(blobs_b):
             #sign_buffer = read_blu_sign(b, img, kpts_img)
+            #if sign_buffer != 0x00:
+                #break
     return sign_buffer
diff --git a/nicla/traffic_light.py b/nicla/traffic_light.py
index 9499aea..f445690 100644
--- a/nicla/traffic_light.py
+++ b/nicla/traffic_light.py
@@ -32,6 +32,7 @@ def rgb2hsv(rgb):
 
 
 def traf_lights(imgTraffic):
+    original = imgTraffic.copy(copy_to_fb=True)
     img = imgTraffic.to_grayscale()
     for blob in img.find_blobs([(0, 60)], pixels_threshold=100):
         aspect = blob.h() / blob.w()
@@ -62,10 +63,10 @@ def traf_lights(imgTraffic):
         #print(("", "rood", "geel", "groen")[light_status])
 
         if light_status == 1:
-            return 0x06
-        elif light_status == 2:
             return 0x07
-        elif light_status == 3:
+        elif light_status == 2:
             return 0x08
+        elif light_status == 3:
+            return 0x09
         else:
             return 0x01