Merge pull request #18 from arduino/dev

0.4.0 RC1

Author: gbr1

README.md

@@ -70,4 +70,76 @@ You can now use Arduino Lab for Micropython to run your examples remotely from t
 
 <br>
 
+## Default Demo
+
+Use `mpremote` to copy following the files from the examples folder:
+- `main.py`, this file allows you to automatically start the demo
+- `demo.py`, demo launcher
+- `touch_move.py`, programming the robot movements via touch buttons demo
+- `line_follower.py`, black line follower demo
+- `hand_follower.py`, hand following demo, the robot stays always at 10cm from an obstacle placed in front of it.
+
+When the robot is turned on, the demo launcher starts after Alvik boot.
+
+Blue leds on top turn on.
+
+By pressing up and down arrows, it is possible to select different demos identified by different colors (blue, green and red).
+
+Each color allows to run a different demo as following:
+- `red` launches the touch-move demo
+- `green` launches the hand following demo
+- `blue` launches the line follower demo
+
+To run a demo, press the `OK touch button`, after selecting the right demo color.
+
+To run a different demo, turn the robot off and on again or reset the Arduino® Nano ESP32.
+
+### 1. Touch mode example (RED)
+This example starts with the red leds on.
+
+`directional touch buttons` (UP, DOWN, LEFT, RIGHT) program the desired movements.
+
+Everytime a `directional touch button` is pressed, the leds blink in a purple color indicating that the command has been registered.
+- `UP touch button` will register a 10 cm forward movement
+- `DOWN touch button` will register a 10 cm backward movement
+- `RIGHT touch button` will register a 90° clockwise rotation movement
+- `LEFT touch button` will register a 90° counterclockwise rotation movement
+
+To clear the commands queue, press the `CANCEL touch button`.
+The leds will blink in red.
+
+To start the sequence, press the `OK touch button`.
+
+Pressing the `CANCEL touch button` at any time stops the robot and resets the sequence.
+
+<br>
+
+### 2. Hand follower example (GREEN)
+This example starts with the green leds on.
+
+Place an obstacle or your hand in front of the robot.
+
+To start the robot press the `OK touch button`.
+
+The robot will move to keep a 10 centimeters distance from the obstacle/hand.
+
+It is possible to stop the robot at any time by pressing the `CANCEL touch button`.
+
+<br>
+
+### 3. Line Follower example (BLUE)
+This example starts with the blue leds on.
+
+To run this example, a white board and black tape (2cm wide) is required.
+
+Place the robot at the center of the line and press the `OK touch button`.
+
+It is possible to stop the robot at any time by pressing the `CANCEL touch button`.
+
+
+
+<br>
+<br>
+<br>
+
 __Note: not open bin files with Arduino Lab for Micropython because they will be corrupted__

arduino_alvik/arduino_alvik.py

@@ -3,7 +3,7 @@
 import struct
 from machine import I2C
 import _thread
-from time import sleep_ms
+from time import sleep_ms, ticks_ms, ticks_diff
 
 from ucPack import ucPack
 
@@ -15,7 +15,6 @@
 
 
 class ArduinoAlvik:
-
     _update_thread_running = False
     _update_thread_id = None
     _touch_events_thread_running = False
@@ -31,10 +30,10 @@ def __init__(self):
         self.left_wheel = _ArduinoAlvikWheel(self._packeter, ord('L'))
         self.right_wheel = _ArduinoAlvikWheel(self._packeter, ord('R'))
         self._led_state = list((None,))
-        self.left_led = _ArduinoAlvikRgbLed(self._packeter, 'left', self._led_state,
-                                            rgb_mask=[0b00000100, 0b00001000, 0b00010000])
-        self.right_led = _ArduinoAlvikRgbLed(self._packeter, 'right', self._led_state,
-                                             rgb_mask=[0b00100000, 0b01000000, 0b10000000])
+        self.left_led = self.DL1 = _ArduinoAlvikRgbLed(self._packeter, 'left', self._led_state,
+                                                       rgb_mask=[0b00000100, 0b00001000, 0b00010000])
+        self.right_led = self.DL2 = _ArduinoAlvikRgbLed(self._packeter, 'right', self._led_state,
+                                                        rgb_mask=[0b00100000, 0b01000000, 0b10000000])
         self._battery_perc = None
         self._touch_byte = None
         self._behaviour = None
@@ -67,7 +66,8 @@ def __init__(self):
         self._bottom_tof = None
         self._linear_velocity = None
         self._angular_velocity = None
-        self._last_ack = ''
+        self._last_ack = None
+        self._waiting_ack = None
         self._version = [None, None, None]
         self._touch_events = _ArduinoAlvikTouchEvents()
 
@@ -131,7 +131,7 @@ def _idle(self, delay_=1, check_on_thread=False) -> None:
                     LEDR.value(led_val)
                     LEDG.value(1)
                     led_val = (led_val + 1) % 2
-            print("Alvik is on")
+            print("********** Alvik is on **********")
         except KeyboardInterrupt:
             self.stop()
             sys.exit()
@@ -176,7 +176,7 @@ def begin(self) -> int:
         :return:
         """
         if not self.is_on():
-            print("\nTurn on your Arduino Alvik!\n")
+            print("\n********** Please turn on your Arduino Alvik! **********\n")
             sleep_ms(1000)
             self._idle(1000)
         self._begin_update_thread()
@@ -199,6 +199,7 @@ def _wait_for_ack(self) -> None:
         Waits until receives 0x00 ack from robot
         :return:
         """
+        self._waiting_ack = 0x00
         while self._last_ack != 0x00:
             sleep_ms(20)
 
@@ -229,24 +230,31 @@ def _stop_update_thread(cls):
         """
         cls._update_thread_running = False
 
-    def _wait_for_target(self):
-        while not self.is_target_reached():
-            pass
+    def _wait_for_target(self, idle_time):
+        start = ticks_ms()
+        while True:
+            if ticks_diff(ticks_ms(), start) >= idle_time * 1000 and self.is_target_reached():
+                break
+            else:
+                # print(self._last_ack)
+                sleep_ms(100)
 
     def is_target_reached(self) -> bool:
         """
         Returns True if robot has sent an M or R acknowledgment.
         It also responds with an ack received message
         :return:
         """
-        if self._last_ack != ord('M') and self._last_ack != ord('R'):
-            sleep_ms(50)
-            return False
-        else:
+        if self._waiting_ack is None:
+            return True
+        if self._last_ack == self._waiting_ack:
             self._packeter.packetC1B(ord('X'), ord('K'))
             uart.write(self._packeter.msg[0:self._packeter.msg_size])
-            sleep_ms(200)
+            sleep_ms(100)
+            self._last_ack = 0x00
+            self._waiting_ack = None
             return True
+        return False
 
     def set_behaviour(self, behaviour: int):
         """
@@ -269,8 +277,9 @@ def rotate(self, angle: float, unit: str = 'deg', blocking: bool = True):
         sleep_ms(200)
         self._packeter.packetC1F(ord('R'), angle)
         uart.write(self._packeter.msg[0:self._packeter.msg_size])
+        self._waiting_ack = ord('R')
         if blocking:
-            self._wait_for_target()
+            self._wait_for_target(idle_time=(angle / MOTOR_CONTROL_DEG_S))
 
     def move(self, distance: float, unit: str = 'cm', blocking: bool = True):
         """
@@ -284,8 +293,9 @@ def move(self, distance: float, unit: str = 'cm', blocking: bool = True):
         sleep_ms(200)
         self._packeter.packetC1F(ord('G'), distance)
         uart.write(self._packeter.msg[0:self._packeter.msg_size])
+        self._waiting_ack = ord('M')
         if blocking:
-            self._wait_for_target()
+            self._wait_for_target(idle_time=(distance / MOTOR_CONTROL_MM_S))
 
     def stop(self):
         """
@@ -610,7 +620,11 @@ def _parse_message(self) -> int:
             _, self._linear_velocity, self._angular_velocity = self._packeter.unpacketC2F()
         elif code == ord('x'):
             # robot ack
-            _, self._last_ack = self._packeter.unpacketC1B()
+            if self._waiting_ack is not None:
+                _, self._last_ack = self._packeter.unpacketC1B()
+            else:
+                self._packeter.unpacketC1B()
+                self._last_ack = 0x00
         elif code == ord('z'):
             # robot ack
             _, self._x, self._y, self._theta = self._packeter.unpacketC3F()
@@ -899,9 +913,9 @@ def hsv2label(h, s, v) -> str:
                     label = 'GREEN'
                 elif 170 <= h < 210:
                     label = 'LIGHT BLUE'
-                elif 210 <= h < 260:
+                elif 210 <= h < 250:
                     label = 'BLUE'
-                elif 260 <= h < 280:
+                elif 250 <= h < 280:
                     label = 'VIOLET'
                 else:  # h<20 or h>=280 is more problematic
                     if v < 0.5 and s < 0.45:
@@ -1322,3 +1336,44 @@ def register_callback(self, event_name: str, callback: callable, args: tuple = N
         if event_name not in self.__class__.available_events:
             return
         super().register_callback(event_name, callback, args)
+
+
+# UPDATE FIRMWARE METHOD #
+
+def update_firmware(file_path: str):
+    """
+
+    :param file_path: path of your FW bin
+    :return:
+    """
+
+    from sys import exit
+    from stm32_flash import (
+        CHECK_STM32,
+        STM32_endCommunication,
+        STM32_startCommunication,
+        STM32_NACK,
+        STM32_eraseMEM,
+        STM32_writeMEM, )
+
+    if CHECK_STM32.value() is not 1:
+        print("Turn on your Alvik to continue...")
+        while CHECK_STM32.value() is not 1:
+            sleep_ms(500)
+
+    ans = STM32_startCommunication()
+    if ans == STM32_NACK:
+        print("Cannot establish connection with STM32")
+        exit(-1)
+
+    print('\nSTM32 FOUND')
+
+    print('\nERASING MEM')
+    STM32_eraseMEM(0xFFFF)
+
+    print("\nWRITING MEM")
+    STM32_writeMEM(file_path)
+    print("\nDONE")
+    print("\nLower Boot0 and reset STM32")
+
+    STM32_endCommunication()

arduino_alvik/constants.py

@@ -1,5 +1,5 @@
 
 # COLOR SENSOR
 COLOR_FULL_SCALE = 4097
-WHITE_CAL = [444, 342, 345]
-BLACK_CAL = [153, 135, 123]
+WHITE_CAL = [450, 500, 510]
+BLACK_CAL = [160, 200, 190]

arduino_alvik/firmware_updater.py

@@ -3,6 +3,8 @@
 MOTOR_KI_DEFAULT = 450.0
 MOTOR_KD_DEFAULT = 0.0
 MOTOR_MAX_RPM = 70.0
+MOTOR_CONTROL_DEG_S = 100
+MOTOR_CONTROL_MM_S = 100
 WHEEL_TRACK_MM = 89.0
 
 # Wheels parameters

arduino_alvik/robot_definitions.py

@@ -0,0 +1,53 @@
+from arduino_alvik import ArduinoAlvik
+from time import sleep_ms
+import sys
+
+alvik = ArduinoAlvik()
+alvik.begin()
+
+menu_status = 0
+
+
+def update_led_status(val):
+    if val == 0:
+        alvik.left_led.set_color(0, 0, 1)
+        alvik.right_led.set_color(0, 0, 1)
+    elif val == 1:
+        alvik.left_led.set_color(0, 1, 0)
+        alvik.right_led.set_color(0, 1, 0)
+    elif val == -1:
+        alvik.left_led.set_color(1, 0, 0)
+        alvik.right_led.set_color(1, 0, 0)
+
+
+while True:
+
+    update_led_status(menu_status)
+
+    try:
+
+        if alvik.get_touch_ok():
+            if menu_status == 0:
+                import line_follower
+            elif menu_status == 1:
+                import hand_follower
+            elif menu_status == -1:
+                import touch_move
+
+        if alvik.get_touch_up() and menu_status < 1:
+            menu_status += 1
+            update_led_status(menu_status)
+            while alvik.get_touch_up():
+                sleep_ms(100)
+        if alvik.get_touch_down() and menu_status > -1:
+            menu_status -= 1
+            update_led_status(menu_status)
+            while alvik.get_touch_down():
+                sleep_ms(100)
+
+        sleep_ms(100)
+
+    except KeyboardInterrupt as e:
+        print('over')
+        alvik.stop()
+        sys.exit()

examples/demo.py

@@ -0,0 +1,5 @@
+# from machine import reset
+from arduino_alvik import update_firmware
+
+update_firmware('./firmware.bin')
+# reset()