feat(robot): calibrate the color sensor of the robot (#36)

* calibration

* README

* updated readme

Author: dido18

README.md

@@ -24,7 +24,22 @@ The project is composed by two components:
 6. Copy the printed MAC address
 7. `make robot-upload` to upload the code into the alvik
 
-NOTE: the PATCH steps `2` and `3` are no more necessary when the [Arduino_AlvikCarrier](https://github.com/arduino-libraries/Arduino_AlvikCarrier) and the [Arduino-Alvik-mpy](https://github.com/arduino/arduino-alvik-mpy) are merged to master and release ad an official release.
+NOTE: the PATCH steps `2` and `3` are no more necessary when the [Arduino_AlvikCarrier](https://github.com/arduino-libraries/Arduino_AlvikCarrier) and the [Arduino-Alvik-mpy](https://github.com/arduino/arduino-alvik-mpy) are merged to master and released.
+
+#### Color calibration mode
+The robot exposes a `color calibration mode` that can be activated by pressing the `ok` button.
+The robot guides the calibration using different lights code showed on the Modulino Pixels.
+The user must move the robot on WHITE and BLACK color surfaces when a specific lights code is shown.
+
+Steps to calibrate the robot
+0. Prepare a WHITE and BLACK surface
+1. Switch on the robot
+2. Press the `ok` button to enter into the `calibration mode`. The mode has the following light codes
+   - WHITE leds countdown: indicate that the robot must be put on WHITE color. At the end of the countdown, the robot calibrates the WHITE color.
+   - GREEN leds for 2 seconds: white calibration terminated
+   - BLUE leds countdown: indicate that the robot must be put on BLACK color. At the end of the countdown, the robot calibrates the BLACK color
+   - GREEN leds for 2 seconds: black calibration terminated.
+   - the robot automatically reset itself.
 
 ### Configure the controller
 1. Connect the controller

sketches/robot/main.py

@@ -3,6 +3,7 @@
 import struct
 import random
 import sys
+import machine
 
 from arduino_alvik import ArduinoAlvik
 from time import sleep_ms, ticks_add, ticks_diff, ticks_ms
@@ -125,54 +126,105 @@ def map_value(value, from_low, from_high, to_low, to_high):
     )
 
 
+def countdown_color(color, ledoff_every_tick=2):
+    for i in range(7, 0, -ledoff_every_tick):
+        pixels.set_range_color(0, i, color)
+        pixels.show()
+        sleep_ms(500)
+        pixels.clear_all()
+        pixels.show()
+        sleep_ms(500)
+    pixels.clear_all()
+    pixels.show()
+
+def calibrate_color():
+    print("Reading white color")
+    countdown_color(ModulinoColor.WHITE)
+    a.color_calibration("white")
+
+    pixels.set_all_color(ModulinoColor.GREEN, 15)
+    pixels.show()
+    sleep_ms(2000)
+
+    print("Reading black color")
+    countdown_color(ModulinoColor.BLUE)
+    a.color_calibration("black")
+
+    pixels.set_all_color(ModulinoColor.GREEN, 15)
+    pixels.show()
+    sleep_ms(2000)
+
+    pixels.clear_all()
+    pixels.show()
+
+    # hard-reset the board to refresh the calibration (read again the color from the file)
+    machine.reset()
+
+
+STATE_SETUP = -1
 STATE_INIT = 0
 STATE_PLAY = 1
 
 state = STATE_INIT
 
 deadline = 0
 while True:
-    if state == STATE_INIT:
-        a.drive(0, 0)
-        showEndAnimation()
-        if a.get_color_label() is not "BLACK":
-            showReadyToPlayLeds()
-            state = STATE_PLAY
-
-    elif state == STATE_PLAY:
-        receiveAndExecuteFromEspNow()
-        color = a.get_color_label()
-        if color == "BLACK":
-            state = STATE_INIT
-        elif color == "YELLOW":
-            pixels.set_all_color(ModulinoColor.YELLOW, 15)
-            pixels.show()
-            deg = random.choice([30.0, 45.0, 90.0, 130.0, 150.0, 180.0, 275.0, 360.0])
-            a.rotate(deg, "deg")
-            showReadyToPlayLeds()
-        elif color == "BLUE":
+    try:
+        if state == STATE_SETUP:
+            calibrate_color()
+
+        if state == STATE_INIT:
             a.drive(0, 0)
-            for x in range(0, FREEZE_FOR_SECONDS):
-                sleep_ms(500)
-                pixels.set_all_color(ModulinoColor.BLUE, 15)
+            showEndAnimation()
+            if a.get_touch_ok():
+                state = STATE_SETUP
+            if a.get_color_label() is not "BLACK":
+                showReadyToPlayLeds()
+                state = STATE_PLAY
+
+        elif state == STATE_PLAY:
+            receiveAndExecuteFromEspNow()
+
+            if a.get_touch_ok():
+                state = STATE_SETUP
+
+            color = a.get_color_label()
+            if color == "BLACK":
+                state = STATE_INIT
+            elif color == "YELLOW":
+                pixels.set_all_color(ModulinoColor.YELLOW, 15)
                 pixels.show()
-                sleep_ms(500)
+                deg = random.choice([30.0, 45.0, 90.0, 130.0, 150.0, 180.0, 275.0, 360.0])
+                a.rotate(deg, "deg")
+                showReadyToPlayLeds()
+            elif color == "BLUE":
+                a.drive(0, 0)
+                for x in range(0, FREEZE_FOR_SECONDS):
+                    sleep_ms(500)
+                    pixels.set_all_color(ModulinoColor.BLUE, 15)
+                    pixels.show()
+                    sleep_ms(500)
+                    pixels.clear_all()
+                    pixels.show()
+                showReadyToPlayLeds()
+            elif color == "GREEN" or color == "LIGHT GREEN":
+                if not isPlayingReverted:
+                    deadline = ticks_add(ticks_ms(), REVERT_CONTROLLER_FOR_SECONDS * 1000)
+                    isPlayingReverted = True
+
+            if isPlayingReverted:
+                elapsed = ticks_diff(deadline, ticks_ms())
+                mapped = map_value(elapsed, 0, REVERT_CONTROLLER_FOR_SECONDS * 1000, 0, 7)
                 pixels.clear_all()
+                pixels.set_range_color(0, mapped, ModulinoColor.VIOLET)
                 pixels.show()
-            showReadyToPlayLeds()
-        elif color == "GREEN" or color == "LIGHT GREEN":
-            if not isPlayingReverted:
-                deadline = ticks_add(ticks_ms(), REVERT_CONTROLLER_FOR_SECONDS * 1000)
-                isPlayingReverted = True
-
-        if isPlayingReverted:
-            elapsed = ticks_diff(deadline, ticks_ms())
-            mapped = map_value(elapsed, 0, REVERT_CONTROLLER_FOR_SECONDS * 1000, 0, 7)
-            pixels.clear_all()
-            pixels.set_range_color(0, mapped, ModulinoColor.VIOLET)
-            pixels.show()
-            if elapsed < 0:
-                showReadyToPlayLeds()
-                isPlayingReverted = False
-
+                if elapsed < 0:
+                    showReadyToPlayLeds()
+                    isPlayingReverted = False
+    except AssertionError:
+        print("AssertionError")
+        # If calibration is not done correctly, the _limit() function can raise an AssertError
+        # See https://github.com/arduino/arduino-alvik-mpy/blob/80e66561a2ae06c69adddb3adc21ca88f91a57dd/arduino_alvik/arduino_alvik.py#L724
+        state = STATE_SETUP
+        continue
     sleep_ms(50)