Seguidor de Linha · 4 sensores TCRT5000

/*
 * Seguidor de Linha · 4 sensores TCRT5000 — Arduino UNO
 * Driver: TB6612FNG   |   Sensores: 4x TCRT5000 (lidos como régua QTR)
 * PID proporcional-derivativo simples, sempre avançando.
 * (código original adaptado; comentários traduzidos para o português)
 */

// ===== Pinos do driver TB6612FNG =====
#define PWMA 9   // velocidade do motor A
#define AIN2 8   // direção do motor A
#define AIN1 7   // direção do motor A
#define BIN1 5   // direção do motor B
#define BIN2 4   // direção do motor B
#define PWMB 3   // velocidade do motor B
#define STBY 6   // standby: precisa ficar em HIGH para o driver funcionar

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

// ===== Ganhos do PID =====
float KP = 0.2;
float KD = 5;

int erro = 0;
int derivada = 0;
int erroAnterior = 0;
float correcao;

#include <QTRSensors.h>

#define NUM_SENSORES 4
#define TIMEOUT 2500
#define PINO_EMISSOR 13   // liga os LEDs infravermelhos dos sensores

QTRSensorsRC qtr(
  (unsigned char[]){18, 17, 16, 15},   // A4, A3, A2, A1
  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);
  pinMode(STBY, OUTPUT);

  digitalWrite(STBY, HIGH);   // habilita o TB6612

  // Calibração: passe os 4 sensores sobre a linha e o fundo 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() {
  posicao = qtr.readLine(leiturasSensores, QTR_EMITTERS_ON, 0);

  // Centro para 4 sensores: faixa 0..3000, centro = 1500
  erro = posicao - 1500;

  // ===== PID =====
  derivada = erro - erroAnterior;
  correcao = (erro * KP) + (derivada * KD);
  correcao = constrain(correcao, -velocidadeBase, velocidadeBase);

  int motorA = velocidadeBase - correcao;
  int motorB = velocidadeBase + correcao;
  motorA = constrain(motorA, 0, 255);
  motorB = constrain(motorB, 0, 255);

  // ===== Sempre para frente, ajustando só a velocidade de cada lado =====
  digitalWrite(AIN1, HIGH); digitalWrite(AIN2, LOW); analogWrite(PWMA, motorA);
  digitalWrite(BIN1, HIGH); digitalWrite(BIN2, LOW); analogWrite(PWMB, motorB);

  erroAnterior = erro;
  delay(5);
}