Update utils/tests/colorsensortest.py

utils/tests/colorsensortest.py

@@ -15,11 +15,10 @@ color_sensor = ColorSensor(Port.F)
 print("Default Detected Colors:", color_sensor.detectable_colors())
 
 # Custom color Hue, Saturation, Brightness value for Lego bricks
-Color.MAGENTA = Color(300,100,100)
-Color.VIOLET =  Color(277,68,32)
+Color.MAGENTA = Color(315,100,60)
 Color.BLUE = Color(240,100,100)
 Color.CYAN = Color(180,100,100)
-
+Color.RED = Color(350, 100, 100)
 LEGO_BRICKS_COLOR = [
     Color.BLUE, 
     Color.GREEN, 
@@ -27,19 +26,44 @@ LEGO_BRICKS_COLOR = [
     Color.RED, 
     Color.YELLOW, 
     Color.MAGENTA, 
-    Color.VIOLET,
     Color.NONE
 ]
+magenta_counter = 0
+stable_color = None
+real_color = None
 #Update Detectable colors
 color_sensor.detectable_colors(LEGO_BRICKS_COLOR)
 print(f'Yellow:{Color.YELLOW} : {Color.YELLOW.h}, {Color.YELLOW.s}, {Color.YELLOW.v}')
 print("Updated Detected Colors:", color_sensor.detectable_colors())
 async def main():  
     while True:
+        global magenta_counter, stable_color, real_color
         color_reflected_percent = await color_sensor.reflection()
-        print(color_reflected_percent)
-        
-        if color_reflected_percent > 1:    # Make sure we actually have color reflections before checking for color
+        print("Reflection: ", color_reflected_percent)
+        if color_reflected_percent > 15:
             color_detected = await color_sensor.color()
-            print(f'Detected color:{color_detected} : {color_detected.h}, {color_detected.s}, {color_detected.v}')
+
+            if color_detected == Color.MAGENTA:
+                magenta_counter += 1
+            else:
+                magenta_counter = 0
+                stable_color = color_detected
+
+            # Only accept magenta if it's been stable for a while - usually triggers before other colors so we gotta do this :|
+            if magenta_counter > 10:
+                stable_color = Color.MAGENTA
+                if stable_color != Color.MAGENTA:
+                    stable_color = await color_sensor.color()
+
+            real_color = stable_color
+            #if(color_detected != Color.NONE):
+            #    return
+            
+            print("Magenta counter: ", magenta_counter)
+            if real_color is not None:
+                print(f'Detected color: {real_color} : {real_color.h}, {real_color.s}, {real_color.v}')
+            else:
+                print("No valid color detected yet.")
+        await wait(50)
+
 run_task(main())