Sistema de Control PID
Índice
- Arquitectura de Control
- Lazos PID
- Fórmula PID
- Compensación de Batería
- Rampas de Aceleración
- Detección de Saturación
- Control de Ventilador
- Diagrama de Cascada
Arquitectura de Control
control_loop() en control.c:351-615 se ejecuta a 1 kHz desde el SysTick ISR. Implementa 7 lazos PID en cascada para controlar velocidad lineal, velocidad angular y correcciones basadas en sensores.
target_linear_speed ──► [PID lineal] ──────────────────┐
ideal_angular_speed ──► [PID angular] ──────┐ │
side_sensors ─────────► [PID lateral] ──────┤ │
front_angle ──────────► [PID ángulo front] ─┤ ▼
front_distance ───────► [PID dist front] ───┤ ┌──────────┐
front_diagonal ───────► [PID diag front] ───┘───► │ linear + │
│ angular │
│ voltage │
└────┬─────┘
▼
voltage_to_pwm()
▼
set_motors_pwm(L,R)
Lazos PID
1. PID Lineal (KPI_LINEAR)
linear_error = ideal_linear_speed − measured_linear_speed
Controla la velocidad lineal del robot. measured_linear_speed es el promedio de la velocidad de ambas ruedas (encoders).
2. PID Angular (KPI_ANGULAR)
angular_error = ideal_angular_speed − measured_angular_speed
Controla la velocidad angular. measured_angular_speed = −lsm6dsr_get_gyro_z_radps() (giroscopio, signo negado).
3. PID Lateral (KPI_SIDE_SENSORS)
side_error = get_side_sensors_error()
Corrige el centrado entre paredes laterales. Ver Sensores - Error Lateral.
4. PID Ángulo Frontal (KPI_FRONT_ANGLE_SENSORS)
front_angle_error = get_front_sensors_angle_error() // dist_L − dist_R
Corrige la orientación respecto a la pared frontal.
5. PID Distancia Frontal (KPI_FRONT_DISTANCE_SENSORS)
front_distance_error = measured_front_dist − ideal_front_distance
Mantiene distancia constante a la pared frontal (usado en keep_front_distance()).
6. PID Distancia Frontal Raw (KPI_FRONT_RAW_DISTANCE_SENSORS)
Similar al anterior pero usando valores raw del ADC (modo USE_RAW_SENSORS).
7. PID Diagonal Frontal (KPI_FRONT_DIAGONAL_SENSORS)
diagonal_error = get_front_sensors_diagonal_error()
Corrige la trayectoria en aproximaciones diagonales.
Fórmula PID
Para cada lazo:
output = kp × error + ki × ∫error + kd × (error − last_error)
Donde:
- ∫error es la suma acumulada del error (integrador)
- (error − last_error) es la derivada discreta
Combinación de Voltajes
linear_voltage = PID_linear + PID_front_distance + PID_front_raw_distance
angular_voltage = PID_angular + PID_side + PID_front_angle + PID_front_diagonal
voltage_left = linear_voltage + angular_voltage
voltage_right = linear_voltage − angular_voltage
⚠️ Anti-Windup
Los 6 integradores acumulan error sin clamping. Si los motores están saturados pero no alcanzan la velocidad deseada, el error se acumula. Al liberarse la condición, el overshoot puede desestabilizar el robot. Ver MV-01.
Compensación de Batería
pwm = voltage / battery_voltage × MOTORES_MAX_PWM
Compensa variaciones de voltaje de batería para mantener el par motor constante. A menor batería, mayor PWM para la misma velocidad.
Rampas de Aceleración
update_ideal_linear_speed() aplica aceleración progresiva hacia target_linear_speed:
if (ideal < target) {
accel = (ideal < speed_hard) ? accel_hard : accel_soft;
ideal += accel / CONTROL_FREQUENCY_HZ; // 1000
}
- accel_hard: aceleración inicial (más agresiva)
- accel_soft: aceleración tras superar
speed_hard(más suave) - break_accel: deceleración (frenada)
Las estrategias de velocidad más altas (FAST, SUPER, HAKI) usan aceleración en dos etapas para evitar deslizamiento.
Detección de Saturación
Saturación de PWM
if (abs(pwm) >= MOTORES_SATURATION_PWM) // 1000 de 1024
saturation_count++;
else
saturation_count = 0;
MAX_MOTOR_SATURATION_COUNT = 100 → 100 ms de saturación continua → parada de emergencia.
Saturación Angular
if (abs(encoder_angular_speed) >= MOTORES_SATURATION_ANGULAR_SPEED) // 50 rad/s
angular_saturation_count++;
MAX_MOTOR_ANGULAR_SATURATION_COUNT = 60 → 60 ms → parada de emergencia.
Acción de Emergencia
- Motores → 0
- Ventilador → 0
- LED RGB parpadea (rojo = PWM, azul = angular)
- Si pasan 3 segundos →
set_race_started(false)
Control de Ventilador
El ventilador de succión también tiene control con rampa:
update_fan_speed():
if (ideal < target) ideal += fan_speed_accel / 1000
set_fan_speed(ideal)
La velocidad del ventilador depende de la estrategia y del voltaje de batería (2S vs 3S).
Diagrama de Cascada
┌─────────────────────────────────────────────────────────────┐
│ control_loop() @ 1 kHz │
│ │
│ ┌───────────────────┐ │
│ │ Rampa aceleración │ ideal += accel/1000 │
│ └────────┬──────────┘ │
│ │ │
│ ┌────────┴────────┐ │
│ │ Error = ideal − │ measured (encoders) │
│ │ measured_speed │ │
│ └────────┬────────┘ │
│ │ │
│ ┌────────┴────────────────────────────────────┐ │
│ │ PID Lineal: kp·e + ki·∫e + kd·(e−e_prev) │ │
│ └────────┬────────────────────────────────────┘ │
│ │ linear_voltage │
│ ▼ │
│ ┌────────────────────────────────────────────┐ │
│ │ Σ correcciones angulares: │ │
│ │ + PID angular (gyro) │ │
│ │ + PID lateral (side sensors) │ │
│ │ + PID ángulo frontal │ │
│ │ + PID diagonal frontal │ │
│ └────────┬───────────────────────────────────┘ │
│ │ angular_voltage │
│ ▼ │
│ ┌────────────────────────────────────────────┐ │
│ │ voltage_L = linear + angular │ │
│ │ voltage_R = linear − angular │ │
│ └────────┬───────────────────────────────────┘ │
│ │ │
│ ┌────────┴────────────────────────────────────┐ │
│ │ PWM = voltage / Vbat × MAX_PWM │ │
│ └────────┬────────────────────────────────────┘ │
│ │ │
│ ┌────────┴────────┐ │
│ │ set_motors_pwm │ │
│ │ check_saturation│ │
│ └─────────────────┘ │
└─────────────────────────────────────────────────────────────┘
Documento generado el 2026-06-12. Ver también Movimiento, Sensores, Cinemática. Issues: MV-01, MV-10.