Lego Mindstorms NXT: BlueTooth

embeddedArco

Comunicar el NXT con el PC por medio de Bluetooth. Se explica brevemente el protocolo de comunicaciones y se presenta un ejemplo: un servidor de echo.

Introducción

Como sabes, el NXT es capaz de comunicarse con otros dispositivos mediante Bluetooth. Puede ‘hablar’ con otros NXT’s pero también con móviles, PDA’s, PC’s, etc. El único requisito para establecer un enlace con el NXT es que el dispositivo implemente el perfil SP (Serial Profile), que emula una conexión serie inalámbrica. De momento, probaremos usando un PC con Bluetooth.

Ingredientes

Para esta receta necesitarás soporte Bluetooth en el PC y algunas cosillas como:

  • bluez-gnome, bluez-utils, bluez-hcidump, libbluetooth-dev
  • NXTRC
  • nbc

Los paquetes relacionados con Bluetooth los puedes instalar como de costumbre: usando apt[-get,itude]. El NXTRC es una herramienta que permite comunicarse con el NXT usando Bluetooth. Es libre y su código fuente es muy útil para aprender muchas cosas. La puedes descargar de la página del autor, que está en las referencias. En cuanto a nbc, ya lo conocerás, hablamos de él en Lego Mindstorms NXT: programar con NXC. La instalación del NXTRC es similar al ‘Talk 2 NXT’: simplemente copia el binario en una carpeta que esté en el PATH y listo.

Nota: EL sdptool, en su versión 3.13, no funciona correctamente con los kernels nuevos (no sé la causa), pero el problema está corregido en la versión 3.18. Te la puedes descargar de su página y compilarla (aunque quizá ya esté empaquetada).

Preparando las comunicaciones

Antes de nada, un pequeño detalle. Ya sabes que el perfil que vamos a usar es el SP. En el caso que veremos, tiene ciertas restricciones: en la comunicación entre el NXT y nuestro PC se establece un maestro y un esclavo. El que inicia las comunicaciones es el maestro y es el que toma el rol activo en la conversación. Es decir, el esclavo sólo responde a los mensajes, nunca empieza las transmisiones.

Teniendo esto en mente, podemos establecer la comunicación de dos formas: usando el NXT como maestro o como esclavo. Por tanto, si desde el PC te conectas al NXT (el PC se convierte en maestro) y quieres, por ejemplo, saber el estado de un sensor, primero le envias un comando para que ejecute una determinada acción (leer el valor del sensor) y luego otro para leer la respuesta. Es una limitación del protocolo, pero hay que atenerse a ella.

Otro apunte, el core del bluetooth tiene su propio firmware, que interpreta los mensajes que envíes siguiendo un formato de paquete bastante sencillo. Esto está pensado para que puedas controlar el NXT remotamente sin tener un servidor corriendo en el brick. Así pues, si quieres activar un motor o leer un fichero de la flash, el core del bluetooth lo hace transparentemente. Esto implica que si quieres hablar con un programa que corre dentro del NXT OS, tienes que usar el protocolo que habla el core bluetooth, que ofrece un ‘método’ para pasar mensajes (como strings) al programa activo en el ladrillo. Así mismo, ofrece otro ‘método’ para leer los mensajes que el programa quiera enviar al PC (todo esto, teniedo en cuenta que el maestro es el PC).

Bien, hasta ahora no hemos hecho nada práctico. Vamos a ello. La forma más sencilla de empezar es buscando el NXT. Para ello, enchufa o activa el Bluetooth en el PC y en el NXT (de forma que sea localizable) y ejecuta lo siguiente:

$ NXTRC -s
00:11:22:33:44:55  NXT
…
$

La opción -s (de scan) sirve para localizar dispositivos, mostrando la MAC de los que encuentra. También puedes utilizar hcitool scan. Lo que nos interesa es que descubra el NXT para saber la dirección que tiene.

Una vez que tengas la dirección, puedes trastear un poco con las opciones del NXTRC para obtener información del ladrillo. Por ejemplo:


$ NXTRC -a 00:11:22:33:44:55 -i # muestra información del ladrillo
=: ** Getting Brick Info

=: Device Name : NXT_ARCO
=: BT Address : 00:11:22:33:44:55
=: Free space : 79976
=: Battery Level: 7415mV
=: Firmware Ver : 1.3
=: Protocol Ver : 1.124

$ NXTRC -a 00:11:22:33:44:55 -l # lista el contenido de la Flash

=: Files: Name Size NVConfig.sys 1 RPGReader.sys 14346 Try-Touch.rtm 3788 Try-Light.rtm 4456 Try-Sound.rtm 6864 Try-Ultrasonic.rtm 3756 Try-Motor.rtm 2630 Woops.rso 4699 faceopen.ric 316 faceclosed.ric 316 ! Startup.rso 8161 ! Click.rso 451 ! Attention.rso 1755
=: Total 51539 Bytes

Bien, esto confirma que hay comunicación entre el PC y el NXT. La primera vez que te conectes, en el brick aparecerá un ‘popup’ para que escribas el PIN que vas a usar (por defecto 1234). Una vez aceptado, si tienes activo el bluetooth-applet, te saldrá un mensaje para que rellenes el PIN en el PC. Tienes varios intentos. Si algo sale mal, puedes eliminar la configuración para ese dispositivo borrando el directorio cuyo nombre se corresponde con la MAC del brick, que está en /var/lib/bluetooth (también se puede hacer desde el applet). Si sigues teniendo problemas, prueba a arrancar hcidump como root y empieza de nuevo. En la salida de hcidump podrás ver qué sucede en la comunicación. Revisa también los permisos de tu usuario, no vaya a ser que no tengas los suficientes.

Revisemos ahora un poco la estructura de los mensajes que se envian. Los paquetes son secuencias de bytes. Los dos primeros bytes especifican la longuitud del paquete (sin contarse a si mismos). El orden es: LSB MSB. El tercer byte indica el tipo de mensaje, a saber:

  • 0×00: Direct Command, con respuesta
  • 0×01: System Command, con respuesta
  • 0×02: Respuesta
  • 0×80: Direct Command, sin respuesta
  • 0×81: System Command, sin respuesta

Los Direct Command son para controlar aspectos relacionados con acciones del usuario (se pasan a la máquina virtual directamente), como ejecutar un programa, controlar sensores, motores, etc. Los System Command son específicos para controlar la FLASH (abrir, leer, escribir ficheros…), reiniciar el sistema, etc. Los que nos interesan ahora son los Direct Commands. Una cosa a tener en cuenta, por tanto, es el límite de tamaño de los paquetes, que está especificado en 64 bytes.

El cuarto byte indica el comando propiamente dicho. Para una lista completa, consultad “Appendix 2 – LEGO MINDSTORMS NXT Direct commands”, que se encuentra en la documentación disponible en el “Bluetooth Developer Kit” que ofrece Lego en la página de NXTreme. Nos interesan ahora sólamente dos:

  • 0×09: Message Write
  • 0×13: Message Read

El resto de bytes depende del tipo de comando. En nuestro caso, el comando Message Read requiere que se especifique el ‘mailbox’ remoto y el local (que no usamos) y si se desea borrar el contenido una vez leido. Message Write necesita el ‘mailbox’ remoto y el tamaño del mensaje que enviamos, incluyendo el ‘\0’ final, ya que se tratan las cadenas como en C.

Los mailbox simplemente son buffers en donde almacenar mensajes. Existen mailbox de entrada (inbox) en donde el PC escribirá los mensajes, y de salida (outbox), en donde el NXT escribira los suyos. El sistema operativo del brick mantiene 10 inbox y 10 outbox. De momento, solo usaremos uno.

Bien, una vez sabido todo esto, veamos un ejemplo sencillo en el que se envía una cadena (como no, “hello world”) desde el NXT al PC.

Primer test: ‘hello world’

Ya que el brick provee una API específica para las comunicaciones con Bluetooth, nosotros no debemos preocuparnos de los detalles de los mensajes. Usando NXC y el soporte del sistema operativo, hacer este ejemplo es muuy sencillo.

Antes de ver el código, tenemos que preparar el terreno. Para que este test funcione, se debe haber enlazado el NXT con el PC. En este caso, el maestro será el NXT. Para conseguirlo, el PC debe implementar el perfil SP. ¿Cómo hacemos esto? Sencillo, si usamos sdptool (la versión que funciona Smiling). Con sdptool añadimos el perfil SP como sigue:


$ sdptool browse local
Browsing FF:FF:FF:00:00:00 …

$ sdptool add —channel=3 SP
Serial Port service registered

$ sdptool browse local
Browsing FF:FF:FF:00:00:00 …
Service Name: Serial Port
Service Description: COM Port
Service RecHandle: 0×10000
Service Class ID List: “Serial Port” (0×1101)
Protocol Descriptor List: “L2CAP” (0×0100) “RFCOMM” (0×0003) Channel: 3
Language Base Attr List: code_ISO639: 0×656e encoding: 0×6a base_offset: 0×100
Profile Descriptor List: “Serial Port” (0×1101) Version: 0×0100

Recuerda tener los servicios Bluetooth activos. Ahora, nos ponermos a escuchar en el canal que hemos especificado (en este caso el 3), usando rfcomm:

	
  1. rfcomm listen /dev/rfcomm0 3
    Waiting for connection on channel 3

Se especifica el dispositivo (/dev/rfcomm0) que se creará cuando se enlace a dicho canal (3) un cacharro Bluetooth (en este caso, el NXT). Ahora, en el brick, busca el PC y enlázate (usando el endpoint que quieras, por ejemplo, el 1). Si obtienes un mensaje de tipo “line busy” es que ha habido un problema. Usa de nuevo hcidump para ver que puede ser. Si todo va bien, en el terminal que ejecutaste rfcomm deberías ver un mensaje avisándote de la conexión establecida…

	
  1. rfcomm listen /dev/rfcomm0 3
    Waiting for connection on channel 3
    Connection from 00:11:22:33:44:55 to /dev/rfcomm0
    Press CTRL-C for hangup

¿lo ves? Perfecto. Sticking out tongue

Ahora veamos el código que se ejecutará en el NXT:


// * coding: utf-8 *

// “BT test1”, hello world enviado desde NXT por BT al PC
// NXT: maestro, PC: esclavo

// compilar con: nbc bttest1.nxc -O=bttest1.rxc
// mas info en: http://crysol.org/node/777

#define BTCON 1

// Único hilo del programa
task main(){

// Comprobamos si hay conectividad en el ENDP 1 if (BluetoothStatus(BTCON) != NO_ERR){ TextOut(5,LCD_LINE2,“Error”); Wait(2000); Stop(true); } // Enviamos el texto adecuado TextOut(10,LCD_LINE1, “Enviando texto…”); BluetoothWrite(BTCON, “Hola desde el NXT!”); TextOut(10,LCD_LINE2, “Hecho”); Wait(2000); }

Compílalo y súbelo al NXT. Lo puedes subir usando Bluetooth, con NXTRC, de la siguiente forma:

$ NXTRC -a 00:11:22:33:44:55 -w bttest1.rxe

Lo primero que hace es comprobar si has hecho los deberes :-)… debe existir un enlace en el endpoint 1, y ya que los endpoints 1 al 3 son para ‘esclavos’, implicitamente se deja ver que el NXT es el maestro.

Lo siguiente es enviar el mensaje, con BluetoothWrite(). Existen otras primitivas, pero esta es la más sencilla para el ejemplo que nos ocupa. Envía directamente el texto por el endpoint. No utiliza el protocolo estándar, es decir, solo envía el mensaje añadiendo como cabecera dos bytes para la longitud del mismo. Es muy útil si queremos evitarnos la sobrecarga y las limitaciones del protocolo. Una cosa que deberíamos comprobar es que el hardware Bluetooth este ‘idle’, pero, para este ejemplo, no nos vamos a complicar más.

Al ejecutar el código, si todo salió bien, podrás leer en /dev/rfcomm0 el mensaje “Hola desde el NXT!” (cierto, no es el “hello world” prometido, pero fué un alarde de inspiración cambiar el mensaje Sticking out tongue). Puedes verlo usando cat, minicom, cutecom o lo que más te guste, mientras tenga permisos de lectura en el dispositivo:

	
  1. cat /dev/rfcomm0 Hola desde el NXT!

No importa la configuración del serie (ya sabes, paridad, bits de stop, velocidad, etc.), ya que se autonegocia en el enlace.

Claro, este ejemplo es demasiado fácil. La comunicación es one-way, no hay respuesta y el NXT es el maestro. Bien podríamos haber escrito un cliente que leyese el saludo y contestase… ¿porqué no? Vamos a ello, pero con algunos cambios.

Algo más complejo: servidor de echo

En este caso, el PC será el maestro (lo que implica que el NXT recibirá los datos por el endpoint 0). Haremos un servidor de echo y, ya que tenemos que ajustarnos al protocolo del NXT, tendremos que hacer un cliente también (que es básicamente implementar la capa de transporte para el protocolo).

Empezamos por el servidor, escrito en NXC.

echo_server.nxc


// * coding: utf-8 *

/* Servidor de ECHO: asume lo siguiente: – NXT como esclavo – Se reciben los datos en el MAILBOX 0 de entrada (0) – Se escriben los datos en el MAILBOX 0 de salida (10)
*/

#define INBOX 0
#define OUTBOX 10
#define BTCNN 0

// Espera hasta que el BT esté disponible
void BTWaitReady(int con){ until(BluetoothStatus(con) == NO_ERR);
}

// Lee del mbox especificado el contenido
int BTReadMsg(byte con, byte inbox, string &data){ int retval;

BTWaitReady(con); retval = ReceiveMessage(inbox, true, data); BTWaitReady(con); return retval; }

// Escribe en el mbox el mensaje especificado
void BTWriteMsg(byte con, byte outbox, string msg){

BTWaitReady(con); SendMessage(outbox, msg); BTWaitReady(con); }

task main(){

// Para el log de eventos TextOut(0, LCD_LINE1, “Servidor de echo”); string data; while(true){ data = “\0”; BTReadMsg(BTCNN, INBOX, data); if (StrLen(data) > 0){ BTWriteMsg(BTCNN, OUTBOX, data); } } }

Este también lo puedes compilar y subir al NXT Laughing out loud

Como puedes observar, según BTWaitReady(), no se hace nada hasta que la conexión esté disponible. Una vez que se establece, se usa ReceiveMessage() que lee el mensaje disponible (si lo hay) en el mbox dado y lo guarda en una cadena. Este mensaje ya está desencapsulado, es decir, no contiene los campos del paquete, es el mensaje tal cual.

Cuando se desea escribir, se usa SendMessage(). Esta primitiva sólo pone el mensaje en el mailbox especificado, ni más ni menos. El sistema espera entonces a que se envíe el direct command de lectura (Message Read) para enviar el contenido del mbox al maestro.

Ahora toca la parte del cliente:

echo_client.c


/* * coding: utf-8 * */

/* Cliente de ECHO: asume lo siguiente: – PC como maestro – Se envían los datos al MAILBOX 0
*/

#include
#include
#include

#include
#include

/* Crea un socket como maestro en la dirección especificada. */
int BTConnect(char *dest){ int sfd; struct sockaddr_rc addr={0};

sfd = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM); addr.rc_family = AF_BLUETOOTH; addr.rc_channel = (uint8_t) 1; str2ba(dest, &addr.rc_bdaddr); if (connect(sfd, (struct sockaddr *)&addr, sizeof(addr)) < 0){ perror(“ERROR”); close(sfd); return -1; } printf(“Connectado\n”); return sfd; }

/* Envía una cadena de texto ‘msg’, de ‘len’ bytes usando el socket ‘sfd’ al * mailbox especificado por ‘inbox’ */
int BTSendMSG(int sfd, char *msg, int len, int inbox){

/* Máximo número de caracteres a enviar (con ‘\0’ incluido) */
#define MAX 64

if (len > MAX) { printf(“ERROR: el mensaje excede el tamaño máximo, %d bytes!”, MAX); return -1; } /* LSB, MSB, CMD_TYPE, CMD, INBOX, MSG_LEN, MSG */ unsigned char cmd[MAX] = {len+5, 0×0, 0×0, 0×9, inbox, len+1}; int i, r; for (i=0; i<=len; i++) cmd[i+6] = *(msg+i); /* Escribe los datos en el socket */ len += 7; if (write(sfd, cmd, len) < len){ perror(“ERROR enviando msg”); return -1; } /* Si se requiere respuesta, leerla */ if (cmd2 == 0×00){ char resp5; if (read(sfd, resp, 5) < 5){ perror(“ERROR recibiendo resp”); return -1; } if (resp4 != 0×00){ switch((unsigned char)resp4){ case 0xEC: fprintf(stderr, “ERROR, el servidor un está activo\n”, (unsigned char)resp4); break; default: fprintf(stderr, “ERROR %x en el dispositivo\n”, (unsigned char)resp4); } return -1; } } return 0; }

/* Lee una cadena de texto del mbox especificado por ‘outbox’, usando el * socket ‘sfd’ y la escribe en ‘ret’ */
int BTRecvMSG(int sfd, char *ret, int outbox){

/* La respuesta siempre es de 66 bytes */
#define RES_SIZE 66 /* 2 B para tamaño, 64 B de respuesta */

/* LSB, MSB, CMD_TYPE, CMD, OUTBOX, INBOX, REMOVE */ char cmd[] = {0×5, 0×0, 0×0, 0×13, outbox, 0×0, 0×1}; char recv[RES_SIZE]; /* Escribe los datos en el socket */ if (write(sfd, cmd, 7) < 7){ perror(“ERROR enviando msg”); return -1; } /* Lee la respuesta, que son siempre 64 bytes */ if (read(sfd, recv, RES_SIZE) < RES_SIZE){ perror(“ERROR recibiendo resp”); return -1; } strncpy(ret, recv+7, (int)recv6); return 0; }

/* Rutina principal del programa */
int main(int argc, char **argv){

if (argc < 3){ printf(“ERROR: Faltan argumentos.\n”); printf(“Uso: %s BT_ADDR \“MSG\”\n”, argv0); return -1; } /* argv1 = DIR NXT */ char *addr = argv1; /* Destino del mensaje recibido, del tamaño máximo permitido */ char recv60; /* Realmente, son 58 */ /* Socket de la conexión */ int sfd = BTConnect(addr); if (sfd < 0) return -1; /* Enviamos el mensaje */ if (BTSendMSG(sfd, argv2, strlen(argv2), 0) < 0) return -1; /* Y lo recibimos */ BTRecvMSG(sfd, recv, 10); /* Y lo imprimimos */ printf(“RECV: ‘%s’\n”, recv); }

Aquí tenemos varias funciones, hechas ad-hoc para el ejemplo (y por supuesto mejorables). Algunas cosas están sacadas del código del NXTRC (GPL), así, si te animas…

La primera función que vemos, BTConnect() nos crea un socket a la dirección especificada. Con esto, ya podemos enviar y recibir datos.

Las dos funciones siguientes se encargan de leer y escribir los mensajes. Están comentadas y además el código es sencillo de entender. Se crean las cabeceras de los mensajes como arrays de caracteres y se escriben en el socket con write(). En el caso de escribir un mensaje, esperamos una respuesta para comprobar si todo ha ido bien.

Al recibir un mensaje, debemos leer la respuesta porque va contenido en esta.

Para compilarlo, lo mejor es usar GCC Eye-wink

$ gcc echo_client.c -o echo_client -lbluetooth

Para probarlo, recuerda lanzar primero el servidor en el NXT, o te dirá que lo hagas.

Últimos apuntes

Por supuesto, el código es mejorable en muchos aspectos (feel free Laughing out loud). Leer los mensajes de respuesta es buena idea si la latencia no es muy importante, ya que el cambio de modo en el core Bluetooth para pasar de leer a escribir es caro (unos 60 ms).

Otro detalle interesante es que la limitación de 64 bytes es sólo para los “Direct Commands”, es decir, podemos enviar mensajes más largos si usamos comandos de sistema, pero debemos tener en cuenta que cuando superan dicho límite, el core de Bluetooth los guarda en Flash.

Nada más, simplemente disfruta y comenta si tienes cosas interesantes que decir.

Referencias

Comentarios

Opciones de visualización de comentarios

Seleccione la forma que prefiera para mostrar los comentarios y haga clic en «Guardar las opciones» para activar los cambios.

Error en codigo C

El codigo C no compila, presenta varios problemas al compilarlo con GCC, sin entrar en mucho detalle se puede notar que los #include estan incompletos, y muchas variables no estan inicializadas correctamente o simplemente se intentar usar sin inicializacion.