Aquest programa ha de saber el protocol de l'XGO, les adreces que controlen cada part, i usar UART (comunicació sèrie per cable) per enviar bytes al controlador del robot.
Conté la inicialització de UART, les funcions de traducció i enviament, i les de funcionament que són les que utilitzarà test.py
Referències 👇
IDs i adreces dels servos
xgo.pota_posar_servo_id (servo_id, valor) # valor: entre 0x00 i 0xFF. neutre: 0x80
#xgo.py
from microbit import uart, pin14, pin13, sleep
# ============================================================
# UART (lazy init)
# ============================================================
BAUDRATE = 115200 # velocitat de transmissió de dades UART (bits/segon)
_uart_inicialitzat = False
# inicialitza l'UART només un cop, quan cal:
def uart_inicialitzar():
global _uart_inicialitzat
if not _uart_inicialitzat:
uart.init (baudrate=BAUDRATE, tx=pin14, rx=pin13)
_uart_inicialitzat = True
# ============================================================
# Protocol (baix nivell)
# ============================================================
def _chk (length, cmd, addr, data):
return (~((length + cmd + addr + data) & 0xFF)) & 0xFF
def escriure_byte(adreca, dada): # escriu 1 byte a un registre del robot
uart_inicialitzar()
ln = 0x09
cmd = 0x00
d = dada & 0xFF
a = adreca & 0xFF
c = _chk(ln, cmd, a, d)
uart.write(bytes([0x55, 0x00, ln, cmd, a, d, c, 0x00, 0xAA]))
sleep(2) # >=1ms recomanat
# ============================================================
# Funcions base (seguretat / init)
# ============================================================
def aturar_tot():
# atura bucles/periòdics
# IMPORTANT: NO restaura postura (per evitar moviments inesperats del braç)
escriure_byte (0x03, 0x00) # show mode normal (no loop)
# aturar rotació periòdica cos:
escriure_byte (0x39, 0x00); escriure_byte (0x3A, 0x00); escriure_byte (0x3B, 0x00)
# aturar translació periòdica cos:
escriure_byte (0x80, 0x00); escriure_byte (0x81, 0x00); escriure_byte (0x82, 0x00)
# aturar "step/no progress" periòdic:
escriure_byte (0x3C, 0x00)
def restaurar_postura():
# Restaura postura per defecte (crida-la només si TU ho vols)
escriure_byte (0x3E, 0xFF)
def velocitat_servos (valor):
# Velocitat servos en mode control directe: 0..255 (típic 0xA0)
escriure_byte (0x5C, valor & 0xFF)
def potes_mode_debug():
# Posa les 4 potes a mode debug (control directe)
escriure_byte (0x20, 0x21); escriure_byte (0x20, 0x22)
escriure_byte (0x20, 0x23);escriure_byte (0x20, 0x24)
def inicialitzar (velocitat=0xA0):
# Inicialització recomanada (sense restaurar postura)
aturar_tot()
potes_mode_debug()
velocitat_servos(velocitat)
def posicio_inicial_estable():
''' Deixa el robot en un estat físic segur i conegut:
- para tots els bucles
- locomoció a neutre (0x80)
- cos centrat (translació + rotació) """
aturar_tot()
# locomoció
escriure_byte (0x30, 0x80); escriure_byte (0x31, 0x80); escriure_byte (0x32, 0x80)
# cos (translació + rotació)
escriure_byte (0x33, 0x80); escriure_byte (0x34, 0x80); escriure_byte (0x35, 0x80)
escriure_byte (0x36, 0x80); escriure_byte (0x37, 0x80); escriure_byte (0x38, 0x80)
# seguretat extra
escriure_byte (0x3C, 0x00)
# ============================================================
# Locomoció
# ============================================================
def gait_walk():
escriure_byte(0x09, 0x00)
def caminar(vel):
gait_walk()
escriure_byte(0x30, vel & 0xFF)
escriure_byte(0x31, 0x80)
escriure_byte(0x32, 0x80)
def girar(valor):
gait_walk()
escriure_byte(0x30, 0x80)
escriure_byte(0x31, 0x80)
escriure_byte(0x32, valor & 0xFF)
def lateral(valor):
gait_walk()
escriure_byte(0x30, 0x80)
escriure_byte(0x31, valor & 0xFF)
escriure_byte(0x32, 0x80)
def stop():
caminar(0x80)
# ============================================================
# Cos
# ============================================================
def cos_translacio(eix, valor):
v = valor & 0xFF
if eix == 'x': escriure_byte(0x33, v)
elif eix == 'y': escriure_byte(0x34, v)
elif eix == 'z': escriure_byte(0x35, v)
def cos_rotacio(eix, valor):
v = valor & 0xFF
if eix == 'x': escriure_byte(0x36, v)
elif eix == 'y': escriure_byte(0x37, v)
elif eix == 'z': escriure_byte(0x38, v)
def cos_neutre():
escriure_byte(0x33,0x80); escriure_byte(0x34,0x80); escriure_byte(0x35,0x80)
escriure_byte(0x36,0x80); escriure_byte(0x37,0x80); escriure_byte(0x38,0x80)
def desplacar_cos(eix, valor):
cos_translacio(eix, valor)
def girar_cos(eix, valor):
cos_rotacio(eix, valor)
# ============================================================
# Potes
# ============================================================
# Llista amb els IDs i adreces dels 12 servos de les potes
_MAPA_ID_A_ADRECA = {
11:0x50, 12:0x51, 13:0x52,
21:0x53, 22:0x54, 23:0x55,
31:0x56, 32:0x57, 33:0x58,
41:0x59, 42:0x5A, 43:0x5B
}
def pota_posar_servo_id(servo_id, valor):
# Mou un servo de pota per ID
ad = _MAPA_ID_A_ADRECA.get(servo_id, None)
if ad is not None:
escriure_byte(ad, valor & 0xFF)
# ============================================================
# Braç (servos directes) + Pinça
# ============================================================
_ADRECA_BRAC_AVANT = 0x5D # forearm (ID 52)
_ADRECA_BRAC_GRAN = 0x5E # big arm (ID 53)
_ADRECA_PINCA = 0x71 # 0x00 oberta, 0xFF tancada
def avant_brac(valor):
escriure_byte(_ADRECA_BRAC_AVANT, valor & 0xFF)
def brac(valor):
escriure_byte(_ADRECA_BRAC_GRAN, valor & 0xFF)
def pinca_obrir():
escriure_byte(_ADRECA_PINCA, 0x00)
def pinca_tancar():
escriure_byte(_ADRECA_PINCA, 0xFF)
# (Opcional) posicionar directament pinça (X/Z)
_ADRECA_PINCA_X = 0x73
_ADRECA_PINCA_Z = 0x74
def pinca_x(valor):
escriure_byte(_ADRECA_PINCA_X, valor & 0xFF)
def pinca_z(valor):
escriure_byte(_ADRECA_PINCA_Z, valor & 0xFF)
def pinca(valor):
escriure_byte(_ADRECA_PINCA, valor & 0xFF)
# ============================================================
# Accions
# ============================================================
# llista amb les durades de les accions:
DUR = (0,
3500,3500,5500,6500,2000,
4500,5500,5500,5500,8000,
7000,5000,7000,10000,6000,
6000,4000,8000,10000
)
def stop_accio():
escriure_byte(0x3E, 0x00)
sleep(50)
aturar_tot()
escriure_byte(0x30,0x80); escriure_byte(0x31,0x80); escriure_byte(0x32,0x80)
escriure_byte(0x3C,0x00)
def executar_accio(action_id, esperar=True):
dur = _DUR[action_id] if 0 < action_id < len(_DUR) else 0
escriure_byte(0x3E, action_id & 0xFF)
if esperar and dur:
sleep(dur)
stop_accio()
return dur
def _mk_acc(n):
def f(esperar=True):
return executar_accio(n, esperar)
return f
for _i in range(1, 20):
globals()["accio_%d" % _i] = _mk_acc(_i)
#test.py
En prèmer el botó A de microbit es mou una articulació del robot
Amb el botó B es mou l'avantbraç
Tocant el logo s'obre i tanca la pinça
A+B surt del programa i torna a main
from microbit import *
import xgo
def executar(): # funció que s'executarà des de main.py
NEUTRE = 0x80
BAIX = 0x60
ALT = 0xA0
SERVO_POTA_ID = 11 # servo amb el que farem la prova: pota davantera dreta
pos_pota = NEUTRE
pos_brac = NEUTRE
pinca_oberta = True
temps_ab = 0
while True:
# Sortir amb A+B mantingut
if button_a.is_pressed() and button_b.is_pressed():
temps_ab += 50
if temps_ab >= 1000:
return
else:
temps_ab = 0
# A: toggle pota, mou l'articulació inferior
if button_a.was_pressed():
pos_pota = BAIX if pos_pota == NEUTRE else NEUTRE
xgo.pota_posar_servo_id(SERVO_POTA_ID, pos_pota)
# B: toggle braç (avantbraç)
if button_b.was_pressed():
pos_brac = ALT if pos_brac == NEUTRE else NEUTRE
xgo.avant_brac(pos_brac)
# Logo: toggle pinça
if pin_logo.is_touched():
sleep(200) # debounce
pinca_oberta = not pinca_oberta
if pinca_oberta:
xgo.pinca_obrir()
else:
xgo.pinca_tancar()
sleep(30)
