Adquiriendo señal…
Adquiriendo señal…
Lección 11 de 12· Control y estimación
0 de 12 lecciones · 0 %
Hasta ahora todo el lazo de la Estación vive en un PC: pid_controller corre a 20 Hz como un nodo de Python, mount_sim hace de montura y el reloj en lockstep garantiza que nada llega tarde. Una montura real no funciona así. El motor necesita un mando nuevo cada milisegundo, el encoder hay que leerlo con un periférico de conteo, el paro de emergencia tiene que actuar en microsegundos aunque el PC se cuelgue, y el reloj ya no se puede detener: el eje sigue girando. Esta lección baja el lazo a un ESP32 con micro-ROS, siguiendo firmware/esp32_eje/README.md, el paquete src/atp_mcu, el documento de requisitos docs/HARDWARE.md y el puente rotctld de src/atp_hw.
El PID y el anti-windup son los del curso Sistemas APT; el encoder como fuente de ruido de cuantización es el de Señales y Sistemas. Lo nuevo es el oficio: dónde corre cada cosa, cuánto tarda, qué pasa cuando se corta un cable.
Con la honestidad del repositorio. El firmware compila en Docker, su lógica está probada en el host contra la planta de
atp_rosy el mismo nodo micro-ROS corre sobre Linux con un agente y nodos ROS 2 normales. Nunca se ha cargado en una placa. Todo lo que dependa de la placa, los periféricos o la mecánica está marcado SIN PROBAR o SUPUESTO, y así lo citamos.
Un ESP32 no puede correr DDS. micro-ROS pone en el microcontrolador la biblioteca cliente rclc (C, memoria estática) sobre el protocolo XRCE-DDS, y en el PC un agente (microros/micro-ros-agent:jazzy) que traduce a DDS de verdad. Para el resto del grafo, el ESP32 es un nodo más. El transporte es serie (UART a 921 600 baudios, 8N1) o UDP por Wi-Fi (puerto 8888), y el ROS_DOMAIN_ID (42 por defecto) debe coincidir en firmware, agente y Estación.
El firmware tiene dos tareas de FreeRTOS, ancladas a núcleos distintos:
configMAX_PRIORITIES − 2), despertada cada milisegundo por vTaskDelayUntil: encoder, eje_paso(), PWM;Entre ambas tareas hay buzones con secciones críticas de ~1 µs. La regla es que nada del lado de la red pueda retrasar al lazo.
/* firmware/esp32_eje/main/tarea_lazo.c (recortado) */
for (;;) {
vTaskDelayUntil(&despertar, 1); /* 1 tick = 1 ms (CONFIG_FREERTOS_HZ=1000) */
const int64_t t0 = esp_timer_get_time();
const int32_t periodo = (int32_t)(t0 - t_prev);
if (periodo > 1500) perdidos++; /* se paso medio periodo del plazo */
/* ... leer encoder, eje_paso(), motor_aplicar(), telemetria ... */
const uint32_t t_lazo_us = (uint32_t)(esp_timer_get_time() - t0);
}
xTaskCreatePinnedToCore(tarea, "lazo_1khz", 4096, NULL, configMAX_PRIORITIES - 2, NULL, 1);
La interfaz respeta el contrato de la Estación: el ESP32 suscribe /atp/reference (sensor_msgs/JointState, el mismo de siempre) y publica en /atp/mcu/<eje>/joint_states (reliable, 50 Hz) y ~/diag (best effort, 2 Hz, con t_lazo_us, jitter_max_us y ticks_perdidos). Como el contrato pide un único /joint_states con name=[az, el] y dos placas no pueden compartirlo, atp_mcu/mcu_merge los une, y si un eje lleva más de 0.5 s sin dato deja de publicar en vez de inventar.
¿Por qué 921 600 baudios? Una telemetría de ~100 B a 50 Hz con 10 bits por byte (8N1) son bit/s: el 5.4 % de 921 600 baudios, pero el 43 % de 115 200.
cd firmware/esp32_eje && ./build.sh # variante serie -> build/atp_esp32_eje.bin
./agente.sh serie /dev/ttyUSB0 921600 # agente micro-ROS en Docker
ros2 topic pub --once /atp/mcu/az/cmd/enable std_msgs/msg/Bool "{data: true}" # armar
eje_pasoLa lógica del lazo vive en core/, en C puro sin ESP-IDF ni ROS: la misma fuente corre en el chip, en las pruebas del host y en el nodo de Linux. El PID es pid_controller.py línea a línea:
/* firmware/esp32_eje/core/eje_lazo.c */
float eje_pid_paso(eje_pid_t *p, float e, float ff)
{
const float d_bruta = (e - p->e_prev) / p->dt;
const float alfa = p->dt / (p->tau_d + p->dt);
p->d_filt += alfa * (d_bruta - p->d_filt);
const float u_raw = p->kp * e + p->ki * p->integral + p->kd * p->d_filt + ff;
const float u = eje_limitar(u_raw, -p->u_max, p->u_max);
const int sat = fabsf(u_raw) > p->u_max + 1e-12f;
const int congelar = sat && (u_raw * e > 0.0f); /* anti-windup por clamping */
if (!congelar) p->integral += e * p->dt;
p->e_prev = e;
return u;
}
Con las ganancias de atp_ros (kp 25, ki 2, kd 8, = 80 ms, = 8 N·m), que son de la planta didáctica de kg·m²: con la montura real hay que identificar y volver a sintonizar. Entre la referencia y el PID hay una etapa que el lockstep no necesitaba: la referencia llega a 20 Hz con saltos, así que el lazo la extrapola con su velocidad, la recorta a los límites de software, le pone un tope de velocidad de 12 °/s (supuesto: por encima de los 9 °/s del pase N2) y frena con el perfil al acercarse a un límite.
La validación es por vectores: gen_vectores.py corre la planta y el PID de atp_ros en float64 y el C (PID en float) corre los mismos escenarios, fila a fila. En la rampa de 9 °/s la mayor diferencia de posición es 1.7e-9 rad; en el escalón de 90° saturado, 1.3e-7 rad: el límite de la aritmética de 32 bits.
Anti-windup. Mientras el par está saturado, el error no se reduce por mucho que crezca la integral, y lo que se acumula hay que «desacumularlo» después con sobrepaso. El clamping congela la integral cuando el mando satura y el error empuja hacia la saturación (). En el escalón de 90° la integral llega a 0.20 rad·s con clamping y a 1.02 sin él (test_lazo.c); el bloque ejecutable de abajo reproduce 0.20 frente a 0.98, con el sobrepaso pasando de 4.3° a 8.4°.
Un encoder de bits divide la vuelta en cuentas. La resolución es
El error de redondeo queda en . Si el eje se mueve lo bastante (muchas cuentas entre muestras, sin correlación con la rejilla), es uniforme en ese intervalo, con densidad . Su media es cero por simetría y su varianza
así que el ruido de cuantización rms es . Con 14 bits: . Un incremental de 10 000 pulsos por vuelta contado en cuadratura ×4 da 40 000 cuentas (el valor por defecto de cuentas_por_vuelta en core/eje_lazo.c): y .
Lo que cuesta de verdad no es la posición sino la velocidad: derivar un ruido de con un filtro de constante a periodo da una cota (HARDWARE.md §2.4). Con 14 bits, = 1 ms y el filtro de velocidad de 20 ms: °/s. En la derivada del PID ( = 80 ms) son 0.71 °/s, que por da unos 0.1 N·m de ruido de par: el 1 % del tope. Con 12 bits sería 4 veces más; de ahí que HARDWARE.md pida 12 bits como mínimo y recomiende 14 para el banco (escenario A), y 17 o más para el plato de 3 m en X (escenario C). Advertencia del propio documento: con 14 bits la cuantización ya no manda; mandan la exactitud absoluta del encoder (INL), la holgura y la nivelación, y eso lo corrige el modelo de apuntamiento de la lección 10.
El ESP32 corre a 240 MHz: 1 ms son 240 000 ciclos. eje_paso ocupa 1 215 B de código sin bucles (~400 instrucciones) más ~7 divisiones y una raíz por software (la FPU del ESP32 no divide ni saca raíces): una cota gruesa de ciclos, es decir µs, el 1.25 % del periodo. Sumando encoder, PWM, telemetría y el ADC cada 10 ticks, la cota es µs (4 %); con dos ejes, µs. Son estimaciones por tamaño de código, no mediciones: el firmware mide cada tick con esp_timer y lo publica en ~/diag, y eso es lo primero que hay que leer el día del encendido.
La seguridad es del firmware, no de ROS, y cada regla tiene su prueba en test/test_lazo.c:
| Estado | Driver | Significado |
|---|---|---|
| DESARMADO | apagado | espera cmd/enable=true; no se arma solo |
| ESPERA | apagado | armado, sin referencia fresca |
| ACTIVO | PID | sigue la referencia |
| RETENCION | PID | perdió la referencia: mantiene la posición 2 s |
| FALLA | apagado, enclavada | ESTOP, LIMITE, VEL, ATASCO, ENCODER, CORR o CFG; solo se quita rearmando |
/* firmware/esp32_eje/core/eje_lazo.c, maquina de estados (recortado) */
case EJE_ACTIVO:
if (e->ref_edad_s > c->t_watchdog_s) { /* 0.5 s: perdio la referencia */
e->estado = EJE_RETENCION;
e->t_retencion = 0;
}
break;
case EJE_RETENCION:
if (e->ref_edad_s <= c->t_watchdog_s) {
e->estado = EJE_ACTIVO; /* volvio: retoma desde donde esta */
} else if ((float)(++e->t_retencion) * dt >= c->t_retencion_s) {
e->estado = EJE_ESPERA; /* 2 s sin referencia: motor libre */
eje_pid_reiniciar(&e->pid, 0.0f);
apagar(e, out);
return;
}
break;
Ejemplo numérico. La referencia llega a 20 Hz; la última llega en y la siguiente, que debía llegar en ms, no llega (se cortó el cable). El watchdog dispara cuando la edad supera 500 ms, en el tick ms, es decir 451 ms después de la referencia que faltó; la retención dura 2 s más y el driver se apaga a los 2.45 s. Es exactamente lo que mide test_watchdog_* (RETENCION a 452 ms, driver apagado a 2.45 s), y lo que reproduce el bloque ejecutable. En la prueba sobre Linux cargado, la misma transición tardó 0.97 s: el host no es de tiempo real.
El paro de emergencia es un contacto normalmente cerrado (un cable cortado para igual que un pulsador) con una ISR que baja EN en microsegundos; el lazo lo lee cada tick y enclava FALLA. El README insiste: el paro de software no sustituye a un contactor que corte la potencia sin pasar por el ESP32, y nada de esto está certificado como función de seguridad.
prueba_hil_lockstep.sh sustituye mount_sim y pid_controller por el nodo micro-ROS del firmware compilado para Linux, con la planta J/b en lugar de motor y encoder, y corre el pase N2 con los nodos de siempre. El puente nuevo atp_mcu/mcu_lockstep publica /joint_states(t_k) con el estado del MCU interpolado en el instante de pared y confirma /atp/step_ack(t_k) cuando llega el eco de la referencia. El MCU gira en tiempo real: el lockstep garantiza que cada estado y cada referencia llevan el mismo sello, pero ya no que el resultado sea independiente de la carga.
| Qué (una corrida, host con carga 5.6 → 10.1) | Valor |
|---|---|
| RTT referencia → eco del MCU | p50 0.94 ms, p95 19.4 ms, máx 28.0 ms |
| jitter del tick de 1 ms en Linux | p99 2 287 µs, máx 17 400 µs; 4.7 % de ticks > 1.5 ms |
| deriva contra el gemelo digital | p50 0.0039°, rms 0.038°, máx 0.27° |
| error de apuntamiento | HIL 0.042° rms (95.0 % dentro de 0.1°); gemelo 0.0164° rms |
Lectura: el HIL sigue al gemelo a cuatro milésimas en la mediana; la cola la ponen los tirones del planificador de Linux, no el protocolo. «Cuando llegue la placa, cambia el transporte y nada más»: el mismo ros2 launch atp_mcu mcu_hil_lockstep.launch.py con el agente serie.
La otra vía al hardware es el rotor comercial (Yaesu G-5500, ~6 °/s): atp_hw/rotctld_bridge manda posiciones al demonio rotctld de Hamlib y lee la posición real; el lazo lo cierra el controlador del rotor. Ahí time_scale vale 1 sin opción, y el puente detiene el rotor si /clock se aparta más de un 10 % del reloj de pared.
El cable de datos se corta en pleno pase con el eje ACTIVO. ¿Qué secuencia sigue el firmware?
El bloque traduce eje_pid_paso a Python, integra la planta a 1 kHz, y reproduce tres cifras de la lección: el anti-windup en el escalón de 90°, la resolución y el ruido de un encoder de 14 bits, y la secuencia del watchdog cuando la referencia de 20 Hz se corta.
Busca en la salida: integral máxima 0.20 rad·s con anti-windup y 0.98 sin él; = 0.02197° y un rms de redondeo medido de 0.00634°, igual a ; RETENCION 451 ms después de la referencia que faltó y driver apagado a los 2 451 ms; y el presupuesto de 12.5 µs (1.2 %) y 40 µs (4 %). Cambia BITS a 16: = 0.00549°.
El laboratorio Lazo ESP32 arranca en «Banco HIL: rampa» con kp/ki/kd 25/2/8 y encoder de 14 bits: la lectura de resolución debe marcar 0.02197° y la de cómputo del lazo unos 12 µs de 1000 µs. Pulsa «Cortar enlace» y mira la máquina de estados: ACTIVO se mantiene medio segundo, pasa a RETENCION (la posición se congela, el PWM sigue vivo) y dos segundos después a ESPERA con el PWM a cero; «Restablecer» lo devuelve a ACTIVO sin salto. Carga «Escalón 90° saturado» y compara el sobrepaso con el anti-windup encendido y apagado. Baja los bits del encoder a 10: pasa a 0.35° y el PWM empieza a vibrar por el ruido de la derivada. Pulsa «Paro»: el driver se apaga en el acto y el estado queda en FALLA hasta «Rearmar».
SCHED_FIFO; sus cifras de jitter son de un host cargado, no del ESP32.Cierre de la lección
Sigue el recorrido de la lección paso a paso.
Con una cuenta desbloqueas el cuestionario, el foro de dudas y el tutor con IA de esta lección, además del registro de progreso y el certificado verificable al terminar «ROS 2 aplicado: la Estación ATP de punta a punta».