Proyecto: integración de navegación, evasión y estados

01 · Alcance

De módulos a comportamiento

El programa del tema 46 seguía la línea con un control proporcional sencillo y mandaba órdenes directas a los motores, sin medir si las ruedas las cumplían. Era una prueba de integración mínima. Este tema agrega lo que falta para cumplir los requisitos F1 a F5 del tema 45:

• Un PID de línea que mantiene el desvío bajo 15 mm incluso en las curvas.
• Lazos de velocidad en cada rueda, que hacen que el robot ande a la velocidad pedida aunque un motor sea más débil que el otro.
• Odometría para medir giros y avances durante la maniobra.
• Una máquina de estados con subestados (tema 38) que rodea la caja y vuelve a la línea.

Todo el código de este tema se ejecutó dentro de un simulador físico en la computadora: la pista de 6,4 m, la caja de 10 × 10 cm, un motor con zona muerta y un 7 % más débil de un lado, encoders por interrupción, sensores de línea calculados desde la geometría y un ultrasónico por rayos. Los resultados del texto salen de esas ejecuciones. El simulador no reemplaza al robot, pero encontró un fallo de seguridad que habría costado encontrar a mano (sección 08).

02 · Dos lazos, uno dentro del otro

Control en cascada

Seguir una línea es un problema de posición lateral, y mover las ruedas es un problema de velocidad. Mezclarlos en un solo lazo hace que el ajuste de uno estropee al otro. La solución habitual es una cascada: el lazo externo decide qué velocidad debe tener cada rueda, y los lazos internos se ocupan de que la rueda la tenga.

Los dos niveles trabajan a escalas de tiempo distintas, y por eso se separan bien. El lazo de rueda es rápido: la velocidad de una rueda responde en décimas de segundo. El de línea es más lento: la posición lateral cambia cuando el robot gira, y gira cuando las ruedas cambian de velocidad. Regla de cascada: el lazo interno debe ser claramente más rápido que el externo. Si no, el externo ve un sistema que cambia mientras lo ajusta.

La interfaz entre ambos es una sola cosa: la velocidad pedida a cada rueda, en cm/s. El lazo de línea no sabe nada de PWM ni de motores; el de rueda no sabe nada de la pista. Esto también facilita reemplazar uno sin tocar al otro, y es lo que permite que las maniobras de evasión (que no usan la línea) pidan velocidades a las mismas ruedas.

03 · El lazo interno

El lazo de velocidad de rueda

En el tema 46 el motor recibía una orden y producía una velocidad cualquiera. Con dos motores que nunca son idénticos, el robot se desvía de la recta aunque ambas órdenes sean iguales. El lazo de rueda corrige eso con dos aportes sumados:

• Prealimentación (feedforward): la orden que, con un motor ideal, produce la velocidad pedida. Con la zona muerta compensada por PWM_MIN (tema 46), la relación entre orden y velocidad es casi lineal, y KFF = 0,021 es la pendiente: para 30 cm/s, u = 0,63 (PWM 183). Da una respuesta rápida desde el primer instante.
• Un PI sobre la diferencia entre la velocidad pedida y la medida. Corrige lo que la prealimentación no sabe: un motor un 7 % más débil, la batería que se agota, el piso.

Antes de aplicar la velocidad, la referencia pasa por una rampa de aceleración máxima (1,0 m/s², el valor del frenado del tema 45). Evita pedirle al motor un salto que no puede cumplir y saturar el integrador. Con la rampa, un escalón de 0 a 30 cm/s tarda 0,3 s.

// control.h - Regulador PID genérico (tema 43) y lazos de velocidad de las dos ruedas.
#ifndef CONTROL_H
#define CONTROL_H

// PID discreto: derivada sobre la medida, salida limitada e integración condicional como anti-windup.
struct PID {
  float kp, ki, kd, umin, umax;
  float integral = 0, medidaPrev = 0;
  bool primera = true;

  PID(float p, float i, float d, float lo, float hi) : kp(p), ki(i), kd(d), umin(lo), umax(hi) {}
  void reiniciar() { integral = 0; primera = true; }

  float calcular(float ref, float medida, float dt) {
    float e = ref - medida;
    float d = primera ? 0.0 : -(medida - medidaPrev) / dt;
    primera = false;
    medidaPrev = medida;
    float nueva = integral + ki * e * dt;
    float salida = kp * e + nueva + kd * d;
    bool satura = salida > umax || salida < umin;
    if (!satura || (salida > umax && e < 0) || (salida < umin && e > 0)) integral = nueva;
    return constrain(salida, umin, umax);
  }
};

// Lazo externo: de la posición de la línea (mm) sale una diferencia de velocidad (cm/s) entre las ruedas.
// Lazos internos: de la velocidad pedida a cada rueda sale la orden del motor.
PID pidLinea(0.9, 0.3, 0.012, -25, 25);
PID pidRueda[2] = {PID(0.010, 0.10, 0, -1, 1), PID(0.010, 0.10, 0, -1, 1)};

float velPedida[2] = {0, 0};            // lo que pide el comportamiento (cm/s)
float velRampa[2] = {0, 0};             // la misma, con la aceleración limitada

void ruedasPedir(float izq, float der) { velPedida[0] = izq; velPedida[1] = der; }

void ruedasParar() {                    // parada inmediata, para fallas y espera
  velPedida[0] = velPedida[1] = velRampa[0] = velRampa[1] = 0;
  pidRueda[0].reiniciar(); pidRueda[1].reiniciar();
  motorParar();
}

// Se llama cada ciclo de control con las velocidades medidas (cm/s).
void ruedasActualizar(const float medida[2], float dt) {
  for (byte i = 0; i < 2; i++) {
    float paso = ACEL_MAX * dt;
    velRampa[i] += constrain(velPedida[i] - velRampa[i], -paso, paso);
    float u = KFF * velRampa[i] + pidRueda[i].calcular(velRampa[i], medida[i], dt);   // prealimentación + PI
    motorSet(i, u);
  }
}

#endif

Resultado de un escalón de 0 a 30 cm/s en cada rueda, con el motor derecho un 7 % más débil que el izquierdo:

Sin el PI, la rueda izquierda se queda en 29,6 y la derecha en 27,5 cm/s: una diferencia de 2,1 cm/s entre ruedas, que sobre una recta desviaría al robot de forma sostenida (un giro de 2,1 / 15 = 0,14 rad/s, unos 8° por segundo). Con el PI las dos ruedas llegan a 30,0. El precio es un sobreimpulso del 12 al 15 %, aceptable para una rueda que luego sigue una referencia lenta. Ni siquiera la prealimentación sola es mala: aporta la rapidez; el PI aporta la exactitud, los dos papeles que ya vimos en el PID (tema 43).

04 · El lazo externo

El lazo de línea con PID

La posición de la línea (en mm, positiva a la derecha) entra a un PID que produce una diferencia de velocidad Δ en cm/s: la rueda izquierda va a v + Δ y la derecha a v − Δ. Si la línea está a la derecha, Δ es positiva, la rueda izquierda va más rápido y el robot gira hacia la derecha. El signo es la fuente de error más común de esta etapa: si el robot huye de la línea en vez de acercarse, está invertido.

• P (kp = 0,9 cm/s por mm): 10 mm de desvío piden 9 cm/s de diferencia.
• I (ki = 0,3): en una curva sostenida el robot necesita una diferencia constante entre las ruedas; sin integral, el P puro lo logra con un desvío permanente (e_ss, tema 42). La integral lo elimina.
• D (kd = 0,012 s): como la posición se mide con el arreglo 8 cm adelante del eje, la derivada anticipa el giro y amortigua la oscilación.

Dos detalles más. La salida se limita a ±25 cm/s (con anti-windup). Y la velocidad base baja cuando el robot corrige mucho: v = max(12, 30 − 0,5 · |Δ|), para tomar las curvas con más calma. Las ganancias se ajustaron con el método del tema 44: primero el P para la velocidad de reacción, después el D para amortiguar, y por último el I.

El resultado de recorrer dos vueltas del circuito (con la caja), en el simulador:

Lo que enseña la tabla:

• El PID reduce el desvío máximo a menos de la mitad respecto del P solo (3,2 contra 7,0 mm). Los dos cumplen F1 (≤ 15 mm), pero el PID deja margen para el ruido real.
• Sin los lazos de rueda el desvío es mayor (9,0 mm) y, sobre todo, la velocidad media cae a 26,7 cm/s, fuera del rango de F2 (30 ± 10 %, es decir, de 27 a 33 cm/s). Con ellos, 28,8 cm/s. La velocidad es un requisito que se cumple con el lazo de rueda, no con las órdenes directas.
• La caja cuesta unos 9,6 s por vuelta (31,3 contra 21,7 s). Las dos vueltas con caja difieren en 0,3 s: la repetibilidad pedida en C2 era de ±3 s.

05 · Medir lo que se avanza y se gira

Odometría y primitivas de movimiento

Durante la evasión el robot no ve la línea, así que necesita saber cuánto avanzó y cuánto giró. La odometría del tema 27 lo da: a partir de las cuentas de cada rueda, la distancia del centro es el promedio, y el giro es la diferencia dividida por el ancho de vía. Cada ciclo actualiza la pose usando el punto medio del giro, que reduce el error cuando se gira mientras se avanza.

// odometria.h - Pose del robot (x, y en cm; th en rad, + hacia la izquierda) a partir de las cuentas de los encoders (tema 27).
#ifndef ODOMETRIA_H
#define ODOMETRIA_H

float odo_x = 0, odo_y = 0, odo_th = 0;
long odo_previaIzq = 0, odo_previaDer = 0;

void odoActualizar() {
  long ci = encLeer(0), cd = encLeer(1);
  float dI = (ci - odo_previaIzq) * CM_POR_CUENTA;
  float dD = (cd - odo_previaDer) * CM_POR_CUENTA;
  odo_previaIzq = ci; odo_previaDer = cd;
  float d = (dI + dD) / 2.0;
  float dth = (dD - dI) / ANCHO_VIA_CM;
  odo_x += d * cos(odo_th + dth / 2.0);              // punto medio del paso: menos error en los giros
  odo_y += d * sin(odo_th + dth / 2.0);
  odo_th += dth;
}

float angDif(float a, float b) {                     // a - b, llevado a [-PI, PI]
  float d = a - b;
  while (d > PI) d -= 2.0 * PI;
  while (d < -PI) d += 2.0 * PI;
  return d;
}

#endif

La función angDif resuelve un problema que parece menor y no lo es: los ángulos dan la vuelta. Si el robot está en 3,0 rad y el objetivo es −3,0 rad, la resta directa da −6,0, pero el camino más corto es girar +0,283 rad (16°) hacia la izquierda. Llevar la diferencia a [−π, π] asegura que el giro vaya siempre por el lado corto.

Sobre la odometría se construyen dos primitivas reutilizables, que las fases de la evasión combinan:

• girarHacia(th): gira en el lugar (una rueda para cada lado) hacia el rumbo th. La velocidad angular es proporcional al error, con un máximo de 2 rad/s para no pasarse y un mínimo de 0,6 rad/s para vencer la zona muerta. Termina cuando el error es menor a 2°.
• avanzarRecto(th, v): avanza a velocidad v corrigiendo el rumbo a th con una diferencia de velocidad proporcional al error de rumbo.
• avanceFase(): la distancia recorrida desde el comienzo de la fase actual, con la pose de odometría.

La odometría deriva con el tiempo (tema 27), pero una maniobra de 11 s y menos de un metro la usa a muy corto plazo: el error acumulado es pequeño, y el seguimiento de línea corrige lo que quede cuando se vuelve a ver la línea.

06 · Decidir qué hacer

La máquina de estados

La máquina tiene cuatro estados, y uno de ellos, EVITAR, contiene una secuencia de ocho fases:

Qué hace cada fase y por qué:

El total es de 11,3 s, dentro de los 15 s de F4. La fase 7 esconde un detalle de geometría que conviene entender. El arreglo de sensores está 8 cm adelante del eje de las ruedas. Cuando el arreglo ve la línea, el eje todavía está 8 cm antes. Si el robot girara a la izquierda en ese momento, el arreglo describiría un arco y quedaría 8 cm al costado de la línea, sin verla. Por eso la fase 7 avanza los 8 cm restantes antes de girar: hay que alinear el eje con la línea, no el sensor.

La figura es la trayectoria real del centro del robot en el simulador: los puntos blancos marcan el comienzo de cada fase. Se ve la parada antes de la caja, el desplazamiento lateral, el paso, el cruce de la línea y la alineación final. Al volver a SEGUIR el robot queda realmente a 1,5 cm de la línea (el error de la odometría en toda la maniobra): el arreglo la ve, y el PID termina el trabajo.

El programa principal se reduce a un switch sobre la fase, que sólo llama a las primitivas. Los comportamientos son legibles y se pueden cambiar sin tocar el resto: para rodear por la derecha bastan los signos de los giros. Lo que no está en el código es una decisión de diseño importante: la evasión no usa el sensor de línea hasta la fase 7. Mientras se rodea la caja, el robot está a ciegas respecto de la pista, y sólo confía en la odometría.

07 · Los números de la maniobra

Parámetros de la maniobra

Tres distancias definen la trayectoria, y cada una sale de la geometría del robot (18 cm de ancho, 9 cm de frente al eje, 8 cm de cola) y de la caja (10 × 10 cm). Todas están en config.h:

// config.h - Pines, medidas y umbrales del robot. Es el único archivo que se toca al cambiar de hardware.
#ifndef CONFIG_H
#define CONFIG_H

// Pines (presupuesto del tema 45: 18 de 18)
const byte PIN_ENC_IZQ_A = 2, PIN_ENC_DER_A = 3, PIN_ENC_IZQ_B = 4, PIN_ENC_DER_B = 9;
const byte PIN_PWM_IZQ = 5, PIN_PWM_DER = 6, PIN_DIR_IZQ = 7, PIN_DIR_DER = 8;
const byte PIN_TRIG = 10, PIN_ECHO = 11, PIN_BOTON = 12, PIN_LED = 13;
const byte PIN_LINEA[5] = {A0, A1, A2, A3, A4};
const byte PIN_BATERIA = A5;

// Mecánica (tema 27)
const float RADIO_RUEDA_CM = 3.25;
const float ANCHO_VIA_CM = 15.0;
const int CPR = 1440;                                         // cuentas por vuelta de rueda, decodificación 4x
const float CM_POR_CUENTA = 2.0 * PI * RADIO_RUEDA_CM / CPR;  // 0,01418 cm
const float D_ARREGLO_CM = 8.0;                               // distancia del arreglo de línea al eje de las ruedas

// Motores
const byte PWM_MIN = 60;                // PWM al que el motor apenas empieza a girar (medilo, tema 46)
const float ZONA_MUERTA_U = 0.02;       // órdenes menores a esto se tratan como cero
const float KFF = 0.021;                // orden por cm/s de la rueda (pendiente medida: 0,5 de orden da unos 23 cm/s)

// Línea
const byte N_LINEA = 5;
const float X_SENSOR_MM[N_LINEA] = {-20, -10, 0, 10, 20};   // posición de cada sensor respecto del centro
const float UMBRAL_LINEA = 0.3;         // oscuridad total mínima para decir que hay línea

// Batería (divisor 10 kΩ + 10 kΩ)
const float VREF = 5.0, DIVISOR = 2.0;
const float V_BAT_MIN = 6.4;

// Tiempos (ms) y umbrales de seguridad
const unsigned long TS_CONTROL_MS = 20, TS_DISTANCIA_MS = 60, TS_TELEMETRIA_MS = 100, TS_BATERIA_MS = 100;
const unsigned long T_LINEA_PERDIDA_MS = 1000, T_BATERIA_BAJA_MS = 1000, T_EVITAR_MAX_MS = 15000;
const float DIST_OBSTACULO_CM = 25.0;       // umbral de evasión: con 20 cm el margen de frenado queda justo (ver el texto)

// Velocidades y aceleración (cm/s, cm/s²)
const float VEL_NOM = 30.0;             // velocidad nominal en recta (F2)
const float VEL_MIN = 12.0;             // piso de velocidad en curvas
const float VEL_MANIOBRA = 25.0;        // velocidad al rodear el obstáculo
const float VEL_BUSQUEDA = 15.0;        // velocidad al buscar la línea de nuevo
const float ACEL_MAX = 100.0;           // rampa de las referencias de rueda (1,0 m/s², tema 45)

// Maniobra de evasión (cm) - dependen del tamaño del robot y del obstáculo
const float D_LATERAL_CM = 25.0;        // desplazamiento lateral: media caja + medio robot + holgura
const float D_EXTRA_CM = 33.0;          // lo que se avanza después de la distancia de parada para pasar la caja
const float TOL_GIRO_RAD = 0.035;       // 2 grados

#endif

• Desplazamiento lateral D_LATERAL_CM = 25. El costado del robot debe librar la caja: media caja (5) más medio robot (9) son 14 cm; la holgura lateral es D_LATERAL − 14 = 11 cm.
• Avance extra D_EXTRA_CM = 33. El robot parte parado a distParada de la caja y debe avanzar lo suficiente para que, al girar hacia la línea, su costado supere el borde trasero de la caja: distParada + 10 (caja) + 9 (frente) + 9 (medio ancho) es el mínimo, y se agregan 5 cm de holgura: 33 = 28 + 5. Como se suma a la distancia de parada medida, la maniobra se adapta a donde el robot haya frenado.
• Distancia de arreglo D_ARREGLO_CM = 8: la que se explicó en la fase 7.

La holgura mínima del cuerpo con la caja en la maniobra ideal es mín(D_LATERAL − 14, D_EXTRA − 28). Con 25 y 33 son 11 y 5: gana la menor, 5 cm. El simulador, con los errores reales de odometría, dio entre 3,9 y 4,8 cm. Con un desplazamiento lateral de 15 cm la holgura ideal baja a 1 cm, y con un avance extra de 28 cm o menos el robot toca la caja. El laboratorio 2 lo muestra.

El umbral de detección

En el tema 45 el presupuesto de frenado daba un margen de 13,4 cm (11,6 cm con el filtro de mediana del tema 46) detectando a 20 cm. El simulador, que incluye el retardo del motor y la rampa de desaceleración, da menos:

Con el umbral de 20 cm el robot pasaba la prueba de referencia por 3 mm, pero 49 de las 216 variaciones (la caja en otro lugar, el robot torcido, motores distintos, ruido en los sensores) quedaban por debajo de los 10 cm de F3. Con 25 cm, los cinco centímetros extra se traducen casi exactamente en cinco centímetros de margen, y todas las variaciones cumplen. Es un buen ejemplo del valor de la simulación: el cálculo a mano decía que 20 cm alcanzaba, y era cierto en el caso ideal; el margen real exige un umbral más alto. El requisito F3 sigue cumpliéndose («detener con al menos 10 cm»); lo que se movió es el umbral interno que lo garantiza.

08 · Que falle bien

Seguridad integrada

Los estados de seguridad del tema 46 siguen vigentes, pero ahora hay más estados y más formas de fallar. Resultados del simulador, con el programa final:

Dos observaciones. El requisito S1 pide los motores en cero en 100 ms: se cumple (13 ms), pero el robot sigue rodando 3,5 cm por su inercia, y eso hay que tenerlo en cuenta en cualquier prueba con obstáculos cerca. Y mientras se evade, la regla de línea perdida (S2) no se aplica, porque en esa fase la línea no se espera; en su lugar, hay un límite de distancia para encontrarla y un límite de tiempo para toda la maniobra. Cada estado tiene las condiciones de falla que le corresponden.

El error que encontró la simulación

La primera versión pasaba todas las pruebas de comportamiento, pero la de línea perdida mostró un PWM de 133 en un estado FALLA. Ocurría así: tareaSeguir detectaba la pérdida y llamaba a pasarA(FALLA), que ponía los motores en cero y limpiaba las referencias. Pero tareaControl seguía, y en la línea siguiente llamaba a ruedasActualizar. Ese lazo veía una velocidad pedida de cero y una medida de 30 cm/s, y ordenaba marcha atrás para frenar. El robot estaba en FALLA y los motores en marcha atrás a media potencia.

La corrección es una condición: sólo actualizar las ruedas si el estado sigue siendo SEGUIR o EVITAR. La lección es general: un cambio de estado dentro de una función no termina la tarea que la llamó; después de cada pasarA hay que preguntarse qué código sigue ejecutándose en ese mismo ciclo. En el robot, este error se habría visto como un robot que «no se detiene del todo» o que se sacude al fallar, y habría sido difícil de reproducir.

09 · Todo junto

El programa completo

El programa principal reúne los módulos del tema 46 (sin cambios, salvo los nuevos valores de config.h), el control y la odometría. El planificador es el mismo: las tareas y sus períodos no cambiaron, y la tarea de control es ahora la que concentra la lógica. Las distintas partes del programa se explican en las secciones anteriores.

// robot.ino - Integración: seguimiento de línea con PID, evasión de obstáculos por fases y estados de seguridad.
// Los módulos de dispositivo (tema 46) no cambian; se agregan control.h (PID) y odometria.h (pose).
#include "config.h"
#include "motores.h"
#include "encoders.h"
#include "linea.h"
#include "distancia.h"
#include "bateria.h"
#include "control.h"
#include "odometria.h"

enum Estado { ESPERA, SEGUIR, EVITAR, FALLA };
enum Fase { F_FRENAR, F_GIRO_IZQ, F_LATERAL, F_GIRO_DER1, F_PASO, F_GIRO_DER2, F_VUELTA, F_ALINEAR };

Estado estado = ESPERA;
Fase fase = F_FRENAR;
float vel[2] = {0, 0};                              // velocidades medidas (cm/s)
bool lineaPerdidaActiva = false;
unsigned long lineaPerdidaDesde = 0, evitarDesde = 0;

// Datos de la maniobra de evasión
float xFase, yFase;                                 // dónde empezó la fase actual (para medir lo avanzado)
float thBase, distParada;                           // rumbo original y distancia al obstáculo al detenerse
bool lineaVista;

float avanceFase() { return sqrt((odo_x - xFase) * (odo_x - xFase) + (odo_y - yFase) * (odo_y - yFase)); }

void iniciarFase(Fase f) { fase = f; xFase = odo_x; yFase = odo_y; }

void pasarA(Estado nuevo) {
  estado = nuevo;
  lineaPerdidaActiva = false;
  if (nuevo == SEGUIR) pidLinea.reiniciar();
  if (nuevo == EVITAR) { evitarDesde = millis(); iniciarFase(F_FRENAR); }
  if (nuevo == ESPERA || nuevo == FALLA) ruedasParar();   // fuera de marcha, los motores siempre quedan en cero
}

bool botonEvento() {                                // flanco de pulsación con antirrebote de 50 ms
  static bool previo = false;
  static unsigned long ultimo = 0;
  bool pulsado = digitalRead(PIN_BOTON) == LOW;
  bool evento = pulsado && !previo && millis() - ultimo > 50;
  if (pulsado != previo) ultimo = millis();
  previo = pulsado;
  return evento;
}

// ---- Primitivas de movimiento (usan las referencias de velocidad de las ruedas) ----

// Gira en el lugar hacia el rumbo th. Devuelve true cuando llegó (error menor a TOL_GIRO_RAD).
bool girarHacia(float th) {
  float err = angDif(th, odo_th);
  if (fabs(err) < TOL_GIRO_RAD) { ruedasPedir(0, 0); return true; }
  float w = constrain(2.5 * err, -2.0, 2.0);        // rad/s: baja al acercarse
  if (fabs(w) < 0.6) w = w < 0 ? -0.6 : 0.6;        // piso para vencer la zona muerta del motor
  ruedasPedir(-w * ANCHO_VIA_CM / 2.0, w * ANCHO_VIA_CM / 2.0);
  return false;
}

// Avanza a velocidad v (cm/s) manteniendo el rumbo th.
void avanzarRecto(float th, float v) {
  float corr = constrain(40.0 * angDif(th, odo_th), -10.0, 10.0);   // error a la izquierda: rueda izquierda más lenta
  ruedasPedir(v - corr, v + corr);
}

// ---- Evasión: rodear la caja por la izquierda y volver a la línea ----
void tareaEvitar() {
  switch (fase) {
    case F_FRENAR:
      ruedasPedir(0, 0);
      if (fabs(vel[0]) < 2.0 && fabs(vel[1]) < 2.0) {
        distParada = dis_cm;
        thBase = odo_th;
        iniciarFase(F_GIRO_IZQ);
      }
      break;
    case F_GIRO_IZQ:
      if (girarHacia(thBase + PI / 2)) iniciarFase(F_LATERAL);
      break;
    case F_LATERAL:
      avanzarRecto(thBase + PI / 2, VEL_MANIOBRA);
      if (avanceFase() >= D_LATERAL_CM) iniciarFase(F_GIRO_DER1);
      break;
    case F_GIRO_DER1:
      if (girarHacia(thBase)) iniciarFase(F_PASO);
      break;
    case F_PASO:
      avanzarRecto(thBase, VEL_MANIOBRA);
      if (avanceFase() >= distParada + D_EXTRA_CM) iniciarFase(F_GIRO_DER2);
      break;
    case F_GIRO_DER2:
      if (girarHacia(thBase - PI / 2)) { lineaVista = false; iniciarFase(F_VUELTA); }
      break;
    case F_VUELTA:                                  // avanzar hacia la línea; al verla, seguir hasta poner el eje sobre ella
      avanzarRecto(thBase - PI / 2, VEL_BUSQUEDA);
      if (!lineaVista && lin_visible) { lineaVista = true; xFase = odo_x; yFase = odo_y; }
      if (lineaVista && avanceFase() >= D_ARREGLO_CM) iniciarFase(F_ALINEAR);
      else if (!lineaVista && avanceFase() > D_LATERAL_CM + 20.0) pasarA(FALLA);   // no la encontró
      break;
    case F_ALINEAR:
      if (girarHacia(thBase)) pasarA(SEGUIR);
      break;
  }
  if (estado == EVITAR && millis() - evitarDesde > T_EVITAR_MAX_MS) pasarA(FALLA);   // F4: máximo 15 s
}

// ---- Seguimiento de línea: lazo externo de línea y lazos internos de rueda ----
void tareaSeguir(float dt) {
  if (!lin_visible) {                               // S2: línea perdida durante más de 1 s
    if (!lineaPerdidaActiva) { lineaPerdidaActiva = true; lineaPerdidaDesde = millis(); }
    if (millis() - lineaPerdidaDesde >= T_LINEA_PERDIDA_MS) { pasarA(FALLA); return; }
  } else {
    lineaPerdidaActiva = false;
  }
  float delta = -pidLinea.calcular(0, lin_pos, dt);          // cm/s; + si la línea está a la derecha
  float v = max(VEL_MIN, VEL_NOM - 0.5 * fabs(delta));       // más lento cuanto más corrige
  ruedasPedir(v + delta, v - delta);
  if (dis_cm <= DIST_OBSTACULO_CM) pasarA(EVITAR);           // F3: obstáculo a 20 cm o menos
}

void tareaControl() {                               // cada 20 ms
  if (botonEvento()) pasarA(estado == ESPERA ? SEGUIR : ESPERA);   // desde ESPERA arranca; en marcha o en FALLA vuelve a ESPERA
  float dt = TS_CONTROL_MS / 1000.0;
  vel[0] = encVelocidad(0, dt);
  vel[1] = encVelocidad(1, dt);
  odoActualizar();
  lineaLeer();
  if (estado == SEGUIR || estado == EVITAR) {
    if (bateriaBaja()) { pasarA(FALLA); return; }            // S3
    if (estado == SEGUIR) tareaSeguir(dt); else tareaEvitar();
    if (estado == SEGUIR || estado == EVITAR) ruedasActualizar(vel, dt);   // si la tarea acaba de pasar a FALLA, no volver a ordenar los motores
  }
}

void tareaDistancia() { distanciaActualizar(); }    // cada 60 ms, justo después del control para no retrasarlo
void tareaBateria() { bateriaActualizar(); }        // cada 100 ms

void tareaTelemetria() {                            // cada 100 ms
  Serial.print((int)estado); Serial.print(',');
  Serial.print((int)fase); Serial.print(',');
  Serial.print(lin_pos, 1); Serial.print(',');
  Serial.print(vel[0], 1); Serial.print(',');
  Serial.print(vel[1], 1); Serial.print(',');
  Serial.print(dis_cm, 0); Serial.print(',');
  Serial.println(bat_v, 2);
  digitalWrite(PIN_LED, estado == FALLA ? (millis() / 250) % 2 : (estado == ESPERA ? LOW : HIGH));
}

struct Tarea { unsigned long periodo; unsigned long proxima; void (*funcion)(); };
Tarea tareas[] = {                                  // el orden importa: dentro de una vuelta, primero el control
  {TS_CONTROL_MS, 0, tareaControl},
  {TS_DISTANCIA_MS, 0, tareaDistancia},
  {TS_BATERIA_MS, 0, tareaBateria},
  {TS_TELEMETRIA_MS, 0, tareaTelemetria},
};

void setup() {
  Serial.begin(115200);
  pinMode(PIN_LED, OUTPUT);
  pinMode(PIN_BOTON, INPUT_PULLUP);
  pinMode(PIN_TRIG, OUTPUT);
  pinMode(PIN_ECHO, INPUT);
  motorBegin();                                     // lo primero: motores en cero
  encBegin();
  lineaCalibrar(3000);                              // 3 s: pasá el arreglo sobre la línea y el blanco
  bateriaActualizar();
  for (auto &t : tareas) t.proxima = millis();      // la calibración tardó: sin esto, las tareas se ejecutarían en ráfaga
}

void loop() {
  unsigned long ahora = millis();
  for (auto &t : tareas) {
    if ((long)(ahora - t.proxima) >= 0) {
      t.proxima += t.periodo;                       // período fijo, sin acumular retrasos
      t.funcion();
    }
  }
}

Algunas decisiones de estilo que importan:

• Una sola salida a los motores. Todo lo que mueve las ruedas pasa por ruedasPedir y ruedasActualizar. Así la rampa de aceleración, la prealimentación y el PI valen para el seguimiento de línea y para la evasión.
• Cada fase es corta y no espera. Una fase pide velocidades, revisa su condición de fin y vuelve. Nunca hay un bucle dentro de una fase: el planificador sigue girando y los estados de seguridad siguen vigilando.
• El estado de la maniobra está en variables globales (thBase, distParada, xFase, yFase), reiniciadas en iniciarFase. No hay estado oculto en variables estáticas de funciones.
• El reinicio es explícito. Al entrar a SEGUIR se reinicia el PID de línea, para que no arrastre la integral y la derivada de antes de la evasión.

10 · Comprobar los requisitos

Pruebas de integración

Los requisitos del tema 45 se verifican contra las mediciones del simulador (en el robot real se repiten, tema 48):

216 ejecuciones variadas

Una sola ejecución puede salir bien por casualidad. Para medir la robustez se repitió la prueba con variaciones: la caja a 55, 65 y 75 cm; el robot desplazado −1, 0 y +1 cm de la línea al arrancar; torcido −4,6°, 0° y +4,6°; el motor derecho con 85, 93, 100 y 105 % de la fuerza del izquierdo; y los sensores de línea sin ruido y con ruido de ±3 cuentas. Son 3 × 3 × 3 × 4 × 2 = 216 combinaciones. Resultados con el umbral de 25 cm:

• Las 216 completaron la vuelta sin pasar a FALLA.
• El peor margen de frenado fue de 14,4 cm (todos sobre los 10 cm).
• La peor holgura del cuerpo con la caja fue de 3,9 cm (ninguna colisión).
• La maniobra más larga duró 12,5 s.
• El mayor desvío lateral fue de 11,3 mm.

Todo esto es dentro del simulador, que no tiene patinaje, luz ambiente, rebotes del ultrasónico ni batería cayendo. Los números reales serán peores, y por eso los márgenes (15 cm contra 10, 11 mm contra 15) importan. Pero el simulador permite algo que el robot no: repetir 216 pruebas en segundos y cambiar un solo parámetro.

11 · Exploración interactiva

Laboratorios de integración

Con 0,93, Kp = 0,010 y Ki = 0,10 se obtienen los valores de la tabla (0,32 s, 33,6 y 30,0). Poné Kp y Ki en cero: queda la prealimentación sola y la rueda se queda en 27,5. Ahora cambiá la fuerza a 1,10: sin PI, el robot va más rápido de lo pedido. El PI ajusta el resultado en los dos sentidos.

Con los valores iniciales la holgura es de 5 cm, la de la fórmula D_EXTRA − 28. Bajá D_EXTRA a 28: el robot toca la caja al girar hacia la línea. Bajá D_LATERAL a 15: la holgura cae a 1 cm. Fijate que la distancia de parada no cambia la holgura, porque el avance se calcula a partir de ella.

12 · Código ejecutable

Lazo de rueda y geometría en Python

El mismo modelo de motor, el mismo regulador y la misma geometría del tema, para probar cambios sin placa. Reproduce exactamente la tabla del escalón de velocidad del simulador y calcula la holgura mínima de la maniobra ideal.

from math import pi, cos, sin, radians

# --- Robot (los mismos valores de config.h) ---
R, B, CPR = 3.25, 15.0, 1440
CM_CUENTA = 2 * pi * R / CPR
PWM_MIN, ZONA_MUERTA, KFF, ACEL_MAX = 60, 0.02, 0.021, 100.0

# --- Motor simulado: arranca con PWM_MIN, 11,35 cm/s por voltio, 5,4 V efectivos, constante de tiempo de 0,12 s ---
TAU_M, KV, VC, VEFF = 0.12, 11.35, 1.27, 5.4

def pwm_desde_orden(u):
    a = abs(u)
    if a < ZONA_MUERTA:
        return 0
    return int(PWM_MIN + (255 - PWM_MIN) * min(a, 1.0) + 0.5)

class PID:
    """Igual que la estructura PID de control.h: derivada sobre la medida e integración condicional."""
    def __init__(self, kp, ki, kd, umin, umax):
        self.kp, self.ki, self.kd, self.umin, self.umax = kp, ki, kd, umin, umax
        self.integral, self.previa, self.primera = 0.0, 0.0, True

    def calcular(self, ref, medida, dt):
        e = ref - medida
        d = 0.0 if self.primera else -(medida - self.previa) / dt
        self.primera, self.previa = False, medida
        nueva = self.integral + self.ki * e * dt
        salida = self.kp * e + nueva + self.kd * d
        satura = salida > self.umax or salida < self.umin
        if not satura or (salida > self.umax and e < 0) or (salida < self.umin and e > 0):
            self.integral = nueva
        return min(self.umax, max(self.umin, salida))

def escalon_rueda(ganancia_motor, lazos, v_ref=30.0, t_fin=3.0):
    """Escalón de velocidad de una rueda con prealimentación (y PI si lazos=True). Control cada 20 ms."""
    pid = PID(0.010 if lazos else 0.0, 0.10 if lazos else 0.0, 0.0, -1.0, 1.0)
    vw = acc = 0.0
    cuentas = previa = 0
    rampa, pwm = 0.0, 0
    t90, pico = None, 0.0
    for k in range(round(t_fin / 0.02)):
        medida = (cuentas - previa) * CM_CUENTA / 0.02            # velocidad medida en la ventana de 20 ms
        previa = cuentas
        rampa += min(max(v_ref - rampa, -ACEL_MAX * 0.02), ACEL_MAX * 0.02)
        u = KFF * rampa + pid.calcular(rampa, medida, 0.02)
        pwm = pwm_desde_orden(u)
        for _ in range(20):                                      # física a 1 ms
            vss = max(0.0, KV * (pwm / 255.0 * VEFF - VC)) * ganancia_motor if pwm > 0 else 0.0
            vw += 0.001 * (vss - vw) / TAU_M
            acc += vw * 0.001 / CM_CUENTA
            while acc >= 1:
                acc -= 1
                cuentas += 1
        pico = max(pico, vw)
        if t90 is None and vw >= 0.9 * v_ref:
            t90 = (k + 1) * 0.02
    return t90, pico, vw

# --- Geometría de la evasión ---
CAJA = (65.0, 75.0, -5.0, 5.0)            # x0, x1, y0, y1 (cm)
FRENTE, ATRAS, MEDIO_ANCHO = 9.0, 8.0, 9.0

def holgura(x, y, th, caja=CAJA):
    """Distancia mínima (cm) entre el cuerpo del robot y la caja, 0 si se tocan."""
    x0, x1, y0, y1 = caja
    mejor = 1e9
    for i in range(41):
        a = -ATRAS + (FRENTE + ATRAS) * i / 40
        for lado in (-MEDIO_ANCHO, MEDIO_ANCHO):                 # bordes izquierdo y derecho
            candidatos = [(a, lado)]
            candidatos += [((FRENTE if j else -ATRAS), -MEDIO_ANCHO + 2 * MEDIO_ANCHO * i / 40) for j in (0, 1)]
            for (aa, ll) in candidatos:
                px = x + aa * cos(th) + ll * sin(th)
                py = y + aa * sin(th) - ll * cos(th)
                dx = max(x0 - px, 0.0, px - x1)
                dy = max(y0 - py, 0.0, py - y1)
                mejor = min(mejor, (dx * dx + dy * dy) ** 0.5)
    return mejor

def holgura_trayectoria(dist_parada, lateral=25.0, extra=33.0):
    """Recorre la maniobra ideal (giros en el lugar y rectas) y devuelve la holgura mínima con la caja."""
    x = CAJA[0] - FRENTE - dist_parada                           # el robot se detiene con la caja a dist_parada del frente
    y, th, peor = 0.0, 0.0, 1e9
    def girar(destino):
        nonlocal th, peor
        paso = 1.0 if destino > th else -1.0
        while abs(destino - th) > 1e-9:
            th += paso * radians(1.0) if abs(destino - th) > radians(1.0) else destino - th
            peor = min(peor, holgura(x, y, th))
    def avanzar(d):
        nonlocal x, y, peor
        n = int(d)
        for _ in range(n):
            x += cos(th); y += sin(th)
            peor = min(peor, holgura(x, y, th))
    peor = holgura(x, y, th)
    girar(pi / 2); avanzar(lateral)
    girar(0.0); avanzar(dist_parada + extra)
    girar(-pi / 2); avanzar(lateral + 5)
    return peor

if __name__ == "__main__":
    print("Escalón de 0 a 30 cm/s en una rueda (motor derecho 7 % más débil):")
    for lazos in (True, False):
        for nombre, g in (("izquierda", 1.0), ("derecha", 0.93)):
            t90, pico, final = escalon_rueda(g, lazos)
            print(f"  {'con lazo PI' if lazos else 'solo prealimentación':<21} {nombre:<9}: 90 % en {t90:.2f} s, pico {pico:.1f}, a los 3 s {final:.1f} cm/s")

    print("\nHolgura mínima del cuerpo con la caja en la maniobra ideal (lateral 25 cm, extra 33 cm):")
    for dp in (10.0, 15.0, 20.0):
        print(f"  se detiene con la caja a {dp:4.1f} cm: holgura {holgura_trayectoria(dp):5.1f} cm")
    print("  con un desplazamiento lateral de 15 cm, parando a 15 cm: "
          f"{holgura_trayectoria(15.0, lateral=15.0):.1f} cm")

La salida repite las tablas del texto: el escalón de velocidad con y sin PI para los dos motores, y la holgura de la maniobra con el avance extra y el desplazamiento lateral elegidos.

Probá cambiar KFF a 0,018 para ver cómo el PI compensa una prealimentación mal medida, o extra=28 en holgura_trayectoria para comprobar que el robot toca la caja.

13 · Diagnóstico

Cuando la integración falla

14 · Práctica guiada

Predecí antes de comprobar

1. Prealimentación

Con KFF = 0,021 y PWM_MIN = 60, ¿qué orden y qué PWM pide la prealimentación para 30 cm/s?

2. Un desvío de 5 mm

La línea está 5 mm a la derecha y sólo actúa el término P (kp = 0,9). ¿Qué velocidades piden a cada rueda?

3. El lado corto

El rumbo actual es 3,0 rad y el objetivo, −3,0 rad. ¿Hacia dónde gira el robot y cuánto?

4. Holgura con la caja

Con D_LATERAL = 25 y D_EXTRA = 31, ¿qué holgura ideal queda? ¿Y con D_EXTRA = 25?

5. El desplazamiento lateral mínimo

Se quiere una holgura mínima de 3 cm con D_EXTRA = 33. ¿Cuál es el menor D_LATERAL?

6. Elegir el umbral

La ejecución de referencia dio 10,3 cm de margen con un umbral de 20 cm y el peor de 216 casos, 9,4 cm. ¿Qué umbral elegirías para F3 con margen de seguridad?

7. Qué corre después de pasarA

En tareaControl, tareaSeguir llama a pasarA(FALLA) y la función sigue. ¿Qué pasaba antes de agregar la condición a ruedasActualizar?

15 · Síntesis

Ideas para recordar

¿Listo para el tema 48?

Antes de avanzar, ejecutá el programa en una pista de papel con una caja y registrá la telemetría: desvío, velocidad, tiempo de maniobra y distancia de parada. Compará cada número con los del simulador y anotá cuál es la mayor diferencia: ahí está lo que el modelo no sabía del robot real.

En el último tema cerramos el proyecto y el curso: calibración final, pruebas de aceptación contra los requisitos, métricas de desempeño, mejoras posibles y próximos pasos.