Carro Dual: Modo RC & Line Follower (ESP-32 e Firebase)

#include <Arduino.h>
#include <WiFi.h>
#include <FirebaseESP32.h>
// Provide the RTDB payload printing info and other helper functions.
#include <addons/RTDBHelper.h>

/* 1. Define the WiFi credentials */
#define WIFI_SSID "xxxx"
#define WIFI_PASSWORD "xxxx"

/* 2. If work with RTDB, define the RTDB URL and database secret */
#define DATABASE_URL "xxxx" //<databaseName>.firebaseio.com or <databaseName>.<region>.firebasedatabase.app
#define DATABASE_SECRET "xxxx"
 
// Objetos principais do Firebase
FirebaseData fbdo;
FirebaseAuth auth;
FirebaseConfig config;
int velocidadeManual = 50;
bool modoFollow = false;


#define DEBUG

#define PIN_BUTTON 0
#define SENSOR_R4 34 //sensor lateral Right 4

//Ponte H
#define ENA 23
#define IN1 21
#define IN2 22

#define ENB 5
#define IN3 18
#define IN4 19

#include <QTRSensors.h>

// Instancia do sensor QTR digital
QTRSensors qtr;

//Setup of the module of sensors
const uint8_t SENSOR_COUNT = 8;       // The number of sensors, which should match the length of the pins array
uint16_t sensorValues[SENSOR_COUNT];  // An array in which to store the calibrated sensor readings

// Maximum line position, considering the amount of sensors.
const long MAX_POSITION = (SENSOR_COUNT - 1) * 1000;

//Marker sensor variables
unsigned long initialTime =0;
unsigned long timeLap=30000;
int minLateral = 4095;
int maxLateral = 0;

int restart=0;
char comandoAnterior = 'x'; // Variável para armazenar o último comando recebido
int velAnterior = 0;
// Limit value of the margin of error
int marginError = 20;

bool firstRun = true;

//------------------Firebase Variables-------------------
unsigned long ultimaVerificacao = 0;
const unsigned long intervaloVerificacao = 10000; // 10 segundos

//------------------PID Control-------------------
float p = 0, i = 0, d = 0, pid = 0, error = 0, lastError = 0;

float Kp = 0.3;
float Ki = 0.0001;
float Kd = 3.5;

int maxSpeed = 100;
int integralLimit = 200;
int lSpeed, rSpeed;

const bool LINE_BLACK = false;

bool limiter = false;
//-------------------------------------------------

String getPrefix(String message);
String getElement(String data, int index);
double getNumber(String data, int index);
void printParameters();
void setMotor(int pwmB, int pwmA);
bool markerChecker();
int readSensors();
void atualizarVelocidadeManual();
void atualizarComandoManual();
void conectarWiFi();
void iniciarFirebase();
void checarConexaoFirebase();
void blink();
void printSensors();
void resetCalibration();


void setup() {

  qtr.setTypeRC(); // padrão para sensores digitais QTR
  qtr.setSensorPins((const uint8_t[]){ 32, 33, 25, 26, 27, 14, 12, 13 }, SENSOR_COUNT/* ,35},9 */); // Definindo os pinos conectados aos sensores QRE-8D
  
  pinMode(PIN_BUTTON, INPUT_PULLUP); // Configura o pino do botão como entrada com pull-up interno
  pinMode(LED_BUILTIN, OUTPUT); // initialize digital pin LED_BUILTIN as an output.
  //-----Ponte H-----
  pinMode(ENA, OUTPUT);
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);

  pinMode(ENB, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);

  ledcSetup(0, 10000, 8); // canal PWM 0, 1kHz, 8-bit
  ledcAttachPin(ENA, 0); // PWM para motor esquerdo

  ledcSetup(1, 10000, 8);
  ledcAttachPin(ENB, 1); // PWM para motor direito
  
  //-----Ponte H-----

  Serial.begin(115200);
  conectarWiFi();
  Firebase.reconnectWiFi(true);
  iniciarFirebase();
  

  digitalWrite(LED_BUILTIN, LOW); //modo manual

  Firebase.setInt(fbdo, "/TimeLap", timeLap); // Define o tempo de volta para 30 segundos
  Firebase.setFloat(fbdo, "/pid/Kp", Kp); // Inicializa o nó Kp do PID
  Firebase.setFloat(fbdo, "/pid/Ki", Ki); // Inicializa o nó Ki do PID
  Firebase.setFloat(fbdo, "/pid/Kd", Kd); // Inicializa o nó Kd do PID

  // Configuração do Firebase
  Firebase.setFloat(fbdo, "/TempoDeVoltaFollow", initialTime); // Inicializa o nó de tempo de volta com zero
  Firebase.setFloat(fbdo, "/TempoDeVoltaManual", initialTime); // Inicializa o nó de tempo de volta com zero
  Firebase.setString(fbdo, "/comando", "q"); // Inicializa o nó de comando com uma string vazia 
  Firebase.setInt(fbdo, "/velocidadeManual", velocidadeManual); // Inicializa a velocidade manual no Firebase) 
  Firebase.setBool(fbdo, "/modoFollow", modoFollow);

  // calibration
  resetCalibration(); // Reseta os valores de calibração para os sensores
}

void loop() {

  if (!modoFollow){ //modo manual
    if (firstRun){
      //Firebase.setFloat(fbdo, "/TempoDeVoltaManual", 0); // Inicializa o nó de tempo de volta com zero
      initialTime = millis(); // Save the initial time for the marker checker
      firstRun = false;
    }
    if (millis() - ultimaVerificacao > intervaloVerificacao) {
      checarConexaoFirebase();
      ultimaVerificacao = millis();
    }
    atualizarVelocidadeManual();
    if (restart == 1) { // Resetando os valores do PID
      Kp = 0.3;
      Ki = 0.0001;
      Kd = 3.5;
      integralLimit = 200;
      initialTime = millis();
      restart = 0;
    }
    atualizarComandoManual();
    delay(100);

  }else{ //modo follow
    if (firstRun) { //pegando o PID e calibrando os sensores
      digitalWrite(LED_BUILTIN, HIGH);
      Serial.println("Modo Follow Ativado. Ajuste o PID e o tempo mínimo da volta no Firebase, e aperte o botão para calibrar os sensores.");
      while (digitalRead(PIN_BUTTON) == HIGH){
        if (digitalRead(PIN_BUTTON) == LOW) {  // If the button is pressed, start calibration
          blink();  // Pisca o LED enquanto aguarda o botão ser pressionado
          //atualizando o PID direto do firebase
          if (Firebase.getFloat(fbdo, "/pid/Kp")) {
            Kp = fbdo.floatData();
          }
          if (Firebase.getFloat(fbdo, "/pid/Ki")) {
            Ki = fbdo.floatData();
          }
          if (Firebase.getFloat(fbdo, "/pid/Kd")) {
            Kd = fbdo.floatData();
          }
          if (Firebase.getInt(fbdo, "/TimeLap")) {
            timeLap = fbdo.intData();
          }
          Serial.println("Calibrando...");
          break;
        }
      }
      while (digitalRead(PIN_BUTTON) == HIGH) {  // Calibrates until the button is pressed
        qtr.calibrate();
        int valor = analogRead(SENSOR_R4);
        if (valor < minLateral) minLateral= valor;
        if (valor > maxLateral) maxLateral = valor;
      }

      #ifdef DEBUG
      // Print the calibration minimum values measured when emitters were on
      for (uint8_t i = 0; i < SENSOR_COUNT; i++) {
        Serial.print(qtr.calibrationOn.minimum[i]);
        Serial.print(' ');
      }
      Serial.println();

      // Print the calibration maximum values measured when emitters were on
      for (uint8_t i = 0; i < SENSOR_COUNT; i++) {
        Serial.print(qtr.calibrationOn.maximum[i]);
        Serial.print(' ');
      }
      Serial.println();
      #endif

      Firebase.setInt(fbdo, "/calibration/Min/s1", qtr.calibrationOn.minimum[0]);
      Firebase.setInt(fbdo, "/calibration/Min/s2", qtr.calibrationOn.minimum[1]);
      Firebase.setInt(fbdo, "/calibration/Min/s3", qtr.calibrationOn.minimum[2]);
      Firebase.setInt(fbdo, "/calibration/Min/s4", qtr.calibrationOn.minimum[3]);
      Firebase.setInt(fbdo, "/calibration/Min/s5", qtr.calibrationOn.minimum[4]);
      Firebase.setInt(fbdo, "/calibration/Min/s6", qtr.calibrationOn.minimum[5]);
      Firebase.setInt(fbdo, "/calibration/Min/s7", qtr.calibrationOn.minimum[6]);
      Firebase.setInt(fbdo, "/calibration/Min/s8", qtr.calibrationOn.minimum[7]);
      Firebase.setInt(fbdo, "/calibration/Min/sensorDireita", minLateral); // Salva o valor mínimo do sensor lateral direito
   
      Firebase.setInt(fbdo, "/calibration/Max/s1", qtr.calibrationOn.maximum[0]);
      Firebase.setInt(fbdo, "/calibration/Max/s2", qtr.calibrationOn.maximum[1]);
      Firebase.setInt(fbdo, "/calibration/Max/s3", qtr.calibrationOn.maximum[2]);
      Firebase.setInt(fbdo, "/calibration/Max/s4", qtr.calibrationOn.maximum[3]);
      Firebase.setInt(fbdo, "/calibration/Max/s5", qtr.calibrationOn.maximum[4]);
      Firebase.setInt(fbdo, "/calibration/Max/s6", qtr.calibrationOn.maximum[5]);
      Firebase.setInt(fbdo, "/calibration/Max/s7", qtr.calibrationOn.maximum[6]);
      Firebase.setInt(fbdo, "/calibration/Max/s8", qtr.calibrationOn.maximum[7]);
      Firebase.setInt(fbdo, "/calibration/Max/sensorDireita", maxLateral); // Salva o valor máximo do sensor lateral direito

      digitalWrite(LED_BUILTIN, LOW);
      firstRun = false;
      initialTime = millis(); // Save the initial time for the marker checker
    }else{ //rodando com o PID e sensores calibrados

      // readSensors() returns the line position between 0 and MAX_POSITION.
      // error is a re-map from -1000 to 1000 range.
      error = map(readSensors(), 0, MAX_POSITION, -1000, 1000);

      // Calculate PID
      p = error;
      i = i + error;
      if (limiter == true){
        i = constrain(i, -integralLimit, integralLimit); // Limita o valor do integral
      }
      d = error - lastError;
      pid = (Kp * p) + (Ki * i) + (Kd * d);
      lastError = error;

      // Control Motors
      lSpeed = maxSpeed + pid;
      rSpeed = maxSpeed - pid;

      lSpeed = constrain(lSpeed, -maxSpeed, maxSpeed);
      rSpeed = constrain(rSpeed, -maxSpeed, maxSpeed);

      printSensors(); // Print the sensor values to the serial monitor

      if (markerChecker()) {  // Count the markers and stop the robot when reach a certain number
        //SerialBT.println("End line detected!");
        delay(100);
        setMotor(0, 0);
      #ifdef DEBUG
        float time = millis() - initialTime;
        time = time / 1000.0; // Convert to seconds
        Firebase.setFloat(fbdo, "/TempoDeVoltaFollow", time);
        //SerialBT.print(time);
        //SerialBT.println(" seconds");
        firstRun = true;  // Reset the firstRun flag to true to reinitialize the calibration
        Firebase.setString(fbdo, "/comando", "q"); // Resets the command to stop the robot
        modoFollow = false;
        Firebase.setBool(fbdo, "/modoFollow", false);
        resetCalibration(); // Reseta os valores de calibração para os sensores
      #endif
      } else if (error >= -marginError && error <= marginError) {  // If the error is within the MARGIN_ERROR, move on
        setMotor(maxSpeed, maxSpeed);
      } else {  // If the error is outside the error range, continue doing PID
        setMotor(lSpeed, rSpeed);
      }
    }
    // Regule a resposta do modo follow. Quanto maior o delay, mais lento o robô vai responder às mudanças de linha.
    //delay(500);  
    
    float time = 0;
  }
}

void resetCalibration(){
  // calibration
  Firebase.setInt(fbdo, "/calibration/Min/s1", 2500); // Inicializa os valores mínimos
  Firebase.setInt(fbdo, "/calibration/Min/s2", 2500); // Inicializa os valores mínimos
  Firebase.setInt(fbdo, "/calibration/Min/s3", 2500); // Inicializa os valores mínimos
  Firebase.setInt(fbdo, "/calibration/Min/s4", 2500); // Inicializa os valores mínimos
  Firebase.setInt(fbdo, "/calibration/Min/s5", 2500); // Inicializa os valores mínimos
  Firebase.setInt(fbdo, "/calibration/Min/s6", 2500); // Inicializa os valores mínimos
  Firebase.setInt(fbdo, "/calibration/Min/s7", 2500); // Inicializa os valores mínimos
  Firebase.setInt(fbdo, "/calibration/Min/s8", 2500); // Inicializa os valores mínimos
  Firebase.setInt(fbdo, "/calibration/Min/sensorDireita", 4500); // Inicializa os valores mínimos

  Firebase.setInt(fbdo, "/calibration/Max/s1", 0); // Inicializa os valores máximos
  Firebase.setInt(fbdo, "/calibration/Max/s2", 0); // Inicializa os valores máximos
  Firebase.setInt(fbdo, "/calibration/Max/s3", 0); // Inicializa os valores máximos
  Firebase.setInt(fbdo, "/calibration/Max/s4", 0); // Inicializa os valores máximos
  Firebase.setInt(fbdo, "/calibration/Max/s5", 0); // Inicializa os valores máximos
  Firebase.setInt(fbdo, "/calibration/Max/s6", 0); // Inicializa os valores máximos
  Firebase.setInt(fbdo, "/calibration/Max/s7", 0); // Inicializa os valores máximos
  Firebase.setInt(fbdo, "/calibration/Max/s8", 0); // Inicializa os valores máximos
  Firebase.setInt(fbdo, "/calibration/Max/sensorDireita", 0); // Inicializa os valores máximos
}


void blink(){
  digitalWrite(LED_BUILTIN, LOW); // Turn the LED on
  delay(500);
  digitalWrite(LED_BUILTIN, HIGH); // Turn the LED off
}

void iniciarFirebase() {
  //config.api_key = API_KEY;
  config.database_url = DATABASE_URL;
  Firebase.begin(&config, &auth);
}

void checarConexaoFirebase() {
  if (WiFi.status() != WL_CONNECTED) {
    Serial.println("WiFi desconectado. Reconectando...");
    conectarWiFi();  // sua função já existente
  }

  if (!Firebase.ready()) {
    Serial.println("Firebase não está pronto. Tentando reconectar...");
    Firebase.begin(&config, &auth);
  }
}

void conectarWiFi() {
  WiFi.begin(WIFI_SSID, WIFI_PASSWORD);
  Serial.print("Connecting to Wi-Fi");
  while (WiFi.status() != WL_CONNECTED)
  {
      Serial.print(".");
      delay(300);
  }
  Serial.println();
  Serial.print("Connected with IP: ");
  Serial.println(WiFi.localIP());
  Serial.println();

  Serial.printf("Firebase Client v%s\n\n", FIREBASE_CLIENT_VERSION);

  /* Assign the certificate file (optional) */
  // config.cert.file = "/cert.cer";
  // config.cert.file_storage = StorageType::FLASH;

  /* Assign the database URL and database secret(required) */
  config.database_url = DATABASE_URL;
  config.signer.tokens.legacy_token = DATABASE_SECRET;

  // Comment or pass false value when WiFi reconnection will control by your code or third party library e.g. WiFiManager
  Firebase.reconnectNetwork(true);

  // Since v4.4.x, BearSSL engine was used, the SSL buffer need to be set.
  // Large data transmission may require larger RX buffer, otherwise connection issue or data read time out can be occurred.
  fbdo.setBSSLBufferSize(4096 /* Rx buffer size in bytes from 512 - 16384 */, 1024 /* Tx buffer size in bytes from 512 - 16384 */);

  /* Initialize the library with the Firebase authen and config */
  Firebase.begin(&config, &auth);

  // Or use legacy authenticate method
  // Firebase.begin(DATABASE_URL, DATABASE_SECRET);
}

//print sensors live
void printSensors() {
  Serial.print("Sensor Values: ");
  for (uint8_t i = 0; i < SENSOR_COUNT; i++) {
    Serial.print(sensorValues[i]);
    Serial.print(" ");
  }
  Serial.print("| ");
  int valor = analogRead(SENSOR_R4);
  Serial.print(valor);
  Serial.print("| T:");
  Serial.print(millis() - initialTime);
  Serial.print(" ms");
  Serial.print("| LS:");
  Serial.print(lSpeed);
  Serial.print("| RS:");
  Serial.println(rSpeed);
}

// ====== FUNÇÃO PARA ATUALIZAR velocidadeManual ======
void atualizarVelocidadeManual() {
  if (Firebase.getInt(fbdo, "/velocidadeManual")) {
    if (fbdo.dataType() == "int") {
      velocidadeManual = constrain(fbdo.intData(), 0, 100);
      if (velocidadeManual != velAnterior) { // Verifica se a velocidade mudou
        Serial.print("Velocidade Manual atualizada: ");
        Serial.println(velocidadeManual);
        velAnterior= velocidadeManual; // Atualiza a velocidade anterior para comparação futura
      } 
    }
  } else {
    Serial.print("Erro ao acessar Firebase: ");
    Serial.println(fbdo.errorReason());
  }
}

void atualizarComandoManual() {
  static unsigned long lastCommandTime=0;
  // 1) Lê o comando como string do nó "/comando"
  if ( Firebase.getString(fbdo, "/comando") ) {
    String cmd = fbdo.stringData();
    if (cmd.length() > 0) {
      // 2) Pega o primeiro caractere e converte para minúscula
      char c = tolower(cmd.charAt(0));
      lastCommandTime = millis();
      // 3) Calcula velocidades para cada motor (esq/direita)
      int vel = (velocidadeManual*255)/100;
      int velEsq = 0;
      int velDir = 0;
      float tempoVolta = 0;

      switch (c) {
        case 'w':  // frente
          velEsq = vel;
          velDir = vel;
          break;
        case 's':  // ré
          velEsq = -vel;
          velDir = -vel;
          break;
        case 'a':  // giro à esquerda
          velEsq = vel;
          velDir = 0;
          break;
        case 'd':  // giro à direita
          velEsq = 0;
          velDir = vel;
          break;
        case 'q':  // parada
          velEsq = 0;
          velDir = 0;
          break;
        case 'f': //alterna modo follow
          modoFollow = true;
          firstRun = true; // Reseta o primeiro run para recalibrar os sensores
          tempoVolta = millis() - initialTime; // Calcula o tempo de volta
          Firebase.setFloat(fbdo, "/TempoDeVoltaManual", tempoVolta); 
          Firebase.setBool(fbdo, "/modoFollow", modoFollow);
          Firebase.setString(fbdo, "/comando", "q");
          Serial.println("Modo Follow ativado!");
          break;
        case 'r':  // parada
          firstRun = true; // Reseta o primeiro run para recalibrar os sensores
          tempoVolta = millis() - initialTime;
          Firebase.setFloat(fbdo, "/TempoDeVoltaManual", tempoVolta);
          Firebase.setString(fbdo, "/comando", "q");
          velEsq = 0;
          velDir = 0;
          restart = 1; // Reseta o contador de reinicialização
          break;
        default:
          // caractere inválido: mantém último estado ou zera
          velEsq = 0;
          velDir = 0;
          break;
      }
      // 4) Envia velocidade para os motores
      //    supondo função motor_set_speed(esquerdo, direito)
      setMotor(velEsq, velDir);
      
      if (comandoAnterior != c) {
        Serial.printf("Comando '%c' => Esq: %d | Dir: %d\n", c,  velDir, velEsq);
      }
      lastCommandTime = millis();
      comandoAnterior = c;
    }
  }
  else {
    // em caso de erro na leitura
    setMotor(0, 0); // Para os motores
    Serial.print("Erro ao ler comando: ");
    Serial.println(fbdo.errorReason());
  }
  if (millis() - lastCommandTime > 3000) {
    setMotor(0, 0); // Para após 3 segundos sem comando
  }
}

// Função para controlar os motores individualmente
void setMotor(int pwmB, int pwmA) { // pwmB = esquerda, pwmA = direita
  //trocar o sinal do pwma só pra não ter que trocar os fios dos motores, porque eles estão ruins
  pwmA = -pwmA; // Inverte o sinal do motor esquerdo para que o motor esquerdo gire no sentido correto

  // Motor Esquerdo
  if (pwmB > 0) {
    digitalWrite(IN3, HIGH);
    digitalWrite(IN4, LOW);
  } else {
    digitalWrite(IN3, LOW);
    digitalWrite(IN4, HIGH);
    pwmB = -pwmB;
  }
  ledcWrite(1, constrain(pwmB, 0, 255));

  // Motor Direito
  if (pwmA > 0) {
    digitalWrite(IN1, HIGH);
    digitalWrite(IN2, LOW);\
  } else {
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, HIGH);
    pwmA = -pwmA;
  }
  ledcWrite(0, constrain(pwmA, 0, 255));
}

int readSensors() {
  if (LINE_BLACK) {
    return qtr.readLineBlack(sensorValues);
  } else {
    return qtr.readLineWhite(sensorValues);
  }
}

/**
  Verifies if there is a end line after a set time

  @return true if the end line was detected.
*/
bool markerChecker() {
  if (timeLap< (millis() - initialTime)) {
    if (analogRead(SENSOR_R4) < 2500) {
      return true;
    }
  }
  return false;
}

#ifdef DEBUG
/**
  Returns all stream of data sent over bluetooth until the
  button is pressed.

  @return String with the message sent by the bluetooth device
*/

/**
  Returns a sub-string in the String data, in the index
  position.

  @param data String with the message
  @param index Position of the element to be returned
  @return String sub-string in the indicated position. If there is
  no value at this position, it returns empty string.
*/


void printParameters() {
  Serial.println("Configured parameters:");
  Serial.print(">> P: ");
  Serial.print(Kp, 4);
  Serial.print(" | I: ");
  Serial.print(Ki, 4);
  Serial.print(" | Kd: ");
  Serial.println(Kd, 4);

  Serial.print(">> Speed: ");
  Serial.println(maxSpeed);

  Serial.print(">> Time delay: ");
  Serial.println(timeLap);

  Serial.print(">> Margin Error: ");
  Serial.println(marginError);

  Serial.print(">> Limitador: ");
  Serial.println(limiter);
}

#endif