.drone.yml

@@ -1,26 +0,0 @@
----
-kind: pipeline
-type: kubernetes
-name: default
-
-steps:
-- name: build
-  image: harbor.k-space.ee/k-space/micropython-esp32
-  settings:
-    mtu: 1300
-  commands:
-  - cp /drone/src/*.py /src/ports/esp32/modules/
-  - cd /src/ports/esp32
-  - bash -c "source /opt/esp/idf/export.sh && make"
-  - cp build-GENERIC/firmware.bin /drone/src/
-- name: gitea_release
-  image: plugins/gitea-release
-  settings:
-    app_key: xxx
-    base_url: https://git.k-space.ee
-    files: firmware.bin
-  when:
-    event: tag
-    
-image_pull_secrets:
-- dockerconfig

LICENSE

@@ -1,21 +0,0 @@
-MIT License
-
-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
-in the Software without restriction, including without limitation the rights
-to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-copies of the Software, and to permit persons to whom the Software is
-furnished to do so, subject to the following conditions:
-
-The above copyright notice and this permission notice shall be included in all
-copies or substantial portions of the Software.
-
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
-SOFTWARE.

Makefile

@@ -1,7 +1,7 @@
 #!/bin/bash
 
 # When this baud does not work, try 115200
-FLASH_BAUD := 230400
+FLASH_BAUD := 500000
 
 # Image to flash
 FLASH_IMAGE := sumofirmware.bin
@@ -9,7 +9,7 @@ FLASH_IMAGE := sumofirmware.bin
 # Try to automatically find the serialport
 SERIAL_PORT := $(shell find /dev -iname "tty*usb*")
 
-all: erase flash delay config update reset
+all: erase flash delay update reset
 
 delay:
 	sleep 3
@@ -18,12 +18,7 @@ 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
 
 erase:
 	esptool.py -p $(SERIAL_PORT) -b $(FLASH_BAUD) erase_flash

README.md

@@ -1,22 +1,14 @@
-# sumorobot-firmware
+# Special SumoRobot firmware for usage with the remote
+Uses UDP packets from remote to the robot instead of BLE or WebSocket
 
-The software that is running on the SumoRobots
+MicroPython is ESP32 Generic
 
-<img alt="Code" src="https://www.robokoding.com/assets/img/sumorobot_firmware.png" width="50%">
+* https://micropython.org/download/ESP32_GENERIC/
 
-# Instructions
+# Usage
-* Change the SERIAL_PORT in the Makefile
-* Install [Python](https://www.python.org/downloads/)
-* 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
+1. Get latest MicroPython and name it `sumofirmware.bin`
-If you find our work useful, please consider donating : )  
+2. Run `make` to flash the firmware
-[![Donate using Liberapay](https://liberapay.com/assets/widgets/donate.svg)](https://liberapay.com/robokoding/donate)  
 
-
+The sumo robot will connect to `sumo` wifi and during DHCP request it also provides its hostname
-# Credits
+as `sumo-<ID>`. It will be later used by the Remote to do a DNS lookup from dnsmasq DNS server.
-* [Lauri Võsandi](https://lauri.xn--vsandi-pxa.com/)
-* [K-SPACE MTÜ](https://k-space.ee/)

boot.py

@@ -1,34 +0,0 @@
-import os
-import utime
-import machine
-
-
-# Give time to cancel this boot script
-print("Press Ctrl-C to stop new boot script...")
-utime.sleep_ms(1000)
-
-root_files = os.listdir()
-update_files = ['boot.py.new', 'main.py.new', 'hal.py.new']
-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: 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 Exception as error:
-            print("boot.py:", file, "compilation failed:", error)
-            files_to_update.clear()
-            break
-
-# If valid updates replace with new FW
-for file in files_to_update:
-    os.rename(file, file.replace('.new', ''))
-
-# If updates, reboot to load new FW
-if len(files_to_update) != 0:
-    machine.reset()

config.json

@@ -1,15 +0,0 @@
-{
-	"status_led_pin": 5,
-	"battery_coeff": 2.25,
-	"sumorobot_name": "SumoRobot",
-	"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,
-	"right_line_value": 1000,
-	"left_line_threshold": 1000,
-	"right_line_threshold": 1000
-}

hal.py

@@ -1,282 +0,0 @@
-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

@@ -1,186 +1,101 @@
-import utime
+import ubinascii
 import _thread
-import ubluetooth
+import os
-import micropython
+import network
+from utime import sleep_ms
+from machine import Timer, Pin, PWM
 
-from hal import *
+print("Press Ctrl-C to stop boot script...")
-# Loading libraries takes ca 400ms
+sleep_ms(200)
+
+led_power = Pin(25, Pin.OUT)
+led_power.value(1)
+
+pwm_left = PWM(Pin(15), freq=50, duty=0)
+pwm_right = PWM(Pin(13), freq=50, duty=0)
+
+HOSTNAME = "sumo-%s" % \
+  ubinascii.hexlify(network.WLAN().config('mac'),':').decode().replace(":","")[6:]
+
+url = "ws://sumo.koodur.com:80/p2p/%s/browser/" % HOSTNAME
+
+ap_if = network.WLAN(network.AP_IF)
+ap_if.active(False)
+
+wlan = network.WLAN(network.STA_IF)
+wlan.active(True)
+wlan.config(dhcp_hostname=HOSTNAME)
+wlan.connect("sumo", "salakala")
+#wlan.connect("k-space.ee legacy","")
+
+while not wlan.isconnected():
+    print("Connecting to wifi...")
+    sleep_ms(100)
+
+print("Connected to wifi!", wlan.ifconfig())
+print("Hi my name is:")
+print(HOSTNAME)
+def map_vals(value, leftMin, leftMax, rightMin, rightMax):
+	#http://stackoverflow.com/questions/1969240/mapping-a-range-of-values-to-another
+	# Figure out how 'wide' each range is
+	leftSpan = leftMax - leftMin
+	rightSpan = rightMax - rightMin
+
+	# Convert the left range into a 0-1 range (float)
+	valueScaled = float(value - leftMin) / float(leftSpan)
+
+	# Convert the 0-1 range into a value in the right range.
+	return int(rightMin + (valueScaled * rightSpan))
 
 
-# BLE events
-_IRQ_CENTRAL_CONNECT = micropython.const(1)
-_IRQ_CENTRAL_DISCONNECT = micropython.const(2)
-_IRQ_GATTS_WRITE = micropython.const(3)
 
-# SumoRobot functionality
+import socket
-sumorobot = Sumorobot()
+import time
+from time import sleep_ms
 
-
+sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP)
-def advertise_ble_name(name):
+sock.settimeout(0.3)
-    payload = b'\x02\x01\x02' + bytes([len(name) + 1])
+sock.bind(("", 44444))
-    payload += b'\x09' + name.encode()
+while True:
-    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]))
-
-
-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 threads
-        utime.sleep_ms(50)
-
-        # When no code to execute
-        if python_code == b'':
-            continue
-
-        # Try to execute the Python code
-        try:
-            exec(compile(python_code, "snippet", 'exec'))
-        except Exception as error:
-            print("main.py: the python code had errors:", error)
-        finally:
-            print("main.py: finized python code 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)
-        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_WRITE:
-        # Read the command
-        cmd = ble.gatts_read(rx)
-        print(cmd)
-
-        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:
-            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:
-            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 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(sumorobot.config['boot_code'], 'r') as file:
+        buf = sock.recv(20)
-            boot_code = file.read()
+    except OSError: # timed out
-            compile(boot_code, "snippet", 'exec')
+        pwm_left.duty(0)
-            python_code = boot_code
+        pwm_right.duty(0)
-    except Exception as error:
+        continue
-        print("main.py:", sumorobot.config['boot_code'], "compilation failed:", error)
 
-# Start BLE
+    try:
-ble = ubluetooth.BLE()
+        value, forward, backward = buf.decode("ascii").split(":")
-ble.config(gap_name=sumorobot.config['sumorobot_name'])
+    except ValueError:
-ble.active(True)
+        continue
+    forward, backward = int(forward), int(backward)
 
-# Register the BLE hander
+    if forward:
-ble.irq(ble_handler)
+        left, right = 10, -10
+    elif backward:
+        left, right = -10, 10
+    else:
+        left, right = 0, 0
 
-# BLE info serivce
+    mapped = map_vals(int(value),-400,400,-10,10)
-INFO_SERVICE_UUID = ubluetooth.UUID(0x180a)
+    if abs(mapped) > 5:
-MODEL_CHARACTERISTIC = (ubluetooth.UUID(0x2a24), ubluetooth.FLAG_READ,)
+        left -= mapped
-FIRMWARE_CHARACTERISTIC = (ubluetooth.UUID(0x2a26), ubluetooth.FLAG_READ,)
+        right -= mapped
-MANUFACTURER_CHARACTERISTIC = (ubluetooth.UUID(0x2a29), ubluetooth.FLAG_READ,)
-INFO_SERVICE = (INFO_SERVICE_UUID, (MODEL_CHARACTERISTIC, FIRMWARE_CHARACTERISTIC, MANUFACTURER_CHARACTERISTIC,),)
 
-# BLE battery service
+    if left < -10:
-BATTERY_SERVICE_UUID = ubluetooth.UUID(0x180f)
+        left = -10
-BATTERY_CHARACTERISTIC = (ubluetooth.UUID(0x2a19), ubluetooth.FLAG_READ | ubluetooth.FLAG_NOTIFY,)
+    if left > 10:
-BATTERY_SERVICE = (BATTERY_SERVICE_UUID, (BATTERY_CHARACTERISTIC,),)
+        left = 10
-
+    if right < -10:
-# BLE UART service
+        right = -10
-UART_SERVICE_UUID = ubluetooth.UUID('6E400001-B5A3-F393-E0A9-E50E24DCCA9E')
+    if right > 10:
-RX_CHARACTERISTIC = (ubluetooth.UUID('6E400002-B5A3-F393-E0A9-E50E24DCCA9E'), ubluetooth.FLAG_WRITE,)
+        right = 10
-TX_CHARACTERISTIC = (ubluetooth.UUID('6E400003-B5A3-F393-E0A9-E50E24DCCA9E'), ubluetooth.FLAG_READ | ubluetooth.FLAG_NOTIFY,)
+    if abs(left) > 2:
-UART_SERVICE = (UART_SERVICE_UUID, (TX_CHARACTERISTIC, RX_CHARACTERISTIC,),)
+        pwm_left.duty(78 + left)
-
+    else:
-# Register BLE services
+        pwm_left.duty(0)
-SERVICES = (INFO_SERVICE, BATTERY_SERVICE, UART_SERVICE,)
+    if abs(right) > 2:
-((model, firmware, manufacturer,), (battery,), (tx, rx,),) = ble.gatts_register_services(SERVICES)
+        pwm_right.duty(78 + right)
-
+    else:
-# Set BLE info service values
+        pwm_right.duty(0)
-ble.gatts_write(model, "SumoRobot")
+    sleep_ms(20)
-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 threads
-_thread.start_new_thread(code_process_thread, ())
-_thread.start_new_thread(sensor_feedback_thread, ())
-
-# Start BLE battery percentage update timer
-battery_timer = machine.Timer(machine.Timer.PERIODIC)
-battery_timer.init(period=3000, callback=update_battery_level)
-
-# Clean up
-import gc
-gc.collect()