🏁 Projeto: seguidor de linha com PID

// Seguidor de linha com PID, calibração automática e log no microSD
// ESP32 (pacote esp32 3.x) + barra de 5 sensores IR analógicos + TB6612FNG + módulo microSD
#include <SPI.h>
#include <SD.h>
// ---------- ajuste aqui ----------
float KP = 0.08, KI = 0.0, KD = 0.6;     // ganhos do PID de direção (ponto de partida)
int BASE = 150;                           // velocidade base (0 a 255)
const bool LINHA_ESCURA_ALTA = true;      // a barra lê MAIS alto sobre o preto?
// ---------- pinos ----------
const int SENS[5] = { 36, 39, 34, 35, 32 };   // S0 (esquerda) a S4 (direita), só ADC1
const int PWMA = 25, AIN1 = 26, AIN2 = 27;    // motor esquerdo
const int PWMB = 14, BIN1 = 16, BIN2 = 17;    // motor direito (STBY no 3V3)
const int SD_CS = 5, BOTAO = 0, LED = 2;

int minimo[5], maximo[5], ultimoLado = 1;
float erroAnt = 0, integral = 0;
bool naLinha = false, temSD = false;
unsigned long tCiclo = 0, tViuLinha = 0;
enum Estado { ESPERA, CALIBRA, PRONTO, SEGUE };
Estado estado = ESPERA;
struct Amostra { uint32_t t; int16_t pos, erro, corr, esq, dir; };
QueueHandle_t filaLog;
File arq;

void motor(int pwm, int in1, int in2, int v) {
  v = constrain(v, -255, 255);
  digitalWrite(in1, v > 0); digitalWrite(in2, v < 0); ledcWrite(pwm, abs(v));   // API 3.x
}
void andar(int esq, int dir) { motor(PWMA, AIN1, AIN2, esq); motor(PWMB, BIN1, BIN2, dir); }
bool botaoApertado() { static int antes = HIGH; int a = digitalRead(BOTAO); bool b = (antes == HIGH && a == LOW); antes = a; return b; }

void calibrar() {                            // gira no lugar 2,5 s; é a única espera do programa
  for (int i = 0; i < 5; i++) { minimo[i] = 4095; maximo[i] = 0; }
  unsigned long t0 = millis();
  while (millis() - t0 < 2500) {
    bool lado = ((millis() - t0) / 625) % 2;                 // troca o sentido a cada 0,625 s
    andar(lado ? 110 : -110, lado ? -110 : 110);
    for (int i = 0; i < 5; i++) { int v = analogRead(SENS[i]); minimo[i] = min(minimo[i], v); maximo[i] = max(maximo[i], v); }
  }
  andar(0, 0);
  for (int i = 0; i < 5; i++) Serial.printf("S%d: min %d max %d\n", i, minimo[i], maximo[i]);
}

int lerPosicao() {                           // 0 (linha sob S0) a 4000 (sob S4); 2000 = centro
  long soma = 0, pesos = 0;
  for (int i = 0; i < 5; i++) {
    long n = constrain((long)(analogRead(SENS[i]) - minimo[i]) * 1000 / max(maximo[i] - minimo[i], 1), 0L, 1000L);
    if (!LINHA_ESCURA_ALTA) n = 1000 - n;                    // 1000 = em cima da linha
    if (n > 50) { soma += n * i * 1000; pesos += n; }         // ignora o ruído do fundo
  }
  naLinha = pesos > 300;
  if (!naLinha) return ultimoLado < 0 ? 0 : 4000;            // perdeu: vira para onde ela sumiu
  int pos = soma / pesos;
  ultimoLado = pos < 2000 ? -1 : 1;
  return pos;
}

void registrar(uint32_t t, int pos, int erro, int corr, int esq, int dir) {
  if (!temSD) return;
  Amostra a = { t, (int16_t)pos, (int16_t)erro, (int16_t)corr, (int16_t)esq, (int16_t)dir };
  xQueueSend(filaLog, &a, t <= 1 ? pdMS_TO_TICKS(50) : 0);  // t = 0 abre arquivo, t = 1 fecha
}

void cicloControle() {                       // roda a cada 5 ms (200 Hz)
  static uint8_t n = 0;
  int pos = lerPosicao();
  if (naLinha) tViuLinha = millis();
  else if (millis() - tViuLinha > 400) { andar(0, 0); return; }   // saiu da pista: para
  float erro = pos - 2000;                   // negativo: linha à esquerda
  integral = constrain(integral + erro, -20000, 20000);        // limita o windup
  float corr = KP * erro + KI * integral + KD * (erro - erroAnt);
  erroAnt = erro;
  int esq = constrain(BASE + corr, -255, 255), dir = constrain(BASE - corr, -255, 255);
  andar(esq, dir);
  if (++n % 2 == 0) registrar(millis(), pos, erro, corr, esq, dir);                     // log a 100 Hz
  if (n % 10 == 0) Serial.printf("erro:%.0f,corr:%.0f,esq:%d,dir:%d\n", erro, corr, esq, dir);   // Plotter a 20 Hz
}

void abrirProximo() {                        // /pid001.csv, /pid002.csv... um por corrida
  char nome[16];
  for (int i = 1; i < 1000; i++) { snprintf(nome, sizeof nome, "/pid%03d.csv", i); if (!SD.exists(nome)) break; }
  arq = SD.open(nome, FILE_WRITE);
  arq.printf("# KP=%.3f KI=%.4f KD=%.3f BASE=%d\n", KP, KI, KD, BASE);
  arq.println("t_ms,pos,erro,corr,esq,dir");
  Serial.printf("log: %s\n", nome);
}

void tarefaLog(void *p) {                    // núcleo 0: grava sem atrasar o controle
  Amostra a; int linhas = 0;
  for (;;) {
    if (xQueueReceive(filaLog, &a, portMAX_DELAY) != pdTRUE) continue;
    if (a.t == 0) { abrirProximo(); linhas = 0; }
    else if (a.t == 1) { if (arq) arq.close(); }
    else if (arq) {
      arq.printf("%lu,%d,%d,%d,%d,%d\n", (unsigned long)a.t, a.pos, a.erro, a.corr, a.esq, a.dir);
      if (++linhas % 50 == 0) arq.flush();
    }
  }
}

void setup() {
  Serial.begin(115200);
  pinMode(AIN1, OUTPUT); pinMode(AIN2, OUTPUT); pinMode(BIN1, OUTPUT); pinMode(BIN2, OUTPUT);
  ledcAttach(PWMA, 20000, 8); ledcAttach(PWMB, 20000, 8);
  pinMode(BOTAO, INPUT_PULLUP); pinMode(LED, OUTPUT);
  temSD = SD.begin(SD_CS);
  if (temSD) { filaLog = xQueueCreate(64, sizeof(Amostra)); xTaskCreatePinnedToCore(tarefaLog, "log", 4096, NULL, 1, NULL, 0); }
  Serial.println(temSD ? "SD ok" : "sem SD: segue sem log");
  Serial.println("BOOT: calibrar, depois seguir, depois parar");
}

void loop() {
  bool botao = botaoApertado();
  switch (estado) {
    case ESPERA:  if (botao) estado = CALIBRA; break;
    case CALIBRA: calibrar(); estado = PRONTO; Serial.println("Calibrado: ponha na linha e aperte BOOT"); break;
    case PRONTO:
      if (botao) { erroAnt = 0; integral = 0; tViuLinha = millis(); registrar(0, 0, 0, 0, 0, 0); tCiclo = micros(); estado = SEGUE; }
      break;
    case SEGUE:
      if (botao) { andar(0, 0); registrar(1, 0, 0, 0, 0, 0); estado = PRONTO; Serial.println("Parado"); break; }
      if (micros() - tCiclo >= 5000) { tCiclo += 5000; cicloControle(); }   // 200 Hz, sem delay
      break;
  }
  digitalWrite(LED, estado == SEGUE);
}

# compara.py: antes x depois de um ajuste do PID
# Uso: python compara.py pid001.csv pid002.csv
import sys
import pandas as pd
import matplotlib.pyplot as plt

for arquivo in sys.argv[1:]:
    ganhos = open(arquivo).readline().strip("# \n")           # linha "# KP=... KD=..."
    df = pd.read_csv(arquivo, comment="#")
    t = (df["t_ms"] - df["t_ms"].iloc[0]) / 1000
    rms = (df["erro"] ** 2).mean() ** 0.5
    satur = ((df["esq"].abs() >= 255) | (df["dir"].abs() >= 255)).mean() * 100
    print(f"{arquivo}: {ganhos} | RMS {rms:.0f} | máx {df['erro'].abs().max()} | volta {t.iloc[-1]:.2f} s | saturado {satur:.0f}%")
    plt.plot(t, df["erro"], label=f"{arquivo} ({ganhos})")

plt.axhline(0, color="gray", lw=0.8)
plt.xlabel("tempo (s)"); plt.ylabel("erro"); plt.legend(); plt.grid(alpha=0.3)
plt.savefig("comparacao.png", dpi=120)
plt.show()