From 00857da4700beb5af97a96c038127e248f9ba5f3 Mon Sep 17 00:00:00 2001 From: silbo Date: Thu, 24 Jun 2021 21:06:48 +0200 Subject: [PATCH] Imporove servo PWM calibration --- .gitignore | 2 +- LICENSE | 2 +- Makefile | 31 ++++++++--------- boot.py | 11 +++--- config.json | 10 +++--- main.py | 96 ++++++++++++++++++++++++++++++----------------------- 6 files changed, 83 insertions(+), 69 deletions(-) diff --git a/.gitignore b/.gitignore index 16fdd92..5b981d6 100755 --- a/.gitignore +++ b/.gitignore @@ -2,7 +2,7 @@ code.py # ignore backup files ._* -* Mac OS stuff +# Mac OS stuff .DS_Store # ignore the ESP32 MicroPython binary esp32*.bin diff --git a/LICENSE b/LICENSE index 66cefce..8fc7879 100755 --- a/LICENSE +++ b/LICENSE @@ -1,6 +1,6 @@ MIT License -Copyright (c) 2017 RoboKoding LTD +Copyright (c) 2021 RoboKoding LTD Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal diff --git a/Makefile b/Makefile index 8a6c8ad..ace1262 100755 --- a/Makefile +++ b/Makefile @@ -1,20 +1,15 @@ #!/bin/bash -SERIAL_PORT=/dev/ttyUSB0 +# When this baud does not work, try 115200 +FLASH_BAUD := 230400 -ifeq (,$(wildcard $(SERIAL_PORT))) - SERIAL_PORT=/dev/tty.usbserial-1410 -endif +# Image to flash +FLASH_IMAGE := sumofirmware.bin -ifeq (,$(wildcard $(SERIAL_PORT))) - SERIAL_PORT=/dev/tty.usbserial-1420 -endif +# Try to automatically find the serialport +SERIAL_PORT := $(shell find /dev -iname "tty*usb*") -ifeq (,$(wildcard $(SERIAL_PORT))) - SERIAL_PORT=/dev/tty.SLAB_USBtoUART -endif - -all: flash delay config update reset +all: erase flash delay config update reset delay: sleep 3 @@ -30,9 +25,15 @@ update: config: ampy -d 0.5 -p $(SERIAL_PORT) put config.json +erase: + esptool.py -p $(SERIAL_PORT) -b $(FLASH_BAUD) erase_flash + +image: + esptool.py -p $(SERIAL_PORT) -b $(FLASH_BAUD) read_flash 0x1000 0x250000 sumofirmware.bin + flash: - esptool.py -p $(SERIAL_PORT) -b 460800 erase_flash - esptool.py -p $(SERIAL_PORT) -b 460800 write_flash --flash_mode dio 0x1000 esp32*.bin + esptool.py -p $(SERIAL_PORT) -b $(FLASH_BAUD) write_flash --flash_mode dio 0x1000 $(FLASH_IMAGE) serial: - picocom --baud 115200 $(SERIAL_PORT) + picocom --baud 115200 -q $(SERIAL_PORT) + diff --git a/boot.py b/boot.py index 8c7964a..a5513c7 100755 --- a/boot.py +++ b/boot.py @@ -1,10 +1,11 @@ import os +import utime import machine -from utime import sleep_ms + # Give time to cancel this boot script print("Press Ctrl-C to stop new boot script...") -sleep_ms(1000) +utime.sleep_ms(1000) root_files = os.listdir() update_files = ['boot.py.new', 'main.py.new', 'hal.py.new'] @@ -13,14 +14,14 @@ files_to_update = [] # Check for FW updates and verify new FW files for file in update_files: if file in root_files: - print("boot.py: trying to update " + file) + print("boot.py: starting to update:", file) # Try to load the user code try: with open(file, 'r') as code: compile(code.read(), "snippet", 'exec') files_to_update.append(file) - except: - print("boot.py: " + file + " compilation failed") + except Exception as error: + print("boot.py:", file, "compilation failed:", error) files_to_update.clear() break diff --git a/config.json b/config.json index a64fdd4..7bfc5ac 100755 --- a/config.json +++ b/config.json @@ -2,12 +2,10 @@ "status_led_pin": 5, "battery_coeff": 2.25, "sumorobot_name": "SumoRobot", - "firmware_timestamp": "2019.11.17 16:23:00", - "firmware_version": "1.0.0", - "left_servo_min_tuning": 1, - "left_servo_max_tuning": 100, - "right_servo_min_tuning": 1, - "right_servo_max_tuning": 100, + "firmware_timestamp": "2021.06.04 22:23", + "firmware_version": "1.1", + "left_servo_calib": [37, 73, 81, 116], + "right_servo_calib": [37, 73, 81, 116], "sonar_threshold": 40, "boot_code": "code.py", "left_line_value": 1000, diff --git a/main.py b/main.py index 8882b27..c0cf091 100755 --- a/main.py +++ b/main.py @@ -1,45 +1,47 @@ -import ujson -import struct +import utime import _thread import ubluetooth -from machine import Timer -from micropython import const +import micropython from hal import * # Loading libraries takes ca 400ms + # BLE events -_IRQ_CENTRAL_CONNECT = const(1) -_IRQ_CENTRAL_DISCONNECT = const(2) -_IRQ_GATTS_READ_REQUEST = const(4) +_IRQ_CENTRAL_CONNECT = micropython.const(1) +_IRQ_CENTRAL_DISCONNECT = micropython.const(2) +_IRQ_GATTS_WRITE = micropython.const(3) -# Open and parse the config file -with open('config.json', 'r') as config_file: - config = ujson.load(config_file) +# SumoRobot functionality +sumorobot = Sumorobot() -# Initialize the SumoRobot object -sumorobot = Sumorobot(config) -# Advertise BLE name (SumoRobot name) def advertise_ble_name(name): - ble_name = bytes(name, 'ascii') - ble_name = bytearray((len(ble_name) + 1, 0x09)) + ble_name - ble.gap_advertise(100, bytearray('\x02\x01\x02') + ble_name) + payload = b'\x02\x01\x02' + bytes([len(name) + 1]) + payload += b'\x09' + name.encode() + ble.gap_advertise(100, payload) + def update_battery_level(timer): if conn_handle is not None: battery_level = sumorobot.get_battery_level() ble.gatts_notify(conn_handle, battery, bytes([battery_level])) -# The code processing thread -def process(): + +def sensor_feedback_thread(): + while True: + # Leave time to process other threads + utime.sleep_ms(50) + # Execute to see LED feedback for sensors + sumorobot.update_sensor_feedback() + + +def code_process_thread(): global prev_bat_level, python_code while True: - # Leave time to process other code - sleep_ms(50) - # Execute to see LED feedback for sensors - sumorobot.update_sensor_feedback() + # Leave time to process other threads + utime.sleep_ms(50) # When no code to execute if python_code == b'': @@ -47,12 +49,11 @@ def process(): # Try to execute the Python code try: - python_code = compile(python_code, "snippet", 'exec') - exec(python_code) - except: - print("main.py: the code sent had errors") + exec(compile(python_code, "snippet", 'exec')) + except Exception as error: + print("main.py: the python code had errors:", error) finally: - print("main.py: finized execution") + print("main.py: finized python code execution") # Erase the code python_code = b'' # Stop the robot @@ -60,6 +61,7 @@ def process(): # Cancel code termination sumorobot.terminate = False + # The BLE handler thread def ble_handler(event, data): global conn_handle, python_code, temp_python_code @@ -69,15 +71,17 @@ def ble_handler(event, data): # Turn ON the status LED sumorobot.set_led(STATUS, True) update_battery_level(None) + advertise_ble_name(sumorobot.config['sumorobot_name']) elif event is _IRQ_CENTRAL_DISCONNECT: conn_handle = None # Turn OFF status LED sumorobot.set_led(STATUS, False) # Advertise with name advertise_ble_name(sumorobot.config['sumorobot_name']) - elif event is _IRQ_GATTS_READ_REQUEST: + elif event is _IRQ_GATTS_WRITE: # Read the command cmd = ble.gatts_read(rx) + print(cmd) if b'' in cmd: python_code = b'' @@ -96,8 +100,13 @@ def ble_handler(event, data): python_code = b'' sumorobot.move(RIGHT) elif b'' in cmd: - print(sumorobot.get_sensor_scope()) ble.gatts_notify(conn_handle, tx, sumorobot.get_sensor_scope()) + elif b'' in cmd: + ble.gatts_notify(conn_handle, tx, sumorobot.get_configuration_scope()) + elif b'' in cmd: + servo, speed = cmd[5:].decode().split(',') + servo = LEFT if servo == 'LEFT' else RIGHT + sumorobot.pwm[servo].duty(int(speed)) elif b'' in cmd: temp_python_code = b'\n' elif b'' in cmd: @@ -107,24 +116,28 @@ def ble_handler(event, data): temp_python_code += cmd else: temp_python_code = b'' - print("main.py: unknown cmd=" + cmd) + print("main.py: unknown cmd=", cmd) + conn_handle = None temp_python_code = b'' python_code = b'' -# When user code (code.py) exists -if 'code.py' in root_files: - print("main.py: trying to load code.py") - # Try to load the user code +# When boot code exists +if sumorobot.config['boot_code'] in root_files: + print("main.py: trying to load", sumorobot.config['boot_code']) + # Try to load and compile the boot code try: - with open('code.py', 'r') as code: - python_code = compile(code.read(), "snippet", 'exec') - except: - print("main.py: code.py compilation failed") + with open(sumorobot.config['boot_code'], 'r') as file: + boot_code = file.read() + compile(boot_code, "snippet", 'exec') + python_code = boot_code + except Exception as error: + print("main.py:", sumorobot.config['boot_code'], "compilation failed:", error) # Start BLE ble = ubluetooth.BLE() +ble.config(gap_name=sumorobot.config['sumorobot_name']) ble.active(True) # Register the BLE hander @@ -160,11 +173,12 @@ ble.gatts_write(firmware, sumorobot.config['firmware_version']) # Start BLE advertising with name advertise_ble_name(sumorobot.config['sumorobot_name']) -# Start the code processing thread -_thread.start_new_thread(process, ()) +# Start the threads +_thread.start_new_thread(code_process_thread, ()) +_thread.start_new_thread(sensor_feedback_thread, ()) # Start BLE battery percentage update timer -battery_timer = Timer(Timer.PERIODIC) +battery_timer = machine.Timer(machine.Timer.PERIODIC) battery_timer.init(period=3000, callback=update_battery_level) # Clean up