Nuevo Driver Dynamixel con un Teensy 3.2

Reemplacé my prototipo TTL inicial con un Teensy 3.2, éste es una placa de desarrollo con una CPU ARM de 32 bits y 72MHz en un formato pequeño de 35×18 mm.

Odroid-XU4, Teensy 3.2 and Servo MX-28 Setup
Setup de Odroid-XU4, Teensy 3.2 y Servo MX-28

Seleccioné esta placa porque tiene varias interfaces seriales (UART) que soportan:

  • 1 y 3 Mbps, velocidades que pueden ser usados con el servo MX-28.
  • Un pin de dirección por hardware. Su RTS por hardware puede indicar cuándo la UART está transmitiendo (al configurar que el RTS sea generado por el transmisor de la UART en vez del receptor de la UART), lamentablemente el Odroid-XU4 (como varias otras plataformas) no soporta esta opción por lo que he visto de la documentación disponible.

También el sitio web del Teensy se ve bastante bueno.

Robotis documenta este setup para conectar una UART al bus Dynamixel. Requiere una UART, un pin de dirección y lógica de 5V:

Robotis Citcuit Interface to Dynamixel Bus
Interfaz de Robotis para Bus Dynamixel

Así, al poner el Teensy 3.2 entre el Odroid-XU4 y el bus Dynamixel, puedo generar el pin de dirección por hardware en vez de una implementación por software que puede tener problemas de timing.

Este es un esquema simplificado del setup, cambié el buffer de recepción por una puerta OR para evitar colocar una resistencia pull-up:

Odroid-XU4, Teensy 3.2, Dynamixel Schematic
Esquemático Odroid-XU4, Teensy 3.2, Dynamixel

Este esquemático usa sólo una UART del Teensy. La función del Teensy es sólo retransmitir al bus Dynamixel y generar el pin de dirección. Mientras tando la señal de retorno del Bus va directo al Odroid, no hay necesidad de pasarla por el Teensy. De esta forma, este setup se puede usar con otros micro-controladores que sólo cuentan con 1 UART, además que no hay retardo extra en el retorno. El Odroid provee 1.8V y 5V que alimentan al Teensy y a los level shifters y el Teensy provee 3.3V que también alimenta a los level shifters.

Probablemente mi setup final use 2 UARTs del Teensy, de forma que éste pueda generar un retorno al Odroid y participe como otro dispositivo en el bus Dynamixel (siguiendo su protocolo) de forma que cumpla alguna función como PWM o I/O análogo. Dependerá de si sobra suficiente tiempo libre en el bus para agregar más comandos, que ya está bien limitado con los 8ms del ciclo de control del software de DARwIn-OP.

La Documentación para la UART del sitio de Teensy es bien clara y se programa con un add-on IDE de Arduino llamado Teensyduino.

El siguiente programa (que presenta problemas como se menciona luego) se deriva fácilmente de la documentación para retransmitir a través de la interfaz serial y con un pin de dirección:

void setup() 
{
  Serial1.begin(1000000);
  Serial1.transmitterEnable(6);
}
void loop() 
{
  if (Serial1.available()) 
  {
    uint8_t c = Serial1.read();
    Serial1.write(c);
  }
}

En este ejemplo, el pin 6 es configurado como pin de dirección para señalar cuándo hay una transmisión en proceso.

En el lado del Odroid-XU4, se puede usar la librería estándar de Dynamixel, sólo se necesita cambiar el nombre del dispositivo serial que corresponde a /dev/ttySAC0 para la UART del conector 10 expansión del Odroid.

Esta configuración con un servo MX-28 a 1Mbps funciona. Pero 2 cosas no funcionan como planificado:

  • Hay un retardo de como 5 bytes (50 us) en la retransmisión. Yo estaba esperando sobre 1 byte pero no tanto.
  • El pin de dirección 6 no funciona correctamente siempre (esto no se notó incialmente).

Retardo de Retransmisión

Teensy Retransmission Delay
Retardo de Retransmisión en Teensy

La Documentación de la CPU MK20DX256VLH7 del Teensy describe en el capítulo 47 la interfaz UART. En la sección 47.3.21 describe el registro UART_RWFIFO que configura el umbral del buffer de recepción para notificar a la CPU vía interrupción, su valor es 1 después del reset.

Al revisar el código fuente de la librería Serial1.begin(), se ve que este umbral UART_RWFIFO es incremendado a 4. Esto permite bajar el uso de CPU al recibir data, pero agrega latencia. También, la librería maneja la UART vía interrupciones así que la CPU sabe que llega data sólo después de la recepción de los primeros 4 bytes (si se hubieran transmitido menos de 4 bytes también se notifica a la CPU por una interrupción de inactividad). La función Serial1.available() no interroga la UART, sino que revisa unos buffers en memoria que son llenados vía interrupciones.

Como estoy usando el Teensy sólo para retransmitir, bajé el umbral de vuelta a 1 byte modificando la función setup así:

void setup() 
{
  Serial1.begin(1000000);
  Serial1.transmitterEnable(6);

  // set receiver buffer threshold for interrupt back to 1.
  uint8_t c2 = UART0_C2;
  UART0_C2 = c2 & ~UART_C2_RE; // disable C2[RE] (receiver enable)
  UART0_RWFIFO = 1;            // set receiver threshold
  UART0_C2 = c2;               // restore C2[RE]
}
void loop() 
{
  if (Serial1.available())
  {
    uint8_t c = Serial1.read();
    Serial1.write(c);
  }
}

UART0_C2 y UART0_RWFIFO apuntan a los registros hardware de configuración y están definidos en las librerías de Teensy. La UART #0 de la CPU corresponde al objeto C++ Serial1.

Pin de Dirección con Falla

Después de hacer un test de estrés, 1 de cada 30 a 100 comandos al servo resultaban en un timeout esperando respuesta del servo. Después de varios intentos, logré capturar algunos casos de falla en la señal del pin de dirección:

Direction Pin Failure
Falla 1 Pin de Dirección – El servo responde, pero hay un byte de retorno inicial extra.
Direction Pin Failure 2
Falla 2 Pin de Dirección – El servo recibe el byte #4 corrupto y no responde.

En los diagramas de captura digital, las señales son:

  1. TX del Odroid-XU4
  2. TX de retransmisión del Teensy 3.2
  3. Pin de dirección del Teensy 3.2
  4. Bus Dynamixel
  5. RX de vuelta al Odroid-XU4

Normalmente, el pin de dirección funciona bien pero a veces se desactiva durante la transmisión. La falla 1 muestra un caso que no afecta al mensaje al servo, pero el Odroid recibe un byte 0xFF extra inicial. La falla 2 muestra un mensaje que se corrompe, el 4o byte en el bus dinamixel vale 63 pero debería valer 5.

Durante mi chequeo previo de la librería de Serial1, noté que la funcionalidad de transmitterEnable es implementada por software y no está usando la capacidad del RTS de hardware de la UART. Del análisis de señal se deduce que esta implementación por software no es correcta, como me interesa una solución por hardware, no intenté arreglar la librería, pero al menos noté un caso de carrera crítica no manejada apropiadamente.

Así que luego de revisar nuevamente la Documentación de la CPU ARM MK20DX256VLH7, encontré la solución por hardware en la sección 47.3.14, el registro de configuración UART_MODEM describe cómo configurar el RTS para indicar cuándo el transmisor de la UART está activo.

También, en este otro Documento ARM K20 se describe las múltiples configuraciones de pines de hardware. En el capítulo 8.1, lista cómo las diferentes señales internas de la CPU se pueden exponer en los pines externos. En particular, los pines de la CPU 25, 37 y 61 pueden ser configurados como RTS de la UART 0. Estos son pines de CPU y no de la placa Teensy. Este esquemático muestra que sólo 2 pines están disponibles en la placa: el pin 6 (corresponde al pin 61 de la CPU) y el pin 19 (pin 37 de la CPU). Después de buscar un poco más encontré ejemplos de código de cómo programar la configuración de un pin, en particular, el pin 6 para RTS (funcionalidad ALT3).

Esta es la versión final del setup que usa un pin de dirección por hardware, la llamada a la API Serial1.transmitterEnable() fue eliminada.

#define UART_TXRTSE (2)
#define UART_TXRTSPOL (4)

void setup() 
{
  Serial1.begin(1000000);
  
  // set receiver buffer threshold for interrupt back to 1.
  uint8_t c2 = UART0_C2;
  UART0_C2 = c2 & ~UART_C2_RE; // disable C2[RE] (receiver enable)
  UART0_RWFIFO = 1;            // set receiver threshold
  UART0_C2 = c2;               // restore C2[RE]

  // enable PIN 6 as hardware transmitter RTS with active HIGH.
  CORE_PIN6_CONFIG = PORT_PCR_MUX(3);
  UART0_MODEM = UART_TXRTSE | UART_TXRTSPOL; 
}
void loop() 
{
  if (Serial1.available())
  {
    uint8_t c = Serial1.read();
    Serial1.write(c);
  }
}

El pin de dirección funciona correctamente y tiene mejor consistiencia en los timings:

Teensy Driver Working
Driver Teensy funcionando bien

Ahora me falta reducir este prototipo a una placa debajo del Teensy 3.2.

2 comentarios en “Nuevo Driver Dynamixel con un Teensy 3.2

  1. Muy interesante, hace tiempo que leo este blog!! aparte de felicitarte por el trabajo que realizas, quiero hacer una pregunta, como a sido tu experiencia con el odroid? y porque decidiste comprar este dispositivo en vez de un raspberry pi 2 o el 3 ? y lo ultimo cuanto tiempo demoro la llegada de la placa odroid? hace tiempo quiero comprar una, un saludos y nuevamente felicitaciones por tu trabajo !!!

    1. Gracias,

      Las placas Odroids las ocupo desde hace varios años, en ese tiempo la Raspberry era sólo la primera versión. El Raspberry 2 es recién del 2015 y el Raspberry 3 que es más poderoso es sólo de hace 6 meses.

      En su momento descarté las Raspberry porque tienen bajo rendimiento. Las placas Odroid permiten correr un Ubuntu como si fuera un desktop, yo trabajo directo en la placa vía SSH normalmente.

      El Raspberry 3 se ve mejor, pero el Odroid-XU4 tiene 2 GB de Ram, es más rápido y tiene puerto USB 3 que quiero usar para una camara web sin compresión.

      Odroid es fabricado por Hardkernel, que se caracteriza en sacar frecuentemente placas nuevas y también las va dejando obsoletas más rápido (comparado con Raspberry). Por ello tiene placas con más capacidad. Cuando recién sale una placa nueva, hay que esperar un poco que estabilicen el Kernel para tener una distribución estable de Ubuntu, pero el fabricante responde bien. Hasta tienen un Magazine en inglés y español.

      Odroid no tiene el soporte en Internet que tiene Raspberry, pero muchas soluciones que uno encuentra para Raspberry funciona en Odroid porque al final corren Linux.

      Para Chile, hay que comprar en el representante ameridroid.com que está en USA, solían enviar sólo por Fedex con lo cual las placas llegaban en 1 semana, pero Fedex es caro. La última vez que compré ya tenían la opción de correo normal y llegaron por Correos de Chile después de como 1 mes de estar pegado en la aduana, ahí el tema depende de la aduana.

      Para mi las placas Odroid han salido super buenas, tengo de como 6 modelos distintos. Te recomiendo que le compres un módulo eMMC, porque son mucho más rápidos que un SD. Con un SD el sistema operativo anda a tirones, por lo que es útil para pruebas no más.

Leave a Reply

Your email address will not be published. Required fields are marked *

*