Change to MicroPython BLE

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)

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 : )  

boot.py

@@ -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)

config.json

@@ -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
+}

hal.py

@@ -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

main.py

@@ -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()

platformio.ini

@@ -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

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);
-  }
-}