k-space/sumorobot-firmware
10
0
Fork 0
Code Issues Pull Requests Wiki Activity

Imporove servo PWM calibration

Browse Source
This commit is contained in:
silbo 2021-06-24 21:06:48 +02:00
parent fc11ffdad4
commit 00857da470
6 changed files with 83 additions and 69 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.gitignore vendored
View File

@ -2,7 +2,7 @@
code.py code.py
# ignore backup files # ignore backup files
._* ._*
* Mac OS stuff # Mac OS stuff
.DS_Store .DS_Store
# ignore the ESP32 MicroPython binary # ignore the ESP32 MicroPython binary
esp32*.bin esp32*.bin

2
LICENSE
View File

@ -1,6 +1,6 @@
MIT License 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 Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal of this software and associated documentation files (the "Software"), to deal

31
Makefile
View File

@ -1,20 +1,15 @@
#!/bin/bash #!/bin/bash
SERIAL_PORT=/dev/ttyUSB0 # When this baud does not work, try 115200
FLASH_BAUD := 230400
ifeq (,$(wildcard $(SERIAL_PORT))) # Image to flash
SERIAL_PORT=/dev/tty.usbserial-1410 FLASH_IMAGE := sumofirmware.bin
endif
ifeq (,$(wildcard $(SERIAL_PORT))) # Try to automatically find the serialport
SERIAL_PORT=/dev/tty.usbserial-1420 SERIAL_PORT := $(shell find /dev -iname "tty*usb*")
endif
ifeq (,$(wildcard $(SERIAL_PORT))) all: erase flash delay config update reset
SERIAL_PORT=/dev/tty.SLAB_USBtoUART
endif
all: flash delay config update reset
delay: delay:
sleep 3 sleep 3
@ -30,9 +25,15 @@ update:
config: config:
ampy -d 0.5 -p $(SERIAL_PORT) put config.json 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: flash:
esptool.py -p $(SERIAL_PORT) -b 460800 erase_flash esptool.py -p $(SERIAL_PORT) -b $(FLASH_BAUD) write_flash --flash_mode dio 0x1000 $(FLASH_IMAGE)
esptool.py -p $(SERIAL_PORT) -b 460800 write_flash --flash_mode dio 0x1000 esp32*.bin
serial: serial:
picocom --baud 115200 $(SERIAL_PORT) picocom --baud 115200 -q $(SERIAL_PORT)

11
boot.py
View File

@ -1,10 +1,11 @@
import os import os
import utime
import machine import machine
from utime import sleep_ms
# Give time to cancel this boot script # Give time to cancel this boot script
print("Press Ctrl-C to stop new boot script...") print("Press Ctrl-C to stop new boot script...")
sleep_ms(1000) utime.sleep_ms(1000)
root_files = os.listdir() root_files = os.listdir()
update_files = ['boot.py.new', 'main.py.new', 'hal.py.new'] 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 # Check for FW updates and verify new FW files
for file in update_files: for file in update_files:
if file in root_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 to load the user code
try: try:
with open(file, 'r') as code: with open(file, 'r') as code:
compile(code.read(), "snippet", 'exec') compile(code.read(), "snippet", 'exec')
files_to_update.append(file) files_to_update.append(file)
except: except Exception as error:
print("boot.py: " + file + " compilation failed") print("boot.py:", file, "compilation failed:", error)
files_to_update.clear() files_to_update.clear()
break break

10
config.json
View File

@ -2,12 +2,10 @@
"status_led_pin": 5, "status_led_pin": 5,
"battery_coeff": 2.25, "battery_coeff": 2.25,
"sumorobot_name": "SumoRobot", "sumorobot_name": "SumoRobot",
"firmware_timestamp": "2019.11.17 16:23:00", "firmware_timestamp": "2021.06.04 22:23",
"firmware_version": "1.0.0", "firmware_version": "1.1",
"left_servo_min_tuning": 1, "left_servo_calib": [37, 73, 81, 116],
"left_servo_max_tuning": 100, "right_servo_calib": [37, 73, 81, 116],
"right_servo_min_tuning": 1,
"right_servo_max_tuning": 100,
"sonar_threshold": 40, "sonar_threshold": 40,
"boot_code": "code.py", "boot_code": "code.py",
"left_line_value": 1000, "left_line_value": 1000,

96
main.py
View File

@ -1,45 +1,47 @@
import ujson import utime
import struct
import _thread import _thread
import ubluetooth import ubluetooth
from machine import Timer import micropython
from micropython import const
from hal import * from hal import *
# Loading libraries takes ca 400ms # Loading libraries takes ca 400ms
# BLE events # BLE events
_IRQ_CENTRAL_CONNECT = const(1) _IRQ_CENTRAL_CONNECT = micropython.const(1)
_IRQ_CENTRAL_DISCONNECT = const(2) _IRQ_CENTRAL_DISCONNECT = micropython.const(2)
_IRQ_GATTS_READ_REQUEST = const(4) _IRQ_GATTS_WRITE = micropython.const(3)
# Open and parse the config file # SumoRobot functionality
with open('config.json', 'r') as config_file: sumorobot = Sumorobot()
config = ujson.load(config_file)
# Initialize the SumoRobot object
sumorobot = Sumorobot(config)
# Advertise BLE name (SumoRobot name)
def advertise_ble_name(name): def advertise_ble_name(name):
ble_name = bytes(name, 'ascii') payload = b'\x02\x01\x02' + bytes([len(name) + 1])
ble_name = bytearray((len(ble_name) + 1, 0x09)) + ble_name payload += b'\x09' + name.encode()
ble.gap_advertise(100, bytearray('\x02\x01\x02') + ble_name) ble.gap_advertise(100, payload)
def update_battery_level(timer): def update_battery_level(timer):
if conn_handle is not None: if conn_handle is not None:
battery_level = sumorobot.get_battery_level() battery_level = sumorobot.get_battery_level()
ble.gatts_notify(conn_handle, battery, bytes([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 global prev_bat_level, python_code
while True: while True:
# Leave time to process other code # Leave time to process other threads
sleep_ms(50) utime.sleep_ms(50)
# Execute to see LED feedback for sensors
sumorobot.update_sensor_feedback()
# When no code to execute # When no code to execute
if python_code == b'': if python_code == b'':
@ -47,12 +49,11 @@ def process():
# Try to execute the Python code # Try to execute the Python code
try: try:
python_code = compile(python_code, "snippet", 'exec') exec(compile(python_code, "snippet", 'exec'))
exec(python_code) except Exception as error:
except: print("main.py: the python code had errors:", error)
print("main.py: the code sent had errors")
finally: finally:
print("main.py: finized execution") print("main.py: finized python code execution")
# Erase the code # Erase the code
python_code = b'' python_code = b''
# Stop the robot # Stop the robot
@ -60,6 +61,7 @@ def process():
# Cancel code termination # Cancel code termination
sumorobot.terminate = False sumorobot.terminate = False
# The BLE handler thread # The BLE handler thread
def ble_handler(event, data): def ble_handler(event, data):
global conn_handle, python_code, temp_python_code global conn_handle, python_code, temp_python_code
@ -69,15 +71,17 @@ def ble_handler(event, data):
# Turn ON the status LED # Turn ON the status LED
sumorobot.set_led(STATUS, True) sumorobot.set_led(STATUS, True)
update_battery_level(None) update_battery_level(None)
advertise_ble_name(sumorobot.config['sumorobot_name'])
elif event is _IRQ_CENTRAL_DISCONNECT: elif event is _IRQ_CENTRAL_DISCONNECT:
conn_handle = None conn_handle = None
# Turn OFF status LED # Turn OFF status LED
sumorobot.set_led(STATUS, False) sumorobot.set_led(STATUS, False)
# Advertise with name # Advertise with name
advertise_ble_name(sumorobot.config['sumorobot_name']) advertise_ble_name(sumorobot.config['sumorobot_name'])
elif event is _IRQ_GATTS_READ_REQUEST: elif event is _IRQ_GATTS_WRITE:
# Read the command # Read the command
cmd = ble.gatts_read(rx) cmd = ble.gatts_read(rx)
print(cmd)
if b'<stop>' in cmd: if b'<stop>' in cmd:
python_code = b'' python_code = b''
@ -96,8 +100,13 @@ def ble_handler(event, data):
python_code = b'' python_code = b''
sumorobot.move(RIGHT) sumorobot.move(RIGHT)
elif b'<sensors>' in cmd: elif b'<sensors>' in cmd:
print(sumorobot.get_sensor_scope())
ble.gatts_notify(conn_handle, tx, sumorobot.get_sensor_scope()) ble.gatts_notify(conn_handle, tx, sumorobot.get_sensor_scope())
elif b'<config>' in cmd:
ble.gatts_notify(conn_handle, tx, sumorobot.get_configuration_scope())
elif b'<pwm>' in cmd:
servo, speed = cmd[5:].decode().split(',')
servo = LEFT if servo == 'LEFT' else RIGHT
sumorobot.pwm[servo].duty(int(speed))
elif b'<code>' in cmd: elif b'<code>' in cmd:
temp_python_code = b'\n' temp_python_code = b'\n'
elif b'<code/>' in cmd: elif b'<code/>' in cmd:
@ -107,24 +116,28 @@ def ble_handler(event, data):
temp_python_code += cmd temp_python_code += cmd
else: else:
temp_python_code = b'' temp_python_code = b''
print("main.py: unknown cmd=" + cmd) print("main.py: unknown cmd=", cmd)
conn_handle = None conn_handle = None
temp_python_code = b'' temp_python_code = b''
python_code = b'' python_code = b''
# When user code (code.py) exists # When boot code exists
if 'code.py' in root_files: if sumorobot.config['boot_code'] in root_files:
print("main.py: trying to load code.py") print("main.py: trying to load", sumorobot.config['boot_code'])
# Try to load the user code # Try to load and compile the boot code
try: try:
with open('code.py', 'r') as code: with open(sumorobot.config['boot_code'], 'r') as file:
python_code = compile(code.read(), "snippet", 'exec') boot_code = file.read()
except: compile(boot_code, "snippet", 'exec')
print("main.py: code.py compilation failed") python_code = boot_code
except Exception as error:
print("main.py:", sumorobot.config['boot_code'], "compilation failed:", error)
# Start BLE # Start BLE
ble = ubluetooth.BLE() ble = ubluetooth.BLE()
ble.config(gap_name=sumorobot.config['sumorobot_name'])
ble.active(True) ble.active(True)
# Register the BLE hander # Register the BLE hander
@ -160,11 +173,12 @@ ble.gatts_write(firmware, sumorobot.config['firmware_version'])
# Start BLE advertising with name # Start BLE advertising with name
advertise_ble_name(sumorobot.config['sumorobot_name']) advertise_ble_name(sumorobot.config['sumorobot_name'])
# Start the code processing thread # Start the threads
_thread.start_new_thread(process, ()) _thread.start_new_thread(code_process_thread, ())
_thread.start_new_thread(sensor_feedback_thread, ())
# Start BLE battery percentage update timer # 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) battery_timer.init(period=3000, callback=update_battery_level)
# Clean up # Clean up