Change to MicroPython BLE
This commit is contained in:
silbo
parent
7148a88818
commit
a18d65dd3b
24
Makefile
24
Makefile
|
|
@ -1,12 +1,28 @@
|
|||
#!/bin/bash
|
||||
|
||||
#SERIAL_PORT=/dev/ttyUSB0
|
||||
SERIAL_PORT=/dev/tty.usbserial-1410
|
||||
SERIAL_PORT=/dev/tty.usbserial-1420
|
||||
#SERIAL_PORT=/dev/tty.SLAB_USBtoUART
|
||||
#SERIAL_PORT=/dev/tty.wchusbserial1410
|
||||
|
||||
all: flash
|
||||
all: flash delay config update reset
|
||||
|
||||
delay:
|
||||
sleep 3
|
||||
|
||||
reset:
|
||||
esptool.py -p $(SERIAL_PORT) --after hard_reset read_mac
|
||||
|
||||
update:
|
||||
ampy -d 0.5 -p $(SERIAL_PORT) put hal.py
|
||||
ampy -d 0.5 -p $(SERIAL_PORT) put main.py
|
||||
ampy -d 0.5 -p $(SERIAL_PORT) put boot.py
|
||||
|
||||
config:
|
||||
ampy -d 0.5 -p $(SERIAL_PORT) put config.json
|
||||
|
||||
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 460800 write_flash --flash_mode dio 0x1000 esp32*.bin
|
||||
|
||||
serial:
|
||||
picocom --baud 115200 $(SERIAL_PORT)
|
||||
|
|
|
|||
10
README.md
10
README.md
|
|
@ -2,12 +2,16 @@
|
|||
|
||||
The software that is running on the SumoRobots
|
||||
|
||||
<img alt="Code" src="https://www.robokoding.com/assets/img/sumorobot_firmware.png" width="50%">
|
||||
|
||||
# Instructions
|
||||
* Change the SERIAL_PORT in the Makefile
|
||||
* Add your WiFi networks to the config.json file
|
||||
* Install [Python](https://www.python.org/downloads/)
|
||||
* Install [esptool](https://github.com/espressif/esptool) (to flash SumoFirmware to the SumoRobot)
|
||||
* Download the [SumoFirmware](https://github.com/robokoding/sumorobot-firmware/releases) to this directory
|
||||
* Upload the SumoFirmware to your SumoRobot (open a terminal and type: make all)
|
||||
* Install [esptool](https://github.com/espressif/esptool) (to flash MicroPython to the ESP32)
|
||||
* Install [ampy](https://github.com/adafruit/ampy) (for uploading files)
|
||||
* Download [the MicroPython binary](http://micropython.org/download#esp32) to this directory
|
||||
* Upload the MicroPython binary and the SumoRobot firmware to your ESP32 (open a terminal and type: make all)
|
||||
|
||||
# Support
|
||||
If you find our work useful, please consider donating : )
|
||||
|
|
|
|||
|
|
@ -0,0 +1,5 @@
|
|||
from utime import sleep_ms
|
||||
|
||||
# Give time to cancel boot script
|
||||
print("Press Ctrl-C to stop boot script...")
|
||||
sleep_ms(500)
|
||||
|
|
@ -0,0 +1,17 @@
|
|||
{
|
||||
"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,
|
||||
"sonar_threshold": 40,
|
||||
"boot_code": "code.py",
|
||||
"left_line_value": 1000,
|
||||
"right_line_value": 1000,
|
||||
"left_line_threshold": 1000,
|
||||
"right_line_threshold": 1000
|
||||
}
|
||||
|
|
@ -0,0 +1,282 @@
|
|||
from utime import sleep_us, sleep_ms
|
||||
from machine import Pin, PWM, ADC, time_pulse_us
|
||||
|
||||
# LEDs
|
||||
STATUS = 0
|
||||
SONAR = 1
|
||||
LEFT_LINE = 2
|
||||
RIGHT_LINE = 3
|
||||
|
||||
# Directions
|
||||
STOP = 0
|
||||
LEFT = 1
|
||||
RIGHT = 2
|
||||
SEARCH = 3
|
||||
FORWARD = 4
|
||||
BACKWARD = 5
|
||||
|
||||
class Sumorobot(object):
|
||||
# Constructor
|
||||
def __init__(self, config = None):
|
||||
# Config file
|
||||
self.config = config
|
||||
|
||||
# Sonar distance sensor
|
||||
self.echo = Pin(14, Pin.IN)
|
||||
self.trigger = Pin(27, Pin.OUT)
|
||||
|
||||
# Servo PWM-s
|
||||
self.pwm = {
|
||||
LEFT: PWM(Pin(15), freq=50, duty=0),
|
||||
RIGHT: PWM(Pin(4), freq=50, duty=0)
|
||||
}
|
||||
|
||||
# LED sensor feedback
|
||||
self.sensor_feedback = True
|
||||
# Bottom status LED
|
||||
self.status_led = Pin(self.config["status_led_pin"], Pin.OUT)
|
||||
# Bottom status LED is in reverse polarity
|
||||
self.status_led.value(1)
|
||||
# Sensor LEDs
|
||||
self.sonar_led = Pin(16, Pin.OUT)
|
||||
self.left_line_led = Pin(17, Pin.OUT)
|
||||
self.right_line_led = Pin(12, Pin.OUT)
|
||||
|
||||
# Battery level in %
|
||||
self.battery_level = 0
|
||||
|
||||
# Battery gauge
|
||||
self.bat_status = 4.3
|
||||
self.move_counter = 0
|
||||
self.adc_battery = ADC(Pin(32))
|
||||
self.bat_charge = Pin(25, Pin.IN)
|
||||
|
||||
# The pullups for the phototransistors
|
||||
Pin(19, Pin.IN, Pin.PULL_UP)
|
||||
Pin(23, Pin.IN, Pin.PULL_UP)
|
||||
|
||||
# The phototransistors
|
||||
self.last_line = LEFT
|
||||
self.adc_line_left = ADC(Pin(34))
|
||||
self.adc_line_right = ADC(Pin(33))
|
||||
|
||||
# Set reference voltage to 3.3V
|
||||
self.adc_battery.atten(ADC.ATTN_11DB)
|
||||
self.adc_line_left.atten(ADC.ATTN_11DB)
|
||||
self.adc_line_right.atten(ADC.ATTN_11DB)
|
||||
|
||||
# To smooth out sonar sensor value
|
||||
self.sonar_score = 0
|
||||
|
||||
# For terminating sleep
|
||||
self.terminate = False
|
||||
|
||||
# For search mode
|
||||
self.search = False
|
||||
self.search_counter = 0
|
||||
|
||||
# Memorise previous servo speeds
|
||||
self.prev_speed = {LEFT: 0, RIGHT: 0}
|
||||
|
||||
# Function to set LED states
|
||||
def set_led(self, led, value):
|
||||
# Turn the given LED on or off
|
||||
if led == STATUS:
|
||||
# Status LED is reverse polarity
|
||||
self.status_led.value(0 if value else 1)
|
||||
elif led == SONAR:
|
||||
self.sonar_led.value(value)
|
||||
elif led == LEFT_LINE:
|
||||
self.left_line_led.value(value)
|
||||
elif led == RIGHT_LINE:
|
||||
self.right_line_led.value(value)
|
||||
|
||||
# Function to get battery level in percentage
|
||||
def get_battery_level(self):
|
||||
# When the SumoRobot is not moving
|
||||
if self.prev_speed[LEFT] == 0 and self.prev_speed[RIGHT] == 0:
|
||||
# Calculate battery voltage
|
||||
battery_voltage = round(self.config["battery_coeff"] * (self.adc_battery.read() * 3.3 / 4096), 2)
|
||||
# Map battery voltage to percentage
|
||||
temp_battery_level = 0.0 + ((100.0 - 0.0) / (4.2 - 3.2)) * (battery_voltage - 3.2)
|
||||
# When battery level changed more than 5 percent
|
||||
if abs(self.battery_level - temp_battery_level) > 5:
|
||||
# Update battery level
|
||||
self.battery_level = round(temp_battery_level)
|
||||
# Return the battery level in percentage
|
||||
return min(100, max(0, self.battery_level))
|
||||
|
||||
# Function to get distance (cm) from the object in front of the SumoRobot
|
||||
def get_sonar_value(self):
|
||||
# Send a pulse
|
||||
self.trigger.value(0)
|
||||
sleep_us(5)
|
||||
self.trigger.value(1)
|
||||
sleep_us(10)
|
||||
self.trigger.value(0)
|
||||
# Wait for the pulse and calculate the distance
|
||||
return round((time_pulse_us(self.echo, 1, 30000) / 2) / 29.1)
|
||||
|
||||
# Function to get boolean if there is something in front of the SumoRobot
|
||||
def is_sonar(self):
|
||||
# Get the sonar value
|
||||
self.sonar_value = self.get_sonar_value()
|
||||
# When the sonar value is small and the ping actually returned
|
||||
if self.sonar_value < self.config["sonar_threshold"] and self.sonar_value > 0:
|
||||
# When not maximum score
|
||||
if self.sonar_score < 5:
|
||||
# Increase the sonar score
|
||||
self.sonar_score += 1
|
||||
# When no sonar ping was returned
|
||||
else:
|
||||
# When not lowest score
|
||||
if self.sonar_score > 0:
|
||||
# Decrease the sonar score
|
||||
self.sonar_score -= 1
|
||||
|
||||
# When the sensor saw something more than 2 times
|
||||
value = True if self.sonar_score > 2 else False
|
||||
|
||||
# Trigger sonar LED
|
||||
self.set_led(SONAR, value)
|
||||
|
||||
return value
|
||||
|
||||
# Function to update the config file
|
||||
def update_config_file(self):
|
||||
# Update the config file
|
||||
with open("config.part", "w") as config_file:
|
||||
config_file.write(ujson.dumps(self.config))
|
||||
os.rename("config.part", "config.json")
|
||||
|
||||
# Function to update line calibration and write it to the config file
|
||||
def calibrate_line_values(self):
|
||||
# Read the line sensor values
|
||||
self.config["left_line_value"] = self.adc_line_left.read()
|
||||
self.config["right_line_value"] = self.adc_line_right.read()
|
||||
|
||||
# Function to get light inensity from the phototransistors
|
||||
def get_line(self, line):
|
||||
# Check if the direction is valid
|
||||
assert line in (LEFT, RIGHT)
|
||||
|
||||
# Return the given line sensor value
|
||||
if line == LEFT:
|
||||
return self.adc_line_left.read()
|
||||
elif line == RIGHT:
|
||||
return self.adc_line_right.read()
|
||||
|
||||
def is_line(self, line):
|
||||
# Check if the direction is valid
|
||||
assert line in (LEFT, RIGHT)
|
||||
|
||||
# Define feedback LED
|
||||
led = LEFT_LINE if line == LEFT else RIGHT_LINE
|
||||
# Define config prefix
|
||||
prefix = "left" if line == LEFT else "right"
|
||||
# Check for line
|
||||
value = abs(self.get_line(line) - self.config[prefix + "_line_value"]) > self.config[prefix + "_line_threshold"]
|
||||
# Show LED feedback
|
||||
self.set_led(led, value)
|
||||
# Update last line direction if line was detected
|
||||
self.last_line = value if value else self.last_line
|
||||
# Return the given line sensor value
|
||||
return value
|
||||
|
||||
def set_servo(self, servo, speed):
|
||||
# Check if the direction is valid
|
||||
assert servo in (LEFT, RIGHT)
|
||||
# Check if the speed is valid
|
||||
assert speed <= 100 and speed >= -100
|
||||
|
||||
# When the speed didn't change
|
||||
if speed == self.prev_speed[servo]:
|
||||
return
|
||||
|
||||
# Save the new speed
|
||||
self.prev_speed[servo] = speed
|
||||
|
||||
# Set the given servo speed
|
||||
if speed == 0:
|
||||
self.pwm[servo].duty(0)
|
||||
else:
|
||||
# Define config prefix
|
||||
prefix = "left" if servo == LEFT else "right"
|
||||
# -100 ... 100 to min_tuning .. max_tuning
|
||||
min_tuning = self.config[prefix + "_servo_min_tuning"]
|
||||
max_tuning = self.config[prefix + "_servo_max_tuning"]
|
||||
self.pwm[servo].duty(int((speed + 100) / 200 * (max_tuning - min_tuning) + min_tuning))
|
||||
|
||||
def move(self, dir):
|
||||
# Check if the direction is valid
|
||||
assert dir in (SEARCH, STOP, RIGHT, LEFT, BACKWARD, FORWARD)
|
||||
|
||||
# Go to the given direction
|
||||
if dir == STOP:
|
||||
self.set_servo(LEFT, 0)
|
||||
self.set_servo(RIGHT, 0)
|
||||
elif dir == LEFT:
|
||||
self.set_servo(LEFT, -100)
|
||||
self.set_servo(RIGHT, -100)
|
||||
elif dir == RIGHT:
|
||||
self.set_servo(LEFT, 100)
|
||||
self.set_servo(RIGHT, 100)
|
||||
elif dir == SEARCH:
|
||||
# Change search mode after X seconds
|
||||
if self.search_counter == 50:
|
||||
self.search = not self.search
|
||||
self.search_counter = 0
|
||||
# When in search mode
|
||||
if self.search:
|
||||
self.move(FORWARD)
|
||||
elif self.last_line == RIGHT:
|
||||
self.move(LEFT)
|
||||
else:
|
||||
self.move(RIGHT)
|
||||
# Increase search counter
|
||||
self.search_counter += 1
|
||||
elif dir == FORWARD:
|
||||
self.set_servo(LEFT, 100)
|
||||
self.set_servo(RIGHT, -100)
|
||||
elif dir == BACKWARD:
|
||||
self.set_servo(LEFT, -100)
|
||||
self.set_servo(RIGHT, 100)
|
||||
|
||||
def update_sensor_feedback(self):
|
||||
if self.sensor_feedback:
|
||||
# Execute to see LED feedback for sensors
|
||||
self.is_sonar()
|
||||
self.is_line(LEFT)
|
||||
self.is_line(RIGHT)
|
||||
|
||||
def get_sensor_scope(self):
|
||||
# TODO: implement sensor value caching
|
||||
return str(self.get_sonar_value()) + ',' + \
|
||||
str(self.get_line(LEFT)) + ',' + \
|
||||
str(self.get_line(RIGHT)) + ',' + \
|
||||
str(self.bat_charge.value()) + ',' + \
|
||||
str(self.get_battery_level())
|
||||
|
||||
def get_configuration_scope(self):
|
||||
return str(self.config["sumorobot_name"]) + ',' + \
|
||||
str(self.config["firmware_version"]) + ',' + \
|
||||
str(self.config["left_line_value"]) + ',' + \
|
||||
str(self.config["right_line_value"]) + ',' + \
|
||||
str(self.config["left_line_threshold"]) + ',' + \
|
||||
str(self.config["right_line_threshold"]) + ',' + \
|
||||
str(self.config["sonar_threshold"])
|
||||
|
||||
def sleep(self, delay):
|
||||
# Check for valid delay
|
||||
assert delay > 0
|
||||
|
||||
# Split the delay into 50ms chunks
|
||||
while delay:
|
||||
# Check for forceful termination
|
||||
if self.terminate:
|
||||
# Terminate the delay
|
||||
return
|
||||
else:
|
||||
sleep_ms(50)
|
||||
|
||||
delay -= 50
|
||||
|
|
@ -0,0 +1,173 @@
|
|||
import os
|
||||
import ujson
|
||||
import struct
|
||||
import _thread
|
||||
import ubluetooth
|
||||
from machine import Timer
|
||||
from micropython import const
|
||||
|
||||
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)
|
||||
|
||||
# Open and parse the config file
|
||||
with open("config.json", "r") as config_file:
|
||||
config = ujson.load(config_file)
|
||||
|
||||
# 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)
|
||||
|
||||
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():
|
||||
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()
|
||||
|
||||
# When no code to execute
|
||||
if python_code == b'':
|
||||
continue
|
||||
|
||||
# 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")
|
||||
finally:
|
||||
print("main.py: finized execution")
|
||||
# Erase the code
|
||||
python_code = b''
|
||||
# Stop the robot
|
||||
sumorobot.move(STOP)
|
||||
# Cancel code termination
|
||||
sumorobot.terminate = False
|
||||
|
||||
# The BLE handler thread
|
||||
def ble_handler(event, data):
|
||||
global conn_handle, python_code, temp_python_code
|
||||
|
||||
if event is _IRQ_CENTRAL_CONNECT:
|
||||
conn_handle, _, _, = data
|
||||
# Turn ON the status LED
|
||||
sumorobot.set_led(STATUS, True)
|
||||
update_battery_level(None)
|
||||
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:
|
||||
# Read the command
|
||||
cmd = ble.gatts_read(rx)
|
||||
|
||||
if b'<stop>' in cmd:
|
||||
python_code = b''
|
||||
sumorobot.move(STOP)
|
||||
sumorobot.terminate = True
|
||||
elif b'<forward>' in cmd:
|
||||
python_code = b''
|
||||
sumorobot.move(FORWARD)
|
||||
elif b'<backward>' in cmd:
|
||||
python_code = b''
|
||||
sumorobot.move(BACKWARD)
|
||||
elif b'<left>' in cmd:
|
||||
python_code = b''
|
||||
sumorobot.move(LEFT)
|
||||
elif b'<right>' in cmd:
|
||||
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'<code>' in cmd:
|
||||
temp_python_code = b'\n'
|
||||
elif b'<code/>' in cmd:
|
||||
python_code = temp_python_code
|
||||
temp_python_code = b''
|
||||
elif temp_python_code != b'':
|
||||
temp_python_code += cmd
|
||||
else:
|
||||
temp_python_code = b''
|
||||
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 os.listdir():
|
||||
print('main.py: trying to load code.py')
|
||||
# Try to load the user code
|
||||
try:
|
||||
with open("code.py", "r") as code:
|
||||
python_code = compile(code.read(), "snippet", "exec")
|
||||
except:
|
||||
print("main.py: code.py compilation failed")
|
||||
|
||||
# Start BLE
|
||||
ble = ubluetooth.BLE()
|
||||
ble.active(True)
|
||||
|
||||
# Register the BLE hander
|
||||
ble.irq(ble_handler)
|
||||
|
||||
# BLE info serivce
|
||||
INFO_SERVICE_UUID = ubluetooth.UUID(0x180a)
|
||||
MODEL_CHARACTERISTIC = (ubluetooth.UUID(0x2a24), ubluetooth.FLAG_READ,)
|
||||
FIRMWARE_CHARACTERISTIC = (ubluetooth.UUID(0x2a26), ubluetooth.FLAG_READ,)
|
||||
MANUFACTURER_CHARACTERISTIC = (ubluetooth.UUID(0x2a29), ubluetooth.FLAG_READ,)
|
||||
INFO_SERVICE = (INFO_SERVICE_UUID, (MODEL_CHARACTERISTIC, FIRMWARE_CHARACTERISTIC, MANUFACTURER_CHARACTERISTIC,),)
|
||||
|
||||
# BLE battery service
|
||||
BATTERY_SERVICE_UUID = ubluetooth.UUID(0x180f)
|
||||
BATTERY_CHARACTERISTIC = (ubluetooth.UUID(0x2a19), ubluetooth.FLAG_READ | ubluetooth.FLAG_NOTIFY,)
|
||||
BATTERY_SERVICE = (BATTERY_SERVICE_UUID, (BATTERY_CHARACTERISTIC,),)
|
||||
|
||||
# BLE UART service
|
||||
UART_SERVICE_UUID = ubluetooth.UUID('6E400001-B5A3-F393-E0A9-E50E24DCCA9E')
|
||||
RX_CHARACTERISTIC = (ubluetooth.UUID('6E400002-B5A3-F393-E0A9-E50E24DCCA9E'), ubluetooth.FLAG_WRITE,)
|
||||
TX_CHARACTERISTIC = (ubluetooth.UUID('6E400003-B5A3-F393-E0A9-E50E24DCCA9E'), ubluetooth.FLAG_READ | ubluetooth.FLAG_NOTIFY,)
|
||||
UART_SERVICE = (UART_SERVICE_UUID, (TX_CHARACTERISTIC, RX_CHARACTERISTIC,),)
|
||||
|
||||
# Register BLE services
|
||||
SERVICES = (INFO_SERVICE, BATTERY_SERVICE, UART_SERVICE,)
|
||||
((model, firmware, manufacturer,), (battery,), (tx, rx,),) = ble.gatts_register_services(SERVICES)
|
||||
|
||||
# Set BLE info service values
|
||||
ble.gatts_write(model, "SumoRobot")
|
||||
ble.gatts_write(manufacturer, "RoboKoding LTD")
|
||||
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 BLE battery percentage update timer
|
||||
battery_timer = Timer(Timer.PERIODIC)
|
||||
battery_timer.init(period=3000, callback=update_battery_level)
|
||||
|
||||
# Clean up
|
||||
import gc
|
||||
gc.collect()
|
||||
|
|
@ -1,17 +0,0 @@
|
|||
;PlatformIO Project Configuration File
|
||||
;
|
||||
; Build options: build flags, source filter
|
||||
; Upload options: custom upload port, speed and extra flags
|
||||
; Library options: dependencies, extra library storages
|
||||
; Advanced options: extra scripting
|
||||
;
|
||||
; Please visit documentation for the other options and examples
|
||||
; https://docs.platformio.org/page/projectconf.html
|
||||
|
||||
[env:lolin32]
|
||||
platform = espressif32
|
||||
board = lolin32
|
||||
framework = arduino
|
||||
upload_speed = 460800
|
||||
monitor_speed = 115200
|
||||
lib_deps = NewPing
|
||||
396
src/main.cpp
396
src/main.cpp
|
|
@ -1,396 +0,0 @@
|
|||
/*
|
||||
This the code that runs on the SumoRobots
|
||||
*/
|
||||
// Include BLE libraries
|
||||
#include <BLEDevice.h>
|
||||
#include <BLEServer.h>
|
||||
#include <BLEUtils.h>
|
||||
#include <BLE2902.h>
|
||||
#include <BLE2904.h>
|
||||
// Include other libraries
|
||||
#include <string.h>
|
||||
#include <Ticker.h>
|
||||
#include <NewPing.h>
|
||||
#include <Arduino.h>
|
||||
#include <Preferences.h>
|
||||
|
||||
#define DEBUG true
|
||||
|
||||
#define VERSION "0.8.0"
|
||||
#define VERSION_TIMESTAMP "2019.08.13 08:00"
|
||||
|
||||
// See the following for generating UUIDs:
|
||||
// https://www.uuidgenerator.net/
|
||||
#define NUS_SERVICE_UUID "6E400001-B5A3-F393-E0A9-E50E24DCCA9E" // NUS service UUID
|
||||
#define NUS_CHARACTERISTIC_RX_UUID "6E400002-B5A3-F393-E0A9-E50E24DCCA9E"
|
||||
#define NUS_CHARACTERISTIC_TX_UUID "6E400003-B5A3-F393-E0A9-E50E24DCCA9E"
|
||||
|
||||
// Cleate BLE variables
|
||||
BLEServer * bleServer = NULL;
|
||||
bool deviceConnected = false;
|
||||
bool oldDeviceConnected = false;
|
||||
BLECharacteristic * nusTxCharacteristic;
|
||||
BLECharacteristic * batteryLevelCharacteristic;
|
||||
|
||||
// Create preferences persistence
|
||||
Preferences preferences;
|
||||
|
||||
// Create timers
|
||||
Ticker sonarTimer;
|
||||
Ticker batteryTimer;
|
||||
Ticker connectionLedTimer;
|
||||
|
||||
// Battery stuff
|
||||
float batteryVoltage;
|
||||
bool robotMoving = false;
|
||||
uint8_t batteryLevel = 0;
|
||||
uint8_t tempBatteryLevel = 0;
|
||||
|
||||
// Sonar stuff
|
||||
uint8_t sonarValue;
|
||||
NewPing sonar(27, 14, 200);
|
||||
uint8_t sonarThreshold = 40;
|
||||
|
||||
// Line stuff
|
||||
uint16_t leftLineValue;
|
||||
uint16_t rightLineValue;
|
||||
uint16_t leftLineValueField = 0;
|
||||
uint16_t rightLineValueField = 0;
|
||||
uint16_t leftLineThreshold = 1000;
|
||||
uint16_t rightLineThreshold = 1000;
|
||||
|
||||
// Other sensor stuff
|
||||
uint8_t sensorValues[6];
|
||||
bool ledFeedbackEnabled = true;
|
||||
|
||||
// Move command names
|
||||
std::string cmdStop("stop");
|
||||
std::string cmdLeft("left");
|
||||
std::string cmdRight("right");
|
||||
std::string cmdForward("forward");
|
||||
std::string cmdBackward("backward");
|
||||
// Other command names
|
||||
std::string cmdLed("led");
|
||||
std::string cmdLine("line");
|
||||
std::string cmdName("name");
|
||||
std::string cmdSonar("sonar");
|
||||
std::string cmdServo("servo");
|
||||
std::string cmdLedFeedback("ledf");
|
||||
|
||||
void setLed(char led, char value) {
|
||||
// Convert the value to a HIGH or LOW
|
||||
bool state = value == '1' ? HIGH : LOW;
|
||||
if (led == 'c') {
|
||||
// Connection status LED is opposite value
|
||||
digitalWrite(5, !state);
|
||||
}
|
||||
else if (led == 's') {
|
||||
digitalWrite(16, state);
|
||||
}
|
||||
else if (led == 'r') {
|
||||
digitalWrite(12, state);
|
||||
}
|
||||
else if (led == 'l') {
|
||||
digitalWrite(17, state);
|
||||
}
|
||||
}
|
||||
|
||||
void setServo(char servo, int8_t speed) {
|
||||
Serial.println(speed);
|
||||
if (servo == 'l') {
|
||||
ledcWrite(1, map(speed, -100, 100, 1, 100));
|
||||
}
|
||||
else if (servo == 'r') {
|
||||
ledcWrite(2, map(speed, -100, 100, 1, 30));
|
||||
}
|
||||
}
|
||||
|
||||
void updateSensorFeedback() {
|
||||
if (sonarValue <= sonarThreshold) {
|
||||
digitalWrite(16, HIGH);
|
||||
}
|
||||
else {
|
||||
digitalWrite(16, LOW);
|
||||
}
|
||||
if (abs(leftLineValue - leftLineValueField) > leftLineThreshold) {
|
||||
digitalWrite(17, HIGH);
|
||||
}
|
||||
else {
|
||||
digitalWrite(17, LOW);
|
||||
}
|
||||
if (abs(rightLineValue - rightLineValueField) > rightLineThreshold) {
|
||||
digitalWrite(12, HIGH);
|
||||
}
|
||||
else {
|
||||
digitalWrite(12, LOW);
|
||||
}
|
||||
}
|
||||
|
||||
void updateSonarValue() {
|
||||
// Update the sensor values
|
||||
sonarValue = sonar.ping_cm();
|
||||
// When we didn't receive a ping back
|
||||
// set to max distance
|
||||
if (sonarValue == 0) sonarValue = 255;
|
||||
leftLineValue = analogRead(34);
|
||||
rightLineValue = analogRead(33);
|
||||
if (ledFeedbackEnabled) updateSensorFeedback();
|
||||
sensorValues[0] = sonarValue;
|
||||
sensorValues[1] = leftLineValue >> 8;
|
||||
sensorValues[2] = leftLineValue;
|
||||
sensorValues[3] = rightLineValue >> 8;
|
||||
sensorValues[4] = rightLineValue;
|
||||
sensorValues[5] = digitalRead(25);
|
||||
// When BLE is connected
|
||||
if (deviceConnected) {
|
||||
// Notify the new sensor values
|
||||
nusTxCharacteristic->setValue(sensorValues, 6);
|
||||
nusTxCharacteristic->notify();
|
||||
}
|
||||
}
|
||||
|
||||
void updateBatteryLevel() {
|
||||
// Don't update battery level when robot is moving
|
||||
// the servo motors lower the battery voltage
|
||||
// TODO: wait still a little more after moving
|
||||
// for the voltage to settle
|
||||
if (robotMoving) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Calculate the battery voltage
|
||||
batteryVoltage = 2.12 * (analogRead(32) * 3.3 / 4096);
|
||||
// Calculate battery percentage
|
||||
tempBatteryLevel = 0.0 + ((100.0 - 0.0) / (4.2 - 3.2)) * (batteryVoltage - 3.2);
|
||||
// When battery level changed more than 3%
|
||||
if (abs(batteryLevel - tempBatteryLevel) > 3) {
|
||||
// Update battery level
|
||||
batteryLevel = tempBatteryLevel;
|
||||
}
|
||||
// Notify the new battery level
|
||||
batteryLevelCharacteristic->setValue(&batteryLevel, 1);
|
||||
batteryLevelCharacteristic->notify();
|
||||
#if DEBUG
|
||||
Serial.print(batteryVoltage);
|
||||
Serial.print(" : ");
|
||||
Serial.println(batteryLevel);
|
||||
#endif
|
||||
}
|
||||
|
||||
void blinkConnectionLed() {
|
||||
digitalWrite(5, LOW);
|
||||
delay(20);
|
||||
digitalWrite(5, HIGH);
|
||||
}
|
||||
|
||||
// BLE connect and disconnect callbacks
|
||||
class MyServerCallbacks: public BLEServerCallbacks {
|
||||
void onConnect(BLEServer * pServer) {
|
||||
deviceConnected = true;
|
||||
};
|
||||
|
||||
void onDisconnect(BLEServer * pServer) {
|
||||
deviceConnected = false;
|
||||
}
|
||||
};
|
||||
|
||||
// BLE NUS received callback
|
||||
class MyCallbacks: public BLECharacteristicCallbacks {
|
||||
void onWrite(BLECharacteristic * nusRxCharacteristic) {
|
||||
// Get the received command over BLE
|
||||
std::string cmd = nusRxCharacteristic->getValue();
|
||||
#if DEBUG
|
||||
Serial.println(cmd.c_str());
|
||||
#endif
|
||||
if (cmd.length() > 0) {
|
||||
//int speed = atoi(rxValue.c_str());
|
||||
//Serial.println(speed);
|
||||
//ledcWrite(1, speed); // left 1 ... 100
|
||||
//ledcWrite(2, speed); // right 1 ... 30
|
||||
// Specify command
|
||||
if (cmd == cmdForward) {
|
||||
robotMoving = true;
|
||||
ledcWrite(1, 100);
|
||||
ledcWrite(2, 1);
|
||||
}
|
||||
else if (cmd == cmdBackward) {
|
||||
robotMoving = true;
|
||||
ledcWrite(1, 1);
|
||||
ledcWrite(2, 30);
|
||||
}
|
||||
else if (cmd == cmdLeft) {
|
||||
robotMoving = true;
|
||||
ledcWrite(1, 1);
|
||||
ledcWrite(2, 1);
|
||||
}
|
||||
else if (cmd == cmdRight) {
|
||||
robotMoving = true;
|
||||
ledcWrite(1, 100);
|
||||
ledcWrite(2, 30);
|
||||
}
|
||||
else if (cmd == cmdStop) {
|
||||
robotMoving = false;
|
||||
ledcWrite(1, 0);
|
||||
ledcWrite(2, 0);
|
||||
}
|
||||
else if (cmd == cmdLedFeedback) {
|
||||
ledFeedbackEnabled = !ledFeedbackEnabled;
|
||||
}
|
||||
else if (cmd.find(cmdLed) != std::string::npos) {
|
||||
setLed(cmd.at(3), cmd.at(4));
|
||||
}
|
||||
else if (cmd.find(cmdLine) != std::string::npos) {
|
||||
// Get the threshold value
|
||||
leftLineThreshold = atoi(cmd.substr(4, cmd.length() - 4).c_str());
|
||||
rightLineThreshold = leftLineThreshold;
|
||||
// Remember value on the field (white or black)
|
||||
leftLineValueField = analogRead(34);
|
||||
rightLineValueField = analogRead(33);
|
||||
// Save the threshold value in the persistence
|
||||
preferences.begin("sumorobot", false);
|
||||
preferences.putUInt("line_threshold", leftLineThreshold);
|
||||
preferences.end();
|
||||
}
|
||||
else if (cmd.find(cmdSonar) != std::string::npos) {
|
||||
sonarThreshold = atoi(cmd.substr(5, cmd.length() - 5).c_str());
|
||||
// Save the threshold value in the persistence
|
||||
preferences.begin("sumorobot", false);
|
||||
preferences.putUInt("sonar_threshold", sonarThreshold);
|
||||
preferences.end();
|
||||
}
|
||||
else if (cmd.find(cmdServo) != std::string::npos) {
|
||||
setServo(cmd.at(5), atoi(cmd.substr(6, cmd.length() - 6).c_str()));
|
||||
}
|
||||
else if (cmd.find(cmdName) != std::string::npos) {
|
||||
preferences.begin("sumorobot", false);
|
||||
preferences.putString("name", cmd.substr(4, cmd.length() - 4).c_str());
|
||||
preferences.end();
|
||||
}
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
void setup() {
|
||||
#if DEBUG
|
||||
Serial.begin(115200);
|
||||
#endif
|
||||
|
||||
// Start preferences persistence
|
||||
preferences.begin("sumorobot", false);
|
||||
|
||||
// Create the BLE device
|
||||
Serial.println(preferences.getString("name", "SumoRobot").c_str());
|
||||
BLEDevice::init(preferences.getString("name", "SumoRobot").c_str());
|
||||
|
||||
preferences.end();
|
||||
|
||||
// Create the BLE server
|
||||
bleServer = BLEDevice::createServer();
|
||||
bleServer->setCallbacks(new MyServerCallbacks());
|
||||
|
||||
// Create device info service and characteristic
|
||||
BLEService * deviceInfoService = bleServer->createService(BLEUUID((uint16_t) 0x180a));
|
||||
BLECharacteristic * modelCharacteristic = deviceInfoService->createCharacteristic(
|
||||
(uint16_t) 0x2A24, BLECharacteristic::PROPERTY_READ);
|
||||
BLECharacteristic * firmwareCharacteristic = deviceInfoService->createCharacteristic(
|
||||
(uint16_t) 0x2A26, BLECharacteristic::PROPERTY_READ);
|
||||
BLECharacteristic * manufacturerCharacteristic = deviceInfoService->createCharacteristic(
|
||||
(uint16_t) 0x2a29, BLECharacteristic::PROPERTY_READ);
|
||||
manufacturerCharacteristic->setValue("RoboKoding LTD");
|
||||
modelCharacteristic->setValue("SumoRobot");
|
||||
firmwareCharacteristic->setValue(VERSION);
|
||||
|
||||
// Create battery service
|
||||
BLEService * batteryService = bleServer->createService(BLEUUID((uint16_t) 0x180f));
|
||||
// Mandatory battery level characteristic with notification and presence descriptor
|
||||
BLE2904* batteryLevelDescriptor = new BLE2904();
|
||||
batteryLevelDescriptor->setFormat(BLE2904::FORMAT_UINT8);
|
||||
batteryLevelDescriptor->setNamespace(1);
|
||||
batteryLevelDescriptor->setUnit(0x27ad);
|
||||
// Create battery level characteristics
|
||||
batteryLevelCharacteristic = batteryService->createCharacteristic(
|
||||
(uint16_t) 0x2a19, BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_NOTIFY);
|
||||
batteryLevelCharacteristic->addDescriptor(batteryLevelDescriptor);
|
||||
batteryLevelCharacteristic->addDescriptor(new BLE2902());
|
||||
|
||||
// Create the BLE NUS service
|
||||
BLEService * nusService = bleServer->createService(NUS_SERVICE_UUID);
|
||||
|
||||
// Create a BLE NUS transmit characteristic
|
||||
nusTxCharacteristic = nusService->createCharacteristic(
|
||||
NUS_CHARACTERISTIC_TX_UUID, BLECharacteristic::PROPERTY_NOTIFY);
|
||||
nusTxCharacteristic->addDescriptor(new BLE2902());
|
||||
|
||||
// Create a BLE NUS receive characteristics
|
||||
BLECharacteristic * nusRxCharacteristic = nusService->createCharacteristic(
|
||||
NUS_CHARACTERISTIC_RX_UUID, BLECharacteristic::PROPERTY_WRITE);
|
||||
nusRxCharacteristic->setCallbacks(new MyCallbacks());
|
||||
|
||||
// Start the services
|
||||
deviceInfoService->start();
|
||||
batteryService->start();
|
||||
nusService->start();
|
||||
|
||||
// Start advertising
|
||||
bleServer->getAdvertising()->start();
|
||||
|
||||
#if DEBUG
|
||||
Serial.println("Waiting a client connection to notify...");
|
||||
#endif
|
||||
|
||||
// Setup BLE connection status LED
|
||||
pinMode(5, OUTPUT);
|
||||
connectionLedTimer.attach_ms(2000, blinkConnectionLed);
|
||||
|
||||
// Setup the left servo PWM
|
||||
ledcSetup(1, 50, 10);
|
||||
ledcAttachPin(15, 1);
|
||||
|
||||
// Setup the right servo PWM
|
||||
ledcSetup(2, 50, 8);
|
||||
ledcAttachPin(4, 2);
|
||||
|
||||
// Phototransistor pull-ups
|
||||
pinMode(19, INPUT_PULLUP);
|
||||
pinMode(23, INPUT_PULLUP);
|
||||
|
||||
// Setup battery charge detection pin
|
||||
pinMode(25, INPUT);
|
||||
|
||||
// Setup sensor feedback LED pins
|
||||
pinMode(16, OUTPUT);
|
||||
pinMode(17, OUTPUT);
|
||||
pinMode(12, OUTPUT);
|
||||
|
||||
// Setup ADC for reading phototransistors and battery
|
||||
analogSetAttenuation(ADC_11db);
|
||||
adcAttachPin(32);
|
||||
adcAttachPin(33);
|
||||
adcAttachPin(34);
|
||||
|
||||
// Setup sonar timer to update it's value
|
||||
sonarTimer.attach_ms(50, updateSonarValue);
|
||||
|
||||
// Setup battery level timer to update it's value
|
||||
batteryTimer.attach(5, updateBatteryLevel);
|
||||
updateBatteryLevel();
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// When BLE got disconnected
|
||||
if (!deviceConnected && oldDeviceConnected) {
|
||||
delay(500); // Give the bluetooth stack the chance to get things ready
|
||||
bleServer->startAdvertising(); // Restart advertising
|
||||
#if DEBUG
|
||||
Serial.println("start advertising");
|
||||
#endif
|
||||
oldDeviceConnected = deviceConnected;
|
||||
connectionLedTimer.attach_ms(2000, blinkConnectionLed);
|
||||
}
|
||||
// When BLE got connected
|
||||
if (deviceConnected && !oldDeviceConnected) {
|
||||
oldDeviceConnected = deviceConnected;
|
||||
connectionLedTimer.detach();
|
||||
digitalWrite(5, LOW);
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue