🛞 Projeto: rover que desvia de obstáculos

// Teste isolado do HC-SR04 (ESP32, pacote esp32 3.x)
const int PINO_TRIG = 18, PINO_ECHO = 19;   // ECHO pelo divisor 1k/2k
void setup() {
  Serial.begin(115200);
  pinMode(PINO_TRIG, OUTPUT); digitalWrite(PINO_TRIG, LOW);
  pinMode(PINO_ECHO, INPUT);
}
void loop() {
  digitalWrite(PINO_TRIG, HIGH); delayMicroseconds(10); digitalWrite(PINO_TRIG, LOW);
  unsigned long us = pulseIn(PINO_ECHO, HIGH, 30000UL);  // espera no maximo 30 ms
  if (us == 0) Serial.println("sem eco (nada ate ~4 m, ou fio do ECHO solto)");
  else Serial.printf("distancia: %.1f cm\n", us / 58.0);  // 58 us por cm, ida e volta
  delay(100);
}

// Rover que desvia de obstaculos - Robotica do Zero ao Expert, projeto 1.6
// ESP32 + HC-SR04 + 2 motores DC (TB6612FNG ou DRV8833) + servo opcional
// Pacote esp32 3.x (ledcAttach / ledcWrite) e biblioteca ESP32Servo
#include <ESP32Servo.h>
// ---------- escolha o hardware ----------
#define USAR_DRV8833 0   // 0 = TB6612FNG, 1 = DRV8833
#define USAR_SERVO   1   // 0 = sensor fixo, olhando para a frente
// ---------- pinos (mapa do topico 1) ----------
const int PINO_PWMA = 25, PINO_AIN1 = 26, PINO_AIN2 = 27;  // motor esquerdo
const int PINO_PWMB = 33, PINO_BIN1 = 32, PINO_BIN2 = 4;   // motor direito
const int PINO_STBY = 13;          // STBY (TB6612) ou nSLEEP (DRV8833)
const int PINO_TRIG = 18, PINO_ECHO = 19;  // ECHO pelo divisor 1k/2k
const int PINO_SERVO = 23;
const int PINO_BOTAO = 0;          // botao BOOT da placa: pausa / continua
// ---------- ajustes (topico 5) ----------
const int PWM_FREQ = 20000, PWM_BITS = 8;
const int VEL_MAX = 180;   // teto: cerca de 6 V medios com 2x 18650 (4x AA NiMH: 255)
const int VEL_MIN = 90;    // zona morta do motor TT
const int VEL_FRENTE = 170, VEL_GIRO = 160;
const int AJUSTE_ESQ = 0, AJUSTE_DIR = 0;  // some +10 no motor mais fraco
const float DIST_PARAR_CM = 25.0;
const unsigned long T_PARAR = 200, T_RE = 350, T_GIRO_90 = 380;  // em ms
#if USAR_SERVO
Servo cabeca;
#endif
enum Estado { ANDAR, PARAR, RE, OLHAR, VIRAR };
const char* NOME[] = {"ANDAR", "PARAR", "RE", "OLHAR", "VIRAR"};
Estado estado = PARAR;
unsigned long inicioEstado = 0, ultimaLeitura = 0, tGiro = T_GIRO_90;
int ladoGiro = 1;            // +1 = direita, -1 = esquerda
bool pausado = true;         // liga parado: aperte BOOT para andar
float hist[3] = {400, 400, 400}; int iHist = 0;  // ultimas 3 leituras
// ---------- motores ----------
void motor(int in1, int in2, int pinoPwm, int vel) {
  vel = constrain(vel, -VEL_MAX, VEL_MAX);
  int forca = abs(vel);
  if (forca > 0 && forca < VEL_MIN) forca = VEL_MIN;
#if USAR_DRV8833
  ledcWrite(in1, vel > 0 ? forca : 0);       // DRV8833: PWM nos dois pinos de entrada
  ledcWrite(in2, vel < 0 ? forca : 0);
#else
  digitalWrite(in1, vel > 0 ? HIGH : LOW);   // TB6612: sentido nos IN, forca no PWM
  digitalWrite(in2, vel < 0 ? HIGH : LOW);
  ledcWrite(pinoPwm, forca);
#endif
}
void andar(int esq, int dir) {   // -255 a 255 em cada roda
  if (esq != 0) esq += (esq > 0) ? AJUSTE_ESQ : -AJUSTE_ESQ;
  if (dir != 0) dir += (dir > 0) ? AJUSTE_DIR : -AJUSTE_DIR;
  motor(PINO_AIN1, PINO_AIN2, PINO_PWMA, esq);
  motor(PINO_BIN1, PINO_BIN2, PINO_PWMB, dir);
}
// ---------- sensor ----------
float medirCm() {
  digitalWrite(PINO_TRIG, HIGH); delayMicroseconds(10); digitalWrite(PINO_TRIG, LOW);
  unsigned long us = pulseIn(PINO_ECHO, HIGH, 30000UL);
  return (us == 0) ? 400.0 : us / 58.0;    // sem eco = nada perto (cerca de 4 m)
}
float mediana3(float a, float b, float c) { return max(min(a, b), min(max(a, b), c)); }
float distanciaAndando() {               // 1 leitura nova + as 2 anteriores
  hist[iHist] = medirCm(); iHist = (iHist + 1) % 3;
  return mediana3(hist[0], hist[1], hist[2]);
}
float distanciaParado() {                // 3 leituras novas, 60 ms entre elas
  float a = medirCm(); delay(60);
  float b = medirCm(); delay(60);
  return mediana3(a, b, medirCm());
}
// ---------- maquina de estados ----------
void mudar(Estado novo) {
  estado = novo; inicioEstado = millis();
  Serial.printf("-> %s\n", NOME[novo]);
}
void escolherLado() {                    // se o servo olhar ao contrario, troque 20 e 160
#if USAR_SERVO
  cabeca.write(20);  delay(400); float dDir = distanciaParado();
  cabeca.write(160); delay(600); float dEsq = distanciaParado();
  cabeca.write(90);  delay(300);
  Serial.printf("direita %.0f cm | esquerda %.0f cm\n", dDir, dEsq);
  ladoGiro = (dDir >= dEsq) ? 1 : -1;
  tGiro = (max(dDir, dEsq) < DIST_PARAR_CM) ? 2 * T_GIRO_90 : T_GIRO_90;  // beco: meia-volta
#else
  ladoGiro = -ladoGiro;                  // sem servo: alterna o lado
  tGiro = T_GIRO_90;
#endif
}
void testar(char c) {                   // testes isolados pelo Serial Monitor
  if (c == 's') Serial.printf("distancia: %.1f cm\n", distanciaParado());
  if (c == 'e') { Serial.println("motor esquerdo, frente"); andar(VEL_FRENTE, 0); delay(1000); andar(0, 0); }
  if (c == 'd') { Serial.println("motor direito, frente"); andar(0, VEL_FRENTE); delay(1000); andar(0, 0); }
#if USAR_SERVO
  if (c == 'c') { Serial.println("servo: direita, esquerda, frente"); cabeca.write(20); delay(600); cabeca.write(160); delay(800); cabeca.write(90); }
#endif
}
void setup() {
  Serial.begin(115200);
  pinMode(PINO_BOTAO, INPUT_PULLUP);
  pinMode(PINO_STBY, OUTPUT); digitalWrite(PINO_STBY, HIGH);
#if USAR_DRV8833
  ledcAttach(PINO_AIN1, PWM_FREQ, PWM_BITS); ledcAttach(PINO_AIN2, PWM_FREQ, PWM_BITS);
  ledcAttach(PINO_BIN1, PWM_FREQ, PWM_BITS); ledcAttach(PINO_BIN2, PWM_FREQ, PWM_BITS);
#else
  pinMode(PINO_AIN1, OUTPUT); pinMode(PINO_AIN2, OUTPUT);
  pinMode(PINO_BIN1, OUTPUT); pinMode(PINO_BIN2, OUTPUT);
  ledcAttach(PINO_PWMA, PWM_FREQ, PWM_BITS); ledcAttach(PINO_PWMB, PWM_FREQ, PWM_BITS);
#endif
  andar(0, 0);
  pinMode(PINO_TRIG, OUTPUT); digitalWrite(PINO_TRIG, LOW); pinMode(PINO_ECHO, INPUT);
#if USAR_SERVO
  ESP32PWM::allocateTimer(3);            // servo num timer separado dos motores
  cabeca.setPeriodHertz(50);
  cabeca.attach(PINO_SERVO, 500, 2400);
  cabeca.write(90);
#endif
  Serial.println("Rover pronto e PAUSADO. Aperte BOOT para andar.");
  Serial.println("Pausado, teste: s=sensor e=motor esq d=motor dir c=servo");
}
void loop() {
  static bool botaoAntes = HIGH;
  bool botao = digitalRead(PINO_BOTAO);
  if (botaoAntes == HIGH && botao == LOW) {      // acabou de apertar
    pausado = !pausado; andar(0, 0); delay(250);   // 250 ms filtram o repique
    Serial.println(pausado ? "PAUSADO" : "ANDANDO");
    if (!pausado) { hist[0] = hist[1] = hist[2] = 400; mudar(ANDAR); }
  }
  botaoAntes = botao;
  if (pausado) { if (Serial.available()) testar(Serial.read()); return; }
  unsigned long agora = millis(), noEstado = agora - inicioEstado;
  switch (estado) {
    case ANDAR:
      andar(VEL_FRENTE, VEL_FRENTE);
      if (agora - ultimaLeitura >= 60) {          // HC-SR04: uma leitura a cada 60 ms
        ultimaLeitura = agora;
        if (distanciaAndando() < DIST_PARAR_CM) mudar(PARAR);
      }
      break;
    case PARAR:
      andar(0, 0);
      if (noEstado >= T_PARAR) mudar(RE);
      break;
    case RE:
      andar(-VEL_GIRO, -VEL_GIRO);
      if (noEstado >= T_RE) { andar(0, 0); mudar(OLHAR); }
      break;
    case OLHAR:
      escolherLado();                              // robo parado, cerca de 1,5 s
      mudar(VIRAR);
      break;
    case VIRAR:
      andar(VEL_GIRO * ladoGiro, -VEL_GIRO * ladoGiro);
      if (noEstado >= tGiro) {
        andar(0, 0);
        hist[0] = hist[1] = hist[2] = 400;         // esquece as leituras antigas
        mudar(ANDAR);
      }
      break;
  }
}