/*******************************************************************/
/* CÓDIGO COMPLETO E FINAL: ROBÔ SEMEADOR 4WD                      */
/* Versão: 1.0 de 12 de Junho de 2025                              */
/*                                                                 */
/* Plataforma: ESP32                                               */
/* Funcionalidades:                                                */
/* - Chassi 4x4 com 2 pontes H L298N.                              */
/* - Seguidor de linha bidirecional com 4 sensores IR.             */
/* - Desvio de obstáculos com sensor de ultrassom HC-SR04.         */
/* - Manobra de 180 graus com rotação alternada (Dir/Esq).         */
/* - Motor semeador auxiliar controlado por um driver L9110S.      */
/* - O semeador pausa automaticamente durante paradas e manobras.  */
/*******************************************************************/

/  PASSO 1: CONFIGURAÇÃO DA LINHA E CALIBRAÇÃO  /
// Escolha a cor da linha a ser seguida (remova o // de uma das linhas)
#define COR_LINHA_PRETA
// #define COR_LINHA_BRANCA

// Calibre este valor em milissegundos para que o giro seja de 90 graus
#define TEMPO_GIRO_90_GRAUS 500 

// Distância em cm para o robô detectar um obstáculo e iniciar o desvio
#define DISTANCIA_OBSTACULO_CM 20

// Velocidade dos motores de locomoção (0 a 255)
#define VELOCIDADE 180


/  PASSO 2: MAPEAMENTO DE PINOS (GPIO) DO ESP32  /

//  MOTOR SEMEADOR (Controlado pelo Driver L9110S)  
#define PINO_MOTOR_SEMEADOR 17

//   SENSOR DE ULTRASSOM (HC-SR04)  
#define PINO_TRIG 18
#define PINO_ECHO 19

//   SENSORES DE LINHA (INFRAVERMELHO)  
#define PINO_SENSOR_DIANTEIRO_DIREITA  2
#define PINO_SENSOR_DIANTEIRO_ESQUERDA 3
#define PINO_SENSOR_TRASEIRO_DIREITA   4
#define PINO_SENSOR_TRASEIRO_ESQUERDA  15

//   MOTORES DE MOVIMENTO (Ponte H 1 - DIANTEIRA)  
#define H1_IN1 13 // Motor Dianteiro Direito
#define H1_IN2 12
#define H1_IN3 14 // Motor Dianteiro Esquerdo
#define H1_IN4 27

//   MOTORES DE MOVIMENTO (Ponte H 2 - TRASEIRA)  
#define H2_IN1 26 // Motor Traseiro Direito
#define H2_IN2 25
#define H2_IN3 33 // Motor Traseiro Esquerdo
#define H2_IN4 32


//  VARIÁVEIS GLOBAIS DE ESTADO E SENSORES 
int sdd, sde, std, ste; // Abreviações para os 4 sensores de linha
int detectou, naoDetectou;

enum Direcao { FRENTE, TRAS };
Direcao direcaoMovimento = FRENTE; // O robô começa andando para frente

bool proximoGiroParaDireita = true; // Controla a alternância do giro de 180 graus


//****************************************************
//* FUNÇÃO DE CONFIGURAÇÃO INICIAL (SETUP)           *
//****************************************************
void setup() {
    Serial.begin(115200);
    Serial.println("Inicializando o Robô Semeador...");

    // Configura o pino do motor semeador como saída e o desliga
    pinMode(PINO_MOTOR_SEMEADOR, OUTPUT);
    digitalWrite(PINO_MOTOR_SEMEADOR, LOW);

    // Configura os pinos do sensor de ultrassom
    pinMode(PINO_TRIG, OUTPUT);
    pinMode(PINO_ECHO, INPUT);

    // Configura os 4 pinos dos sensores de linha como entrada
    pinMode(PINO_SENSOR_DIANTEIRO_DIREITA, INPUT);
    pinMode(PINO_SENSOR_DIANTEIRO_ESQUERDA, INPUT);
    pinMode(PINO_SENSOR_TRASEIRO_DIREITA, INPUT);
    pinMode(PINO_SENSOR_TRASEIRO_ESQUERDA, INPUT);

    // Define a lógica de detecção com base na cor da linha escolhida
    #ifdef COR_LINHA_PRETA
        detectou = 1; naoDetectou = 0;
        Serial.println("Modo: Seguir linha PRETA.");
    #else
        detectou = 0; naoDetectou = 1;
        Serial.println("Modo: Seguir linha BRANCA.");
    #endif

    configurarPwm(); // Prepara os canais de PWM para os motores de locomoção
    
    Serial.println("Robô pronto. Iniciando em 3 segundos.");
    parar(); // Garante que todos os motores comecem parados
    delay(3000);
}


/****************************************************/
/* LOOP PRINCIPAL DE EXECUÇÃO                       */
/****************************************************/
void loop() {
    // 1. VERIFICAÇÃO DE OBSTÁCULO (TAREFA DE MAIOR PRIORIDADE)
    if (lerDistanciaCm() < DISTANCIA_OBSTACULO_CM) {
        Serial.println("!! OBSTÁCULO DETECTADO !!");
        parar(); // Para todos os motores, incluindo o semeador
        delay(500);

        // Executa a manobra de 180 graus, alternando o lado
        if (proximoGiroParaDireita) {
            Serial.println("...Manobra: Girando 180 graus para a DIREITA.");
            virarDireita();
            delay(TEMPO_GIRO_90_GRAUS * 2);
        } else {
            Serial.println("...Manobra: Girando 180 graus para a ESQUERDA.");
            virarEsquerda();
            delay(TEMPO_GIRO_90_GRAUS * 2);
        }
        
        parar(); // Para novamente após o giro
        delay(500);

        proximoGiroParaDireita = !proximoGiroParaDireita; // Inverte o lado para a próxima vez
        direcaoMovimento = (direcaoMovimento == FRENTE) ? TRAS : FRENTE; // Inverte a direção de movimento
        
        Serial.print("Manobra completa. Retomando percurso para: ");
        Serial.println(direcaoMovimento == FRENTE ? "FRENTE" : "TRAS");

        return; // Reinicia o loop para não executar a lógica de linha neste ciclo
    }

    // 2. LÓGICA DE SEGUIR LINHA (EXECUTADA SE NÃO HOUVER OBSTÁCULO)
    sdd = digitalRead(PINO_SENSOR_DIANTEIRO_DIREITA);
    sde = digitalRead(PINO_SENSOR_DIANTEIRO_ESQUERDA);
    std = digitalRead(PINO_SENSOR_TRASEIRO_DIREITA);
    ste = digitalRead(PINO_SENSOR_TRASEIRO_ESQUERDA);

    // Bloco de decisão baseado na direção atual de movimento
    if (direcaoMovimento == FRENTE) {
        if (sde == detectou && sdd == detectou) { // Fim de curso ou cruzamento
            Serial.println("Fim de curso (FRENTE). Invertendo...");
            parar(); delay(1000);
            direcaoMovimento = TRAS;
            return;
        }
        // Lógica de correção de trajetória
        if (sde == naoDetectou && sdd == naoDetectou) { moverFrente(); }
        else if (sde == naoDetectou && sdd == detectou) { virarDireita(); }
        else if (sde == detectou && sdd == naoDetectou) { virarEsquerda(); }
    } else { // direcaoMovimento == TRAS
        if (ste == detectou && std == detectou) { // Fim de curso ou cruzamento
            Serial.println("Fim de curso (TRAS). Invertendo...");
            parar(); delay(1000);
            direcaoMovimento = FRENTE;
            return;
        }
        // Lógica de correção de trajetória (com correções invertidas)
        if (ste == naoDetectou && std == naoDetectou) { moverTras(); }
        else if (ste == naoDetectou && std == detectou) { virarEsquerda(); }
        else if (ste == detectou && std == naoDetectou) { virarDireita(); }
    }
}


/****************************************************/
/* FUNÇÕES AUXILIARES, DE MOVIMENTO E DE PWM        */
/****************************************************/

//   Funções do Motor Semeador  
void ligarSemeador() {
    digitalWrite(PINO_MOTOR_SEMEADOR, HIGH);
}
void desligarSemeador() {
    digitalWrite(PINO_MOTOR_SEMEADOR, LOW);
}

//   Função do Sensor de Ultrassom  
float lerDistanciaCm() {
    digitalWrite(PINO_TRIG, LOW);
    delayMicroseconds(2);
    digitalWrite(PINO_TRIG, HIGH);
    delayMicroseconds(10);
    digitalWrite(PINO_TRIG, LOW);
    long duracao = pulseIn(PINO_ECHO, HIGH);
    return duracao * 0.034 / 2.0; // Cálculo da distância em cm
}

//   Funções de Movimento (com controle do semeador integrado)  
void moverFrente() {
    ligarSemeador();
    motorWrite(H1_IN1, VELOCIDADE); motorWrite(H1_IN2, 0); // Motor Dianteiro Direito
    motorWrite(H1_IN3, VELOCIDADE); motorWrite(H1_IN4, 0); // Motor Dianteiro Esquerdo
    motorWrite(H2_IN1, VELOCIDADE); motorWrite(H2_IN2, 0); // Motor Traseiro Direito
    motorWrite(H2_IN3, VELOCIDADE); motorWrite(H2_IN4, 0); // Motor Traseiro Esquerdo
}

void moverTras() {
    ligarSemeador();
    motorWrite(H1_IN1, 0); motorWrite(H1_IN2, VELOCIDADE);
    motorWrite(H1_IN3, 0); motorWrite(H1_IN4, VELOCIDADE);
    motorWrite(H2_IN1, 0); motorWrite(H2_IN2, VELOCIDADE);
    motorWrite(H2_IN3, 0); motorWrite(H2_IN4, VELOCIDADE);
}

void virarDireita() {
    desligarSemeador();
    // Motores da Esquerda para FRENTE, Motores da Direita para TRÁS
    motorWrite(H1_IN3, VELOCIDADE); motorWrite(H1_IN4, 0);
    motorWrite(H2_IN3, VELOCIDADE); motorWrite(H2_IN4, 0);
    motorWrite(H1_IN1, 0); motorWrite(H1_IN2, VELOCIDADE);
    motorWrite(H2_IN1, 0); motorWrite(H2_IN2, VELOCIDADE);
}

void virarEsquerda() {
    desligarSemeador();
    // Motores da Direita para FRENTE, Motores da Esquerda para TRÁS
    motorWrite(H1_IN1, VELOCIDADE); motorWrite(H1_IN2, 0);
    motorWrite(H2_IN1, VELOCIDADE); motorWrite(H2_IN2, 0);
    motorWrite(H1_IN3, 0); motorWrite(H1_IN4, VELOCIDADE);
    motorWrite(H2_IN3, 0); motorWrite(H2_IN4, VELOCIDADE);
}

void parar() {
    desligarSemeador();
    // Desliga todos os pinos de controle dos motores de locomoção
    motorWrite(H1_IN1, 0); motorWrite(H1_IN2, 0);
    motorWrite(H1_IN3, 0); motorWrite(H1_IN4, 0);
    motorWrite(H2_IN1, 0); motorWrite(H2_IN2, 0);
    motorWrite(H2_IN3, 0); motorWrite(H2_IN4, 0);
}

//   Funções de PWM para o ESP32  
void configurarPwm() {
    // Configura 8 canais de PWM (um para cada pino de controle IN das pontes H)
    // Frequência de 5000 Hz e resolução de 8 bits (0-255)
    for (int i = 0; i < 8; i++) {
        ledcSetup(i, 5000, 8);
    }
    // Anexa cada pino de motor a um canal de PWM
    ledcAttachPin(H1_IN1, 0); ledcAttachPin(H1_IN2, 1);
    ledcAttachPin(H1_IN3, 2); ledcAttachPin(H1_IN4, 3);
    ledcAttachPin(H2_IN1, 4); ledcAttachPin(H2_IN2, 5);
    ledcAttachPin(H2_IN3, 6); ledcAttachPin(H2_IN4, 7);
}

void motorWrite(int pino, int velocidade) {
    // Associa o pino ao seu canal de PWM correspondente para escrever o valor
    if (pino == H1_IN1) ledcWrite(0, velocidade); else if (pino == H1_IN2) ledcWrite(1, velocidade);
    else if (pino == H1_IN3) ledcWrite(2, velocidade); else if (pino == H1_IN4) ledcWrite(3, velocidade);
    else if (pino == H2_IN1) ledcWrite(4, velocidade); else if (pino == H2_IN2) ledcWrite(5, velocidade);
    else if (pino == H2_IN3) ledcWrite(6, velocidade); else if (pino == H2_IN4) ledcWrite(7, velocidade);
}