# mBot2 / CyberPi Wi-Fi MQTT listener — ALL-IN-ONE (upload via mBlock, "Upload mode")

#

# Single-file merge of mbot2_listener.py + simple_mqtt.py: upload just THIS file

# to the CyberPi, nothing else. Functionally identical to the two-file setup.

#

# Turns the robot into a permanent Wi-Fi drone: it joins your LAN, connects to

# the Mosquitto broker from the docker-compose stack, reacts to drive / LED /

# mode commands, and streams encoder + color telemetry back. All piloting logic

# lives on the PC.

#

# API notes (see robot/README.md for sources):

# cyberpi.wifi.connect / is_connect, mbot2.drive_power, cyberpi.led.on,

# mbuild.quad_rgb_sensor.get_red/green/blue.

# The encoder-read and native line-offset function names are auto-detected at

# startup (their exact spelling varies by firmware), each with a fallback.

 

try:

import usocket as socket

except ImportError:

import socket

try:

import ustruct as struct

except ImportError:

import struct

 

import ujson as json

import utime as time

 

import cyberpi

import mbot2

import mbuild

 

# ---------------- configuration: EDIT THESE ----------------

WIFI_SSID = "Get yo virus myah" # /!\ FREQUENCY BAND MUST BE 2.4GHz

WIFI_PASS = "<72260960>"

BROKER_IP = "10.58.116.212" # IP of the machine running docker compose

BROKER_PORT = 1883

 

COLOR_PORT = "L1" # quad-RGB probe used for the color sensor / seek

COLOR_MATCH_TOL = 40 # tolerance for RGB matching (lower = more sensitive, higher = more forgiving)

LINE_L_PORT = "L2" # left probe for RGB-brightness line steering (outer probe, per hardware layout)

LINE_R_PORT = "R2" # right probe for RGB-brightness line steering (outer probe, per hardware layout)

LINE_BASE = 22 # forward power while line-following (lowered from 30: turns detected late, needs more reaction time)

LINE_GAIN = 20 # steering strength (lowered from 30: overshooting/oscillating on turns)

LINE_CORR_MAX = 100 # effectively unclamped, matching robot.py's proven reference for this map

ENC_L_SIGN = 1 # flip to -1 if the left wheel reads backwards

ENC_R_SIGN = 1 # flip to -1 if the right wheel reads backwards

# The two motors are mounted mirrored: drive_power(l, r) with equal signs spins

# the robot in place instead of driving straight. Confirmed on hardware via

# /api/drive {"l":40,"r":40}. Flip to 1 if a unit turns out wired the other way.

DRIVE_R_SIGN = -1

# -----------------------------------------------------------

 

# MQTT contract — keep in sync with backend/app/protocol.py

CMD_DRIVE = "mbot2/cmd/drive"

CMD_LED = "mbot2/cmd/led"

CMD_MODE = "mbot2/cmd/mode"

CMD_STOP = "mbot2/cmd/stop"

TEL_ENC = "mbot2/tel/enc"

TEL_COLOR = "mbot2/tel/color"

TEL_STATUS = "mbot2/tel/status"

TEL_MISSION = "mbot2/tel/mission"

 

NAMED = {

"red": (220, 40, 40), "green": (40, 180, 90), "blue": (50, 90, 220),

"yellow": (235, 210, 60), "black": (20, 20, 20), "white": (235, 235, 235),

}

 

# mode: manual | color_seek | line_follow ; target: [r,g,b] or None ;

# method: "rgb" (brightness steer) | "offset" (native line API) ;

# distance_ticks/start_l/start_r/last_progress: color_seek mission tracking,

# set when CMD_MODE arrives with a distance_ticks (see TEL_MISSION below).

state = {"mode": "manual", "target": None, "method": "rgb", "pl": 0, "pr": 0,

"distance_ticks": None, "start_l": 0.0, "start_r": 0.0, "last_progress": None}

 

 

# =====================================================================

# Vendored umqtt.simple — minimal MQTT 3.1.1 client for MicroPython.

# From micropython-lib (github.com MIT license.

# =====================================================================

class MQTTException(Exception):

pass

 

 

class MQTTClient:

def __init__(self, client_id, server, port=0, user=None, password=None,

keepalive=0, ssl=False, ssl_params={}):

if port == 0:

port = 8883 if ssl else 1883

self.client_id = client_id

self.sock = None

self.server = server

self.port = port

self.ssl = ssl

self.ssl_params = ssl_params

self.pid = 0

self.cb = None

self.user = user

self.pswd = password

self.keepalive = keepalive

self.lw_topic = None

self.lw_msg = None

self.lw_qos = 0

self.lw_retain = False

 

def _send_str(self, s):

if isinstance(s, str):

s = s.encode()

self.sock.write(struct.pack("!H", len(s)))

self.sock.write(s)

 

def _recv_len(self):

n = 0

sh = 0

while 1:

b = self.sock.read(1)[0]

n |= (b & 0x7F) << sh

if not b & 0x80:

return n

sh += 7

 

def set_callback(self, f):

self.cb = f

 

def set_last_will(self, topic, msg, retain=False, qos=0):

assert 0 <= qos <= 2

assert topic

self.lw_topic = topic

self.lw_msg = msg

self.lw_qos = qos

self.lw_retain = retain

 

def connect(self, clean_session=True):

self.sock = socket.socket()

addr = socket.getaddrinfo(self.server, self.port)[0][-1]

self.sock.connect(addr)

if self.ssl:

import ussl

self.sock = ussl.wrap_socket(self.sock, **self.ssl_params)

premsg = bytearray(b"\x10\0\0\0\0\0")

msg = bytearray(b"\x04MQTT\x04\x02\0\0")

 

sz = 10 + 2 + len(self.client_id)

msg[6] = clean_session << 1

if self.user is not None:

sz += 2 + len(self.user) + 2 + len(self.pswd)

msg[6] |= 0xC0

if self.keepalive:

assert self.keepalive < 65536

msg[7] |= self.keepalive >> 8

msg[8] |= self.keepalive & 0x00FF

if self.lw_topic:

sz += 2 + len(self.lw_topic) + 2 + len(self.lw_msg)

msg[6] |= 0x4 | (self.lw_qos & 0x1) << 3 | (self.lw_qos & 0x2) << 3

msg[6] |= self.lw_retain << 5

 

i = 1

while sz > 0x7F:

premsg[i] = (sz & 0x7F) | 0x80

sz >>= 7

i += 1

premsg[i] = sz

 

self.sock.write(premsg, i + 2)

self.sock.write(msg)

self._send_str(self.client_id)

if self.lw_topic:

self._send_str(self.lw_topic)

self._send_str(self.lw_msg)

if self.user is not None:

self._send_str(self.user)

self._send_str(self.pswd)

resp = self.sock.read(4)

assert resp[0] == 0x20 and resp[1] == 0x02

if resp[3] != 0:

raise MQTTException(resp[3])

return resp[2] & 1

 

def disconnect(self):

self.sock.write(b"\xe0\0")

self.sock.close()

 

def ping(self):

self.sock.write(b"\xc0\0")

 

def publish(self, topic, msg, retain=False, qos=0):

if isinstance(msg, str):

msg = msg.encode()

pkt = bytearray(b"\x30\0\0\0")

pkt[0] |= qos << 1 | retain

sz = 2 + len(topic) + len(msg)

if qos > 0:

sz += 2

assert sz < 2097152

i = 1

while sz > 0x7F:

pkt[i] = (sz & 0x7F) | 0x80

sz >>= 7

i += 1

pkt[i] = sz

self.sock.write(pkt, i + 1)

self._send_str(topic)

if qos > 0:

self.pid += 1

pid = self.pid

struct.pack_into("!H", pkt, 0, pid)

self.sock.write(pkt, 2)

self.sock.write(msg)

if qos == 1:

while 1:

op = self.wait_msg()

if op == 0x40:

sz = self.sock.read(1)

assert sz == b"\x02"

rcv_pid = self.sock.read(2)

rcv_pid = rcv_pid[0] << 8 | rcv_pid[1]

if pid == rcv_pid:

return

elif qos == 2:

assert 0

 

def subscribe(self, topic, qos=0):

assert self.cb is not None, "Subscribe callback is not set"

pkt = bytearray(b"\x82\0\0\0")

self.pid += 1

struct.pack_into("!BH", pkt, 1, 2 + 2 + len(topic) + 1, self.pid)

self.sock.write(pkt)

self._send_str(topic)

self.sock.write(qos.to_bytes(1, "little"))

while 1:

op = self.wait_msg()

if op == 0x90:

resp = self.sock.read(4)

assert resp[1] == pkt[2] and resp[2] == pkt[3]

if resp[3] == 0x80:

raise MQTTException(resp[3])

return

 

# Wait for a single incoming MQTT message and process it.

# Subscribed messages are delivered via the callback.

def wait_msg(self):

res = self.sock.read(1)

self.sock.setblocking(True)

if res is None:

return None

if res == b"":

raise OSError(-1)

if res == b"\xd0": # PINGRESP

sz = self.sock.read(1)[0]

assert sz == 0

return None

op = res[0]

if op & 0xF0 != 0x30:

return op

sz = self._recv_len()

topic_len = self.sock.read(2)

topic_len = (topic_len[0] << 8) | topic_len[1]

topic = self.sock.read(topic_len)

sz -= topic_len + 2

if op & 6:

pid = self.sock.read(2)

pid = pid[0] << 8 | pid[1]

sz -= 2

msg = self.sock.read(sz)

self.cb(topic, msg)

if op & 6 == 2:

pkt = bytearray(b"\x40\x02\0\0")

struct.pack_into("!H", pkt, 2, pid)

self.sock.write(pkt)

elif op & 6 == 4:

assert 0

return op

 

# Checks whether a pending message from server is available.

# If not, returns immediately with None. Otherwise processes it.

def check_msg(self):

self.sock.setblocking(False)

return self.wait_msg()

 

 

# =====================================================================

# Robot logic

# =====================================================================

def _valid_rgb(t):

try:

return len(t) == 3 and all(isinstance(v, (int, float)) for v in t)

except TypeError:

return False

 

 

# ---- encoder reader: auto-detect the firmware's function name + port spelling ----

# The Makeblock library defines mbot2.EM_get_angle(port) (UUID

# fadd7b62ec703bbded9b9b63d464e425); examples never read encoders, so the port

# argument spelling isn't documented — probe a few and prefer the confirmed name.

def _make_encoder_reader():

port_pairs = (("EM1", "EM2"), ("em1", "em2"), (1, 2), ("1", "2"))

for name in ("EM_get_angle", "encoder_motor_get_angle",

"encoder_motor_get_pos", "encoder_motor_get_distance"):

fn = getattr(mbot2, name, None)

if not fn:

continue

for pl, pr in port_pairs:

try:

fn(pl)

# Remember which name/port worked so the UI can show whether

# odometry is real or the open-loop fallback.

global ENC_SRC, ENC_PORTS

ENC_SRC = name + ":" + str(pl)

ENC_PORTS = (pl, pr)

return lambda a=pl, b=pr, f=fn: (f(a), f(b))

except Exception:

pass

return None

 

 

# ---- wheel-speed reader (diagnostics only) ----

# mbot2.EM_get_speed(port) is confirmed in the Makeblock API table (UUID

# 9c703762b00091af8ecdf503fab52745), same undocumented port argument as

# EM_get_angle, so reuse whatever spelling the encoder probe settled on.

def _make_speed_reader():

fn = getattr(mbot2, "EM_get_speed", None)

if not fn or not ENC_PORTS:

return None

pl, pr = ENC_PORTS

try:

fn(pl)

return lambda a=pl, b=pr, f=fn: (f(a), f(b))

except Exception:

return None

 

 

# ---- board health probes (all optional; absent APIs simply report None) ----

# Verified in the Makeblock API table: cyberpi.get_battery() (3988e0aa…),

# cyberpi.get_extra_battery() (e8e71442…), cyberpi.get_firmware_version()

# (84130d80…), cyberpi.timer.get() (5535075a…). All take no arguments.

# Note: the API exposes no Wi-Fi RSSI or IP-address call, so signal strength

# cannot be reported — only the boolean wifi.is_connect().

def _probe(fn_path, *args):

obj = cyberpi

for part in fn_path.split("."):

obj = getattr(obj, part, None)

if obj is None:

return None

try:

return obj(*args)

except Exception:

return None

 

 

# ---- native line-offset reader ----

# mbuild.quad_rgb_sensor.get_offset_track(index) -> ~ -100..100 (line deviation:

# negative = line left, positive = line right). `index` is the integer mBuild

# port (1), not a probe string. Names vary slightly by firmware, so auto-detect.

def _make_offset_reader():

for name in ("get_offset_track", "get_line_offset", "get_offset"):

fn = getattr(mbuild.quad_rgb_sensor, name, None)

if fn:

try:

fn(1)

global OFFSET_SRC

OFFSET_SRC = name

return lambda f=fn: f(1)

except Exception:

pass

return None

 

 

ENC_SRC = "open-loop" # set by _make_encoder_reader if a real encoder answers

ENC_PORTS = None

OFFSET_SRC = None # set by _make_offset_reader if the native API exists

 

_read_enc = _make_encoder_reader()

_read_offset = _make_offset_reader()

_read_speed = _make_speed_reader()

FIRMWARE = _probe("get_firmware_version")

 

_pseudo_l = 0.0

_pseudo_r = 0.0

_last_ms = time.ticks_ms()

 

 

def read_encoders():

global _pseudo_l, _pseudo_r, _last_ms

if _read_enc:

l, r = _read_enc()

return ENC_L_SIGN * l, ENC_R_SIGN * r

# Open-loop fallback: integrate commanded power into pseudo-encoder degrees

# (~286.5 deg/s per 100% power for an 80 mm wheel at ~200 mm/s). Apply the

# same wheel-direction signs as the real-encoder path so odometry matches.

now = time.ticks_ms()

dt = time.ticks_diff(now, _last_ms) / 1000.0

_last_ms = now

_pseudo_l += state["pl"] / 100.0 * 286.5 * dt

_pseudo_r += state["pr"] / 100.0 * 286.5 * dt

return ENC_L_SIGN * _pseudo_l, ENC_R_SIGN * _pseudo_r

 

 

def _brightness(port):

return (int(mbuild.quad_rgb_sensor.get_red(port))

+ int(mbuild.quad_rgb_sensor.get_green(port))

+ int(mbuild.quad_rgb_sensor.get_blue(port)))

 

 

def read_color():

r = int(mbuild.quad_rgb_sensor.get_red(COLOR_PORT))

g = int(mbuild.quad_rgb_sensor.get_green(COLOR_PORT))

b = int(mbuild.quad_rgb_sensor.get_blue(COLOR_PORT))

name = "background"

best = 90 * 90 * 3

for n, (cr, cg, cb) in NAMED.items():

d = (r - cr) ** 2 + (g - cg) ** 2 + (b - cb) ** 2

if d < best:

best = d

name = n

return r, g, b, name

 

 

LINE_DEBUG = {"raw": None, "corr": None, "lb": None, "rb": None} # surfaced via TEL_COLOR for live tuning

 

 

def line_powers():

"""Return (left, right) power for line following per the selected method."""

if state["method"] == "offset" and _read_offset:

off = _read_offset() # expected ~ -100..100, 0 = centered

# Sign flipped: the "negative = line left" assumption in get_offset_track's

# docs doesn't hold on this unit — confirmed by hardware turning the wrong

# way on a real corner.

corr = -LINE_GAIN * (off / 100.0)

corr = max(-LINE_CORR_MAX, min(LINE_CORR_MAX, corr))

LINE_DEBUG["raw"], LINE_DEBUG["corr"] = off, corr

return LINE_BASE + corr, LINE_BASE - corr

# RGB-brightness steer: the darker probe is over the line.

lb, rb = _brightness(LINE_L_PORT), _brightness(LINE_R_PORT)

err = (rb - lb) / 765.0 # >0 => left darker => line on the left

corr = LINE_GAIN * err

LINE_DEBUG["lb"], LINE_DEBUG["rb"] = lb, rb

corr = max(-LINE_CORR_MAX, min(LINE_CORR_MAX, corr))

LINE_DEBUG["raw"], LINE_DEBUG["corr"] = round(err, 3), corr

return LINE_BASE - corr, LINE_BASE + corr

 

 

def drive(l, r):

l = max(-100, min(100, int(l)))

r = max(-100, min(100, int(r)))

state["pl"], state["pr"] = l, r

mbot2.drive_power(l, DRIVE_R_SIGN * r)

 

 

def publish_status(client):

"""Status + board diagnostics for the web UI's debug panel (1 Hz).

 

Every health field is best-effort: a firmware that lacks a call reports null

rather than crashing the control loop.

"""

spd = None

if _read_speed:

try:

sl, sr = _read_speed()

spd = [sl, sr]

except Exception:

spd = None

client.publish(TEL_STATUS, json.dumps({

"mode": state["mode"], "method": state["method"], "online": True,

"battery": _probe("get_battery"),

"extra_battery": _probe("get_extra_battery"),

"fw": FIRMWARE,

"uptime": _probe("timer.get"),

"wifi": _probe("wifi.is_connect"),

"enc_src": ENC_SRC,

"offset_src": OFFSET_SRC,

"spd": spd,

}))

 

 

def on_message(topic, payload):

try:

p = json.loads(payload) if payload else {}

except Exception:

return

t = topic.decode() if isinstance(topic, bytes) else topic

if t == CMD_DRIVE:

if state["mode"] == "manual":

drive(p.get("l", 0), p.get("r", 0))

elif t == CMD_STOP:

state["mode"] = "manual"

state["target"] = None

drive(0, 0)

elif t == CMD_LED:

r, g, b = int(p.get("r", 0)), int(p.get("g", 0)), int(p.get("b", 0))

idx = int(p.get("index", 0))

if idx == 0:

cyberpi.led.on(r, g, b) # all LEDs

else:

cyberpi.led.on(r, g, b, id=idx) # single LED 1..5

elif t == CMD_MODE:

state["mode"] = p.get("mode", "manual")

tgt = p.get("target")

state["target"] = tgt if _valid_rgb(tgt) else None

if p.get("method") in ("rgb", "offset"):

state["method"] = p["method"]

dist = p.get("distance_ticks")

if state["mode"] in ("color_seek", "line_follow") and isinstance(dist, (int, float)) and dist > 0:

state["distance_ticks"] = dist

state["start_l"], state["start_r"] = read_encoders()

state["last_progress"] = None

else:

state["distance_ticks"] = None

if state["mode"] == "manual":

drive(0, 0)

 

 

def ensure_wifi():

if not cyberpi.wifi.is_connect():

cyberpi.wifi.connect(WIFI_SSID, WIFI_PASS)

while not cyberpi.wifi.is_connect():

cyberpi.console.println("wifi...")

time.sleep(0.5)

cyberpi.console.println("wifi ok")

 

 

def connect_mqtt(client):

ensure_wifi()

client.connect()

for topic in (CMD_DRIVE, CMD_LED, CMD_MODE, CMD_STOP):

client.subscribe(topic)

publish_status(client)

cyberpi.console.println("mqtt ok")

 

 

def main():

client = MQTTClient("mbot2-" + str(time.ticks_ms()), BROKER_IP,

port=BROKER_PORT, keepalive=30)

client.set_callback(on_message)

# Last-Will: broker publishes this if the robot drops, so the UI clears.

client.set_last_will(TEL_STATUS, json.dumps({"mode": "offline", "online": False}))

connect_mqtt(client)

 

last_enc = last_col = last_status = last_ping = time.ticks_ms()

while True:

try:

client.check_msg() # non-blocking; fires on_message for commands

now = time.ticks_ms()

mode = state["mode"]

 

# Read the color sensor only when a mode needs it or telemetry is due.

col_due = time.ticks_diff(now, last_col) >= 200

color = read_color() if (mode != "manual" or col_due) else None

 

if mode == "color_seek" and state["target"]:

drive(25, 25)

if state["distance_ticks"]:

el, er = read_encoders()

traveled = (abs(el - state["start_l"]) + abs(er - state["start_r"])) / 2.0

pct = int(min(100, traveled / state["distance_ticks"] * 100))

if pct != state["last_progress"]:

state["last_progress"] = pct

client.publish(TEL_MISSION, json.dumps(

{"event": "progress", "progress": pct}))

tr, tg, tb = state["target"]

if color and abs(color[0] - tr) < COLOR_MATCH_TOL and abs(color[1] - tg) < COLOR_MATCH_TOL \

and abs(color[2] - tb) < COLOR_MATCH_TOL:

drive(0, 0)

state["mode"] = "manual"

was_mission = state["distance_ticks"] is not None

state["distance_ticks"] = None

publish_status(client) # report the stop immediately

if was_mission:

client.publish(TEL_MISSION, json.dumps({"event": "arrival"}))

elif mode == "line_follow":

l, r = line_powers()

drive(l, r)

if state["distance_ticks"]:

el, er = read_encoders()

traveled = (abs(el - state["start_l"]) + abs(er - state["start_r"])) / 2.0

pct = int(min(100, traveled / state["distance_ticks"] * 100))

if pct != state["last_progress"]:

state["last_progress"] = pct

client.publish(TEL_MISSION, json.dumps(

{"event": "progress", "progress": pct}))

if state["target"] and color: # optional stop-on-color

tr, tg, tb = state["target"]

if abs(color[0] - tr) < COLOR_MATCH_TOL and abs(color[1] - tg) < COLOR_MATCH_TOL \

and abs(color[2] - tb) < COLOR_MATCH_TOL:

drive(0, 0)

state["mode"] = "manual"

was_mission = state["distance_ticks"] is not None

state["distance_ticks"] = None

publish_status(client)

if was_mission:

client.publish(TEL_MISSION, json.dumps({"event": "arrival"}))

 

if time.ticks_diff(now, last_enc) >= 50: # 20 Hz encoders

el, er = read_encoders()

client.publish(TEL_ENC, json.dumps({"l": el, "r": er, "t": now}))

last_enc = now

if col_due: # 5 Hz color

if color is None:

color = read_color()

client.publish(TEL_COLOR, json.dumps(

{"r": color[0], "g": color[1], "b": color[2], "name": color[3],

"line_raw": LINE_DEBUG["raw"], "line_corr": LINE_DEBUG["corr"],

"line_lb": LINE_DEBUG["lb"], "line_rb": LINE_DEBUG["rb"]}))

last_col = now

if time.ticks_diff(now, last_status) >= 1000:

publish_status(client)

last_status = now

if time.ticks_diff(now, last_ping) >= 15000: # keep session alive

client.ping()

last_ping = now

time.sleep(0.01)

except Exception as e:

# Safety first: stop the motors, then rebuild the Wi-Fi/MQTT link so a

# transient drop can't leave the robot driving at the last power.

try:

mbot2.drive_power(0, 0)

except Exception:

pass

cyberpi.console.println("link lost, reconnecting")

try:

client.disconnect()

except Exception:

pass

while True:

try:

connect_mqtt(client)

break

except Exception:

time.sleep(2)

last_enc = last_col = last_status = last_ping = time.ticks_ms()

 

 

main()