diff --git a/README.md b/README.md index 20abd34..d7d8320 100644 --- a/README.md +++ b/README.md @@ -70,4 +70,76 @@ You can now use Arduino Lab for Micropython to run your examples remotely from t
+## 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. + +
+ +### 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`. + +
+ +### 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`. + + + +
+
+
+ __Note: not open bin files with Arduino Lab for Micropython because they will be corrupted__ diff --git a/arduino_alvik/arduino_alvik.py b/arduino_alvik/arduino_alvik.py index 506295f..69c7e49 100644 --- a/arduino_alvik/arduino_alvik.py +++ b/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,9 +230,14 @@ 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: """ @@ -239,14 +245,16 @@ def is_target_reached(self) -> bool: 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() diff --git a/arduino_alvik/constants.py b/arduino_alvik/constants.py index 1076df3..47a7c06 100644 --- a/arduino_alvik/constants.py +++ b/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] \ No newline at end of file diff --git a/arduino_alvik/firmware_updater.py b/arduino_alvik/firmware_updater.py deleted file mode 100644 index 0367704..0000000 --- a/arduino_alvik/firmware_updater.py +++ /dev/null @@ -1,24 +0,0 @@ -from sys import exit -from stm32_flash import * - -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("firmware.bin") -print("\nDONE") -print("\nLower Boot0 and reset STM32") - -STM32_endCommunication() diff --git a/arduino_alvik/robot_definitions.py b/arduino_alvik/robot_definitions.py index 142adfa..26a2e51 100644 --- a/arduino_alvik/robot_definitions.py +++ b/arduino_alvik/robot_definitions.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 diff --git a/examples/demo.py b/examples/demo.py new file mode 100644 index 0000000..931aa47 --- /dev/null +++ b/examples/demo.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() diff --git a/examples/flash_firmware.py b/examples/flash_firmware.py new file mode 100644 index 0000000..e96a3cf --- /dev/null +++ b/examples/flash_firmware.py @@ -0,0 +1,5 @@ +# from machine import reset +from arduino_alvik import update_firmware + +update_firmware('./firmware.bin') +# reset() diff --git a/examples/hand_follower.py b/examples/hand_follower.py index 5a031f3..fcdc41e 100644 --- a/examples/hand_follower.py +++ b/examples/hand_follower.py @@ -7,14 +7,32 @@ reference = 10.0 -while True: - try: - L, CL, C, CR, R = alvik.get_distance() - print(f'C: {C}') - error = C - reference - alvik.set_wheels_speed(error*10, error*10) - sleep_ms(100) - except KeyboardInterrupt as e: - print('over') - alvik.stop() - sys.exit() \ No newline at end of file +alvik.left_led.set_color(0, 1, 0) +alvik.right_led.set_color(0, 1, 0) + +while alvik.get_touch_ok(): + sleep_ms(50) + +while not alvik.get_touch_ok(): + sleep_ms(50) + +try: + while True: + while not alvik.get_touch_cancel(): + alvik.left_led.set_color(0, 0, 0) + alvik.right_led.set_color(0, 0, 0) + L, CL, C, CR, R = alvik.get_distance() + print(f'C: {C}') + error = C - reference + alvik.set_wheels_speed(error*10, error*10) + sleep_ms(100) + + while not alvik.get_touch_ok(): + alvik.left_led.set_color(0, 1, 0) + alvik.right_led.set_color(0, 1, 0) + alvik.brake() + sleep_ms(100) +except KeyboardInterrupt as e: + print('over') + alvik.stop() + sys.exit() diff --git a/examples/line_follower.py b/examples/line_follower.py index e50136f..041faf6 100644 --- a/examples/line_follower.py +++ b/examples/line_follower.py @@ -23,6 +23,9 @@ def calculate_center(left: int, center: int, right: int): alvik.left_led.set_color(0, 0, 1) alvik.right_led.set_color(0, 0, 1) +while alvik.get_touch_ok(): + sleep_ms(50) + while not alvik.get_touch_ok(): sleep_ms(50) diff --git a/examples/main.py b/examples/main.py new file mode 100644 index 0000000..7713957 --- /dev/null +++ b/examples/main.py @@ -0,0 +1 @@ +import demo \ No newline at end of file diff --git a/examples/touch_move.py b/examples/touch_move.py new file mode 100644 index 0000000..b51e4ca --- /dev/null +++ b/examples/touch_move.py @@ -0,0 +1,102 @@ +from arduino_alvik import ArduinoAlvik +from time import sleep_ms +import sys + +alvik = ArduinoAlvik() +alvik.begin() + +alvik.left_led.set_color(1, 0, 0) +alvik.right_led.set_color(1, 0, 0) + +movements = [] + + +def blink(): + alvik.left_led.set_color(1, 0, 1) + alvik.right_led.set_color(1, 0, 1) + sleep_ms(200) + alvik.left_led.set_color(1, 0, 0) + alvik.right_led.set_color(1, 0, 0) + + +def add_movement(): + global movements + + if alvik.get_touch_up(): + movements.append('forward') + blink() + while alvik.get_touch_up(): + sleep_ms(100) + if alvik.get_touch_down(): + movements.append('backward') + blink() + while alvik.get_touch_down(): + sleep_ms(100) + if alvik.get_touch_left(): + movements.append('left') + blink() + while alvik.get_touch_left(): + sleep_ms(100) + if alvik.get_touch_right(): + movements.append('right') + blink() + while alvik.get_touch_right(): + sleep_ms(100) + if alvik.get_touch_cancel(): + movements = [] + for i in range(0, 3): + val = i % 2 + alvik.left_led.set_color(val, 0, 0) + alvik.right_led.set_color(val, 0, 0) + sleep_ms(200) + while alvik.get_touch_cancel(): + sleep_ms(100) + + +def run_movement(movement): + if movement == 'forward': + alvik.move(10, blocking=False) + if movement == 'backward': + alvik.move(-10, blocking=False) + if movement == 'left': + alvik.rotate(90, blocking=False) + if movement == 'right': + alvik.rotate(-90, blocking=False) + while not alvik.get_touch_cancel() and not alvik.is_target_reached(): + alvik.left_led.set_color(1, 0, 0) + alvik.right_led.set_color(1, 0, 0) + sleep_ms(100) + alvik.left_led.set_color(0, 0, 0) + alvik.right_led.set_color(0, 0, 0) + sleep_ms(100) + +while alvik.get_touch_ok(): + sleep_ms(50) + +while not (alvik.get_touch_ok() and len(movements) != 0): + add_movement() + sleep_ms(50) + +try: + while True: + alvik.left_led.set_color(0, 0, 0) + alvik.right_led.set_color(0, 0, 0) + for move in movements: + run_movement(move) + if alvik.get_touch_cancel(): + break + + movements = [] + + while not (alvik.get_touch_ok() and len(movements) != 0): + alvik.left_led.set_color(1, 0, 0) + alvik.right_led.set_color(1, 0, 0) + alvik.brake() + add_movement() + sleep_ms(100) +except KeyboardInterrupt as e: + print('over') + alvik.stop() + sys.exit() + + diff --git a/install.bat b/install.bat index 4e17379..efbd576 100644 --- a/install.bat +++ b/install.bat @@ -17,6 +17,7 @@ if /i "%1"=="-h" ( :install +python -m mpremote %port_string% fs mkdir lib python -m mpremote %port_string% fs mkdir lib/arduino_alvik python -m mpremote %port_string% fs cp arduino_alvik/__init__.py :lib/arduino_alvik/__init__.py python -m mpremote %port_string% fs cp arduino_alvik/arduino_alvik.py :lib/arduino_alvik/arduino_alvik.py @@ -24,6 +25,7 @@ python -m mpremote %port_string% fs cp arduino_alvik/constants.py :lib/arduino_a python -m mpremote %port_string% fs cp arduino_alvik/conversions.py :lib/arduino_alvik/conversions.py python -m mpremote %port_string% fs cp arduino_alvik/pinout_definitions.py :lib/arduino_alvik/pinout_definitions.py python -m mpremote %port_string% fs cp arduino_alvik/robot_definitions.py :lib/arduino_alvik/robot_definitions.py +python -m mpremote %port_string% fs cp arduino_alvik/stm32_flash.py :lib/arduino_alvik/stm32_flash.py python -m mpremote %port_string% fs cp arduino_alvik/uart.py :lib/arduino_alvik/uart.py echo Installing dependencies diff --git a/install.sh b/install.sh index 736166c..e606f57 100644 --- a/install.sh +++ b/install.sh @@ -42,6 +42,7 @@ fi # Uncomment the following line on windows machines # python_command="python" +$python_command -m mpremote $connect_string fs mkdir lib $python_command -m mpremote $connect_string fs mkdir lib/arduino_alvik $python_command -m mpremote $connect_string fs cp arduino_alvik/__init__.py :lib/arduino_alvik/__init__.py $python_command -m mpremote $connect_string fs cp arduino_alvik/arduino_alvik.py :lib/arduino_alvik/arduino_alvik.py @@ -49,6 +50,7 @@ $python_command -m mpremote $connect_string fs cp arduino_alvik/constants.py :li $python_command -m mpremote $connect_string fs cp arduino_alvik/conversions.py :lib/arduino_alvik/conversions.py $python_command -m mpremote $connect_string fs cp arduino_alvik/pinout_definitions.py :lib/arduino_alvik/pinout_definitions.py $python_command -m mpremote $connect_string fs cp arduino_alvik/robot_definitions.py :lib/arduino_alvik/robot_definitions.py +$python_command -m mpremote $connect_string fs cp arduino_alvik/stm32_flash.py :lib/arduino_alvik/stm32_flash.py $python_command -m mpremote $connect_string fs cp arduino_alvik/uart.py :lib/arduino_alvik/uart.py echo "Installing dependencies" diff --git a/package.json b/package.json index 90b6c77..1bb63ac 100644 --- a/package.json +++ b/package.json @@ -7,11 +7,10 @@ ["arduino_alvik/pinout_definitions.py", "github:arduino/arduino-alvik-mpy/arduino_alvik/pinout_definitions.py"], ["arduino_alvik/robot_definitions.py", "github:arduino/arduino-alvik-mpy/arduino_alvik/robot_definitions.py"], ["arduino_alvik/uart.py", "github:arduino/arduino-alvik-mpy/arduino_alvik/uart.py"], - ["arduino_alvik/firmware_updater.py", "github:arduino/arduino-alvik-mpy/arduino_alvik/firmware_updater.py"], ["arduino_alvik/stm32_flash.py", "github:arduino/arduino-alvik-mpy/arduino_alvik/stm32_flash.py"] ], "deps": [ ["github:arduino/ucPack-mpy", "0.1.5"] ], - "version": "0.3.0" + "version": "0.4.0" } \ No newline at end of file diff --git a/utilities/flash_firmware.bat b/utilities/flash_firmware.bat index f883fee..8fc2d1b 100644 --- a/utilities/flash_firmware.bat +++ b/utilities/flash_firmware.bat @@ -24,11 +24,6 @@ if "%1"=="" ( set "filename=%1" ) -echo Installing flash firmware utilities... - -python -m mpremote %port_string% fs cp ../arduino_alvik/firmware_updater.py :firmware_updater.py -python -m mpremote %port_string% fs cp ../arduino_alvik/stm32_flash.py :stm32_flash.py - echo Uploading %filename% python -m mpremote %port_string% fs cp %filename% :firmware.bin @@ -36,7 +31,7 @@ python -m mpremote %port_string% fs cp %filename% :firmware.bin set /p userInput=Do you want to flash the firmware right now? (y/N): if /i "%userInput%"=="y" ( - python -m mpremote %port_string% run ../arduino_alvik/firmware_updater.py + python -m mpremote %port_string% run ../examples/flash_firmware.py ) else ( echo The firmware will not be written to the device. ) diff --git a/utilities/flash_firmware.sh b/utilities/flash_firmware.sh index c973ad8..81bd12d 100644 --- a/utilities/flash_firmware.sh +++ b/utilities/flash_firmware.sh @@ -51,11 +51,6 @@ fi # Uncomment the following line on windows machines # python_command="python" -echo "Installing flash firmware utilities..." - -$python_command -m mpremote $connect_string fs cp ../arduino_alvik/firmware_updater.py :firmware_updater.py -$python_command -m mpremote $connect_string fs cp ../arduino_alvik/stm32_flash.py :stm32_flash.py - echo "Uploading $filename..." $python_command -m mpremote $connect_string fs cp $filename :firmware.bin @@ -64,7 +59,7 @@ echo "Do you want to flash the firmware right now? (y/N)" read do_flash if [ "$do_flash" == "y" ] || [ "$do_flash" == "Y" ]; then - $python_command -m mpremote $connect_string run ../arduino_alvik/firmware_updater.py + $python_command -m mpremote $connect_string run ../examples/flash_firmware.py else echo "The firmware will not be written to the device." fi