Vamos a programar el microcontrolador esp32 para que se baje las instrucciones de un api y las ejecute. De momento son instrucciones de navegador, tipo LOGO. Gira izquierda, gira derecha, avanza, retrocede, ... y que envíe el log de los pasos realizados en un POST para su posterior análisis. Si no entiendes lo que ves en la foto, los libros son unas cosas hechas con láminas de papel pegadas en un borde que se llama lomo.

Manual

El manual del interfaz sci está mal para la serie 800 pero es lo que hay así que échale un ojo porque vamos a hacer referencia a él en los diferentes apartados: create2-io-spec.pdf .

Cosas que podemos hacer con esp32-wroom

Leer y escribir archivos

Podemos usar la memoria del microcontrolador como un sistema de archivos normal. :-)

Eso significa que podemos leer un archivo de configuración donde almacenamos la contraseña de la wifi.

Y si estructuramos el código correctamente, podemos ver si hay alguna actualización en algún sitio y escribir el .py antes de hacer su import, o podríamos modificar el código y al reiniciar se ejecutaría.

Conexión a la wifi

Aquí, la primera en la frente.

Si te gusta el formato yaml, olvídate de él porque no puedes hacer un import yaml. El "micro" de micropython significa que faltan cosas, es lo que hay.

Primero cargamos la configuración:

import json

def load_config(config="config.json"):
    with open(config, 'r') as file:
        content = json.load(file)
    return content


config = load_config()
print(config)

Ahora nos conectamos a la wifi:

import network

def wifi(essid, password):
    # Establish Wi-Fi connection
    print("Connecting to wifi ...")
    sta_if = network.WLAN(network.STA_IF)
    sta_if.active(True)
    sta_if.connect(essid, password)

    # Wait for connection
    while not sta_if.isconnected():
        print("Connecting to wifi ...")
        time.sleep(1)

wifi(config["wifi"]["essid"], config["wifi"]["password"])

La librería de red permite hacer un montón de cosas. Por ejemplo, podríamos comprobar que redes hay disponibles y elegir la que tenga más intensidad de señal así: print(network.WLAN(network.STA_IF).scan())

¿Que hora es?

Vamos a sincronizar la hora con un servidor de ntp y la configuramos en la roomba:

import ntptime
import time

def sync_time():
    #Sync time
    #In a real application, you might need a more sophisticated approach
    ntptime.settime()
    rtc = machine.RTC()
    print('Current RTC time:', rtc.datetime())

    # Time sync with Roomba
    now = time.localtime()
    SET_DAY_TIME = bytes([168, now[6], now[3], now[4]])
    uart.write(SET_DAY_TIME)
    time.sleep(0.2)

print("syncing time")
sync_time()

Esta función falla más que una escopeta de feria así que métela en un try catch.

En la página 12 del manual se explica como se configura la fecha en la roomba con el Opcode 168.

En mi caso, utilizo una marca de tiempo para identificar la ejecución porque sino en los logs encontrar algo es un lío:

import utime

t = utime.localtime()
run_id = "{:04d}-{:02d}-{:02d}-{:02d}-{:02d}-{:02d}".format(t[0], t[1], t[2], t[3], t[4], t[5])

print("Run id: " + str(run_id))

Empieza el baile

Hablando con la roomba

¡¡¡Recuerda que tiene que estar encendida para que esto funcione!!!

En la entrada anterior muestro como he conectado en el pin 32 un interruptor que pulsa el botón de encender.

Si ya estuviera encendida empezaría la limpieza pero justo después se detendría al hacer el START:

from machine import UART

START = bytes([128])
SAFE_MODE = bytes([131])

def start_roomba():
    # Switch on physically the roomba 8xx using a relay in pin 32
    power_button=Pin(32, Pin.OUT)
    power_button.value(1)
    time.sleep(0.2)
    power_button.value(0)
    time.sleep(1)
    print("powered on")

    # Initialize Roomba sci port
    uart.write(START)
    time.sleep(0.2)
    uart.write(SAFE_MODE)
    time.sleep(0.2)

print("starting roomba")
start_roomba()

Échale un ojo a las páginas 8 y 10 del manual.

No tengo claro que el SAFE_MODE esté funcionando en la serie 800 porque la máquina se choca y las ruedas siguen girando, pero es lo que hay.

Los leds

Esto no es necesario para lo que queremos, pero es que mola un montón. :-D

La documentación de las página está mal así que he ido probando y he sacado estas funciones.

Para el led de power podemos elegir el color, entre verde y rojo en el tercer byte, y la velocidad del parpadeo en el cuarto byte tal que así:

def power_led (color="green", fadding="no"):
    # Page 16 is for 6xx models but can be used to do the tests.
    # This function correspond to 8xx models

    ## Looks like ignores the first bit because 128 backs to green
    ## uart.write(bytes([139, 0, 128, 255]))
    colors = {"green": 0, "white": 64, "red": 127}

    faddings = {"no": 0, "slow": 1, "fast": 2}

    uart.write(bytes([139, 0, colors[color], faddings[fadding]]))
    time.sleep(0.1)

power_led(fadding="slow")

Para el resto de leds podemos elegir entre encendido y apagado en el segundo byte:

def led(led="clean"):
    leds = {
        "blue-left": 1,
        "white-right": 2,
        "white-left": 4,
        "red-up": 8,
        "clean": 16,
        "blue-right": 32,
        "red-down": 64,
        "white-right-up": 128,
        "all": 255
    }
    uart.write(bytes([139, leds[led], 0, 0]))

Si te fijas, son todo potencias de dos porque eso lo pasa a binario y cada bit es un led distinto así que te interesa tener una variable con el estado de todos los leds e ir modificándola.

La máquina tiene más leds pero no he conseguido encenderlos y tampoco he querido investigar más.

Échale un ojo a la página 16 del manual.

Distancia recorrida

En teoría debería devolvernos la distancia en milímetros pero nos devuelve algo "parecido" a centímetros, donde 30.5 unidades son 25 centímetros reales, así que hacemos una regla de 3:

# Function to get distance travelled
def get_distance():
    # 25cm son unos 30 o 31 para get_distance() :-/
    uart.write(bytes([142, 19]))  # Request Distance
    time.sleep(0.02)
    distance_data = uart.read(2)
    if distance_data is not None:
        print("distance: " + str(distance_data))
        high, low = distance_data
        distance = (high << 8) | low
        if distance > 32767:
            distance -= 65536
        
        # rule of three:
        # 30.5     -> 25cm
        # distance -> x
        real_distance = distance * 25 / 30.5
        # When going forward returns negative numbers
        return real_distance * -1
    else:
        print("Error reading distance data from Roomba.")
        return None  # Or handle the error differently if needed.

# Initialize distance because first result could be buggy
get_distance()

Échale un ojo a las páginas 21 y 27 del manual.

Se que es raro, pero puse la roomba encima de una mesa con una regla debajo a moverse alante y atrás para medir cuanto se desplaza. La regla de tres funciona.

Batería

¡¡¡NO dejes que la batería se agote!!!

Las baterías de litio se degradan MUCHO si se quedan sin carga. En su funcionamiento normal, la roomba se apaga si la batería está por debajo del 20% ... creo ... pero cuando envías el comando START esa protección se desactiva.

Vigila el nivel de batería en tu script y nunca dejes que se agote:

# Function to get battery percentage
def get_battery_percentage():
    uart.write(bytes([142, 25]))  # Request battery charge
    time.sleep(0.015)
    battery_charge = int.from_bytes(uart.read(2), 'big')
    
    uart.write(bytes([142, 26]))  # Request battery capacity
    time.sleep(0.015)
    battery_capacity = int.from_bytes(uart.read(2), 'big')

    battery_percentage = (battery_charge / battery_capacity) * 100
    return battery_percentage

print("Battery: " + str(get_battery_percentage()))

Échale un ojo a las páginas 21 y 28 del manual.

Botones

No se porqué perdí el tiempo en esto porque no lo voy a usar, pero aquí tienes también los botones.

Hay botones que dependiendo del que hayas pulsado antes muestran una salida diferente. Aquí está la función para leerlos:

def get_button():
    uart.write(bytes([142, 18]))  # Request buttons
    time.sleep(0.017)
    output=uart.read(1)
    if output is None or output == b'\x00' or output == b'x80':
        button = None
    elif output == b'\x01':
        button = "clean"
    elif output == b'\x04':
        button = "dock"

    elif output == b'\x80':
        button = "clock mode"
    elif output == b'\xa0':
        button = "clock + dock"
    elif output == b'\x90':
        button = "clock + clock"
    elif output == b'\x88':
        button = "clock + schedule"

    elif output == b'@':
        button = "schedule mode"
    elif output == b'`':
        button = "schedule + dock"
    elif output == b'P':
        button = "schedule + clock"
    elif output == b'H':
        button = "schedule + schedule"

    else:
        button = "uart_read: " + str(output)
    print(button)

El misterio del LOGO

¿Recuerdas este libro de cuando éramos pequeños?

Había una tortuga a la que le podías decir, giraizquierda, giraderecha, avanza, bajalapiz, subelapiz, ... y con eso se podían hacer dibujos sobre un lienzo.

Si quieres probarlo, instala el kturtle.

Y si sigues sin saber que es, pregúntale a gemma3:

Al lío

Al lío, vamos a implementar las funciones para moverse en la roomba, tiene una particularidad. No le puedes decir que avance 3 metros pero le puedes decir que arranque una velocidad y esperar el tiempo que se tarda en recorrer tres metros.

Esto viene muy bien porque permite hacer más cosas mientras se están ejecutando instrucciones, por ejemplo puedes usar el reloj interno para saber cuando han pasado x segundos en un bucle o directamente meter un sleep, que es lo que hago a continuación.

La interfaz te permite manejar las dos ruedas a la vez o de forma independiente. Lo tienes todo en la página 13 del manual.

Como esto es una PoC vamos a definir dos posibilidades: avanzar y girar sobre su eje:

DRIVE = bytes([137]) + bytes([0, 100, 128, 0]) # bytes para 100 mm/s de velocidad y radio 'Directo'
DRIVE_BACK = bytes([137]) + bytes([255, 155, 128, 0]) # bytes para -100 mm/s de velocidad y radio 'Directo'
DRIVE_RIGHT = bytes([137]) + bytes([0, 100, 255, 255])
DRIVE_LEFT = bytes([137]) + bytes([0, 100, 0, 1])
STOP = bytes([137, 0, 0, 0, 0])  # velocidad 0, radio 0

Avanzar:

def av(distance):
    # Input is the requested distance in cm where 4.55 are 50cm
    # 50 cm -> 4.55 seconds
    # distance -> x seconds
    led("red-up")
    uart.write(STOP)
    uart.write(DRIVE)
    time.sleep((4.55 * distance)/50)
    uart.write(STOP)
    power_led()

Girar a la izquierda:

def gi(angle):
    # Input is the requested angle. If 1.65 seconds for 90 degrees rotation
    # then 90/angle = x, where x is the multiplier for 1.65 seconds to get
    # the desired rotation speed.
    
    led("white-left")
    uart.write(STOP)
    uart.write(DRIVE_LEFT)
    time.sleep((1.65 * angle)/90)
    uart.write(STOP)
    power_led()

Girar a la derecha:

def gd(angle):
    # Input is the requested angle. If 1.65 seconds for 90 degrees rotation
    # then 90/angle = x, where x is the multiplier for 1<｜begin▁of▁sentence｜> to get
    # the desired rotation speed.
    
    led("white-right")
    uart.write(STOP)
    uart.write(DRIVE_RIGHT)
    time.sleep((1.65 * angle)/90)
    uart.write(STOP)
    power_led()

Y con esto ya somos capaces de dibujar el ejemplo de triángulo del libro:

Que sería así:

av(50)
gd(120)
av(50)
gd(120)
av(50)

O ya nos podemos venir muy arriba y dibujar a Arturo. ;-)

¡¡¡¡Funciona!!!! ... sobre el papel :-/

Y quien dice el papel dice un suelo perfectamente liso y sin colisiones.

Vamos primero al código y al final explico los problemas del mundo real.

Un servidor cutre que va registrando los eventos de la máquina cuando choca y hace un giro semialeatorio:

import os
import yaml

app = Flask(__name__)

@app.route('/event', methods=['POST'])
def store_event():
    with open("event.log", 'a') as file:
        data = request.get_json()
        print(data)
        file.write(str(data)+"\n")
        return '{"ok"}', 200

if __name__ == '__main__':
    app.run(host='0.0.0.0', port=5001, debug=True)

Y la roomba los va enviando de la siguiente forma:

def scan(iterations=2, url="http://192.168.21.41:5001"):
    request_url = url + "/event"
    headers = {'Content-Type': 'application/json'}
    print("Reset distance")
    get_distance()
    for i in range(0, iterations):
        # Advance until collide
        post_data = ujson.dumps({ 'time': actual_time(), 'event': 'drive'})
        res = urequests.post(request_url, headers = headers, data = post_data)        

        led("red-up")
        uart.write(DRIVE)
        # send drive

        collision = False
        while not collision:
            collision = check_for_collision()

            uart.write(bytes([142, 7]))  # Request Bumpers and Wheel Drops
            time.sleep(0.017)
            output = uart.read(1)
            #print("uart_read: " + str(output))
            
            # bump_and_wheel_drops = uart.read(1)[0]
            # print("bump_and_wheel_drops: " + str(bump_and_wheel_drops))

        if output == bytes([0]):
            collision = False
        elif output == bytes([1]):
            collision = 'right'
        elif output == bytes([2]):
            collision = 'left'
        elif output == bytes([3]):
            collision = 'front'
        else:
            collision = 'unknown'

        distance = get_distance()
        post_data = ujson.dumps({ 'time': actual_time(), 'event': 'collision-'+collision, 'distance': str(distance)})
        res = urequests.post(request_url, headers = headers, data = post_data)
        # send distance

        if collision == "right" or collision == "unknown":
            gi(90)
            # send rotation
            post_data = ujson.dumps({ 'time': actual_time(), 'event': 'rotate-left', 'degrees': '90'})
            res = urequests.post(request_url, headers = headers, data = post_data)

        if collision == "left" or collision == "front":
            gd(90)
            # send rotation
            post_data = ujson.dumps({ 'time': actual_time(), 'event': 'rotate-right', 'degrees': '90'})
            res = urequests.post(request_url, headers = headers, data = post_data)
        
        distance = get_distance()
        # send distance
        post_data = ujson.dumps({ 'time': actual_time(), 'event': 'collision-'+collision, 'distance': str(distance)})
        res = urequests.post(request_url, headers = headers, data = post_data)

Conclusión

En mi hipótesis inicial, si a una persona le tapamos los ojos y le pedimos que haga un escaneo de la habitación nos va a poder decir su forma, pero: - las personas tenemos manos que nos indican el ángulo de las paredes - los pies no se mueven del suelo cuando tocamos una pared

En el caso de la roomba solo podemos saber si el golpe viene de la derecha, de la izquierda o del frente. Pero un golpe frontolateral puede detectarse como uno frontal y al hacer un giro de 90 grados puede que te vuelvas a encontrar con la misma pared.

Con cada golpe en la pared, la roomba se gira un poco, solo unos grados pero esa acumulación de errores sumados a la distancia hacen que sea inviable saber donde nos encontramos tras un golpe. Es como si cada vez que tocaras una pared con un dedo te giraras un número aleatorio de grados sin que tu te dieras cuenta. ¡¡es peor que jugar a la gallinita ciega!!

La conclusión es que con los sensores que tenemos en la roomba NO SE PUEDE hacer una navegación inteligente porque nos faltan datos de entrada pero esto es ciencia así que no nos rendimos y la siguiente iteración será con un magnetrómetro (brújula para los amigos) y un sensor giroscópico.

¿que SI se puede hacer con esto?

Tengo la estación de carga en medio de la casa así que puedo hacer lo siguiente: - encender - enviar a la habitación que yo quiera - empezar limpieza - pulsar dock

Como tengo la estación de carga en medio de la casa, cuando pasa por delante vuelve a la base así que solo tengo que comprobar cuanto tiempo ha pasado desde que se fue y cuanta batería le queda. Si lleva menos de x tiempo en ese lado se la vuelve a enviar.

Código para hacer lo anterior

Tienes el código completo en github: https://github.com/jmferrer/controlling-roomba