From 94885dee1f2601bfa432f340ee7951eb7c14ace3 Mon Sep 17 00:00:00 2001
From: d3rped <derped@c3d2.de>
Date: Thu, 22 Mar 2018 07:09:30 +0100
Subject: [PATCH] Switched to RGB-RAW with greyscale calculation. Sensor mode
 switching was too slow.

---
 src/move.py   | 12 +++++-------
 src/sensor.py | 25 +++++++++++++++++++------
 2 files changed, 24 insertions(+), 13 deletions(-)

diff --git a/src/move.py b/src/move.py
index c6b6ed6..570a13e 100644
--- a/src/move.py
+++ b/src/move.py
@@ -12,7 +12,7 @@ class Move:
             self._wheel_r = Wheel('outC')
             self._camera = Sensor()
             self._bumper = ev3.TouchSensor()
-        except:
+        except OSError:
             print("ERROR: Cannot find Motor/Sensor")
 
     '''
@@ -30,7 +30,6 @@ class Move:
     def _aligntoedge(self):
         pass
 
-
     '''
     uses odometry and the planet object to map edges that are connected to
     current node.
@@ -47,11 +46,10 @@ class Move:
         self._wheel_r.speed_set(24)
         self._wheel_l.run()
         self._wheel_r.run()
-        while (self._bumper.value() == False and self._camera.lastcolor == None ):
-            self._camera.setmode('COL-REFLECT')
-            self._wheel_l.speed_set(48 - self._camera.getbrightness())
-            self._wheel_r.speed_set(self._camera.getbrightness())
-            self._camera.setmode('RGB-RAW')
+        while (self._bumper.value() == False and self._camera.lastcolor == None):
+            self._camera.refresh()
+            self._wheel_l.speed_set((60 - (self._camera.getbrightness()/5))/1.5)
+            self._wheel_r.speed_set((self._camera.getbrightness()/5)/1.5)
             self._camera.getcolor()
             if(isknownstation == False):
                 pass  # run odometry stuff here
diff --git a/src/sensor.py b/src/sensor.py
index 0844e62..2d0573f 100644
--- a/src/sensor.py
+++ b/src/sensor.py
@@ -14,6 +14,14 @@ color = (100, 30, 20)
 (color[0] > 90 and (color[1], color[2]) < (40, 25))
 '''
 
+'''
+Calculate brightness/to greyscale from RGB:
+http://ieeexplore.ieee.org/stamp/stamp.jsp?arnumber=5445596
+https://en.wikipedia.org/wiki/YUV
+Formula:
+Y=(0.299 x R) + (0.587 x G) + (0.114 x B);
+'''
+
 
 class Color(tuple, Enum):
     RED = (90, 40, 25)
@@ -23,21 +31,26 @@ class Color(tuple, Enum):
 class Sensor:
     def __init__(self):
         self._sensor = ev3.ColorSensor()
-        self._sensor.mode = 'COL-REFLECT'
+        self._sensor.mode = 'RGB-RAW'
+        self.RGB = self._sensor.bin_data("hhh")
         self.lastcolor = None
 
+    def refresh(self):
+        self.RGB = self._sensor.bin_data("hhh")
+
     def getbrightness(self):
-        if(self._sensor.mode == 'COL-REFLECT'):
-            return self._sensor.value()
+        # if(self._sensor.mode == 'COL-REFLECT'):
+        if(self._sensor.mode == 'RGB-RAW'):
+            return ((0.299 * self.RGB[0]) + (0.587 * self.RGB[1]) + (0.114 * self.RGB[2]))
+            # return self._sensor.value()
         else:
             print("ERROR: Wrong Sensor Mode.")
 
     def getcolor(self):
         if(self._sensor.mode == 'RGB-RAW'):
-            RGB = self._sensor.bin_data("hhh")
-            if(RGB[0] > Color.RED[0] and (RGB[1], RGB[2]) < (Color.RED[1], Color.RED[2])):
+            if(self.RGB[0] > Color.RED[0] and (self.RGB[1], self.RGB[2]) < (Color.RED[1], Color.RED[2])):
                 self.lastcolor = Color.RED
-            if(RGB[0] < Color.BLUE[0] and (RGB[1], RGB[2]) > (Color.BLUE[1], Color.BLUE[2])):
+            if(self.RGB[0] < Color.BLUE[0] and (self.RGB[1], self.RGB[2]) > (Color.BLUE[1], Color.BLUE[2])):
                 self.lastcolor = Color.BLUE
 
     def setmode(self, newmode):