/*
 * Robô Seguidor de Linha (PID) — Arduino UNO
 * Driver: Ponte H L298N   |   Sensores: régua de refletância (QTR, 6 canais)
 * Lê a posição da linha, calcula o erro e corrige a trajetória em tempo real (PID).
 * (código original adaptado; comentários traduzidos para o português)
 */

// ===== Pinos do driver (Ponte H L298N) =====
#define PWMA 9   // ENA - velocidade do motor A
#define AIN2 8   // IN2 - direção do motor A
#define AIN1 7   // IN1 - direção do motor A
#define BIN1 5   // IN3 - direção do motor B
#define BIN2 4   // IN4 - direção do motor B
#define PWMB 3   // ENB - velocidade do motor B

int velocidadeBase = 250;   // velocidade de cruzeiro (0 a 255)

int erro = 0;               // erro atual (termo proporcional)
int derivada = 0;
int integral = 0;
int erroAnterior = 0;
float correcao;

#include <QTRSensors.h>

#define NUM_SENSORES 6
#define TIMEOUT 2500
#define PINO_EMISSOR 13     // liga os LEDs infravermelhos da régua

QTRSensorsRC qtr(
  (unsigned char[]){17, 16, 15, 14, 12, 11},   // A3, A2, A1, A0, D12, D11
  NUM_SENSORES,
  TIMEOUT,
  PINO_EMISSOR
);

unsigned int leiturasSensores[NUM_SENSORES];
unsigned int posicao = 0;

void setup() {
  Serial.begin(115200);

  pinMode(PWMA, OUTPUT);
  pinMode(AIN1, OUTPUT);
  pinMode(AIN2, OUTPUT);
  pinMode(PWMB, OUTPUT);
  pinMode(BIN1, OUTPUT);
  pinMode(BIN2, OUTPUT);

  // Calibração: passe a régua sobre a linha preta e o fundo branco por alguns segundos
  Serial.println("== CALIBRANDO ==");
  digitalWrite(PINO_EMISSOR, HIGH);
  for (int i = 0; i < 400; i++) {
    qtr.calibrate();
    delay(5);
  }
  digitalWrite(PINO_EMISSOR, LOW);
  Serial.println("== PRONTO ==");
}

void loop() {
  qtr.read(leiturasSensores);
  posicao = qtr.readLine(leiturasSensores, QTR_EMITTERS_ON, 0);
  erro = posicao - 3500;    // 3500 = centro (régua de 6 sensores -> faixa 0..7000)

  // ===== Monitor serial (depuração dos sensores) =====
  Serial.print("POS: "); Serial.print(posicao);
  Serial.print("  ERRO: "); Serial.print(erro);
  Serial.print("  CORR: "); Serial.print(correcao);
  Serial.print("  S: ");
  for (int i = 0; i < NUM_SENSORES; i++) {
    Serial.print(leiturasSensores[i]);
    Serial.print(" ");
  }
  Serial.println();

  // Linha bem à esquerda -> gira no lugar para reencontrá-la
  if (erro < -3500) {
    digitalWrite(AIN1, LOW);  digitalWrite(AIN2, HIGH); analogWrite(PWMA, 255);
    digitalWrite(BIN1, LOW);  digitalWrite(BIN2, HIGH); analogWrite(PWMB, 255);
  }
  // Linha bem à direita -> gira no lugar para o outro lado
  else if (erro > 3500) {
    digitalWrite(AIN1, HIGH); digitalWrite(AIN2, LOW);  analogWrite(PWMA, 255);
    digitalWrite(BIN1, HIGH); digitalWrite(BIN2, LOW);  analogWrite(PWMB, 255);
  }
  // Sobre a linha -> o controle PID ajusta as duas velocidades
  else {
    derivada = erro - erroAnterior;
    integral = erro + erroAnterior;

    correcao = (erro * 0.005) + (derivada * 15) + (integral * 0.125);

    if (correcao > velocidadeBase)  correcao = velocidadeBase;
    if (correcao < -velocidadeBase) correcao = -velocidadeBase;

    digitalWrite(AIN1, HIGH); digitalWrite(AIN2, LOW);
    analogWrite(PWMA, velocidadeBase - correcao);

    digitalWrite(BIN1, HIGH); digitalWrite(BIN2, LOW);
    analogWrite(PWMB, velocidadeBase + correcao);

    erroAnterior = erro;
  }

  delay(5); // evita saturar o monitor serial
}