// aspimaker — Micromouse Ubi Sum?
// Seguimiento de pared con PID sobre el error lateral.
// Licencia: CC BY-NC-SA 4.0 — https://creativecommons.org/licenses/by-nc-sa/4.0/
//
// Las variables kp, ki, kd, offset, dt y las lecturas vienen del resto del
// firmware; aquí está solo el lazo de control.

const float error = (lecturaIzquierda - lecturaDerecha) - offset;

integral += error * dt;
const float derivada = (error - errorAnterior) / dt;
const float giro = kp * error + ki * integral + kd * derivada;

const int base = 180; // pasos por segundo
mover(base - giro, base + giro);

errorAnterior = error;