Imporove servo PWM calibration
This commit is contained in:
silbo
parent
fc11ffdad4
commit
00857da470
2
.gitignore
vendored
2
.gitignore
vendored
@ -2,7 +2,7 @@
|
||||
code.py
|
||||
# ignore backup files
|
||||
._*
|
||||
* Mac OS stuff
|
||||
# Mac OS stuff
|
||||
.DS_Store
|
||||
# ignore the ESP32 MicroPython binary
|
||||
esp32*.bin
|
||||
|
||||
2
LICENSE
2
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
|
||||
|
||||
31
Makefile
31
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)
|
||||
|
||||
|
||||
11
boot.py
11
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
|
||||
|
||||
|
||||
10
config.json
10
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,
|
||||
|
||||
96
main.py
96
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'<stop>' in cmd:
|
||||
python_code = b''
|
||||
@ -96,8 +100,13 @@ def ble_handler(event, data):
|
||||
python_code = b''
|
||||
sumorobot.move(RIGHT)
|
||||
elif b'<sensors>' in cmd:
|
||||
print(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:
|
||||
temp_python_code = b'\n'
|
||||
elif b'<code/>' 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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user