🛞 Project: obstacle-avoiding rover

// Isolated HC-SR04 test (ESP32, esp32 package 3.x)
const int PINO_TRIG = 18, PINO_ECHO = 19;   // ECHO through 1k/2k divider
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);  // waits at most 30 ms
  if (us == 0) Serial.println("no echo (nothing up to ~4 m, or ECHO wire disconnected)");
  else Serial.printf("distance: %.1f cm\n", us / 58.0);  // 58 us per cm, round trip
  delay(100);
}

// Obstacle-avoiding rover - Robotics from Zero to Expert, project 1.6
// ESP32 + HC-SR04 + 2 DC motors (TB6612FNG or DRV8833) + optional servo
// esp32 package 3.x (ledcAttach / ledcWrite) and ESP32Servo library
#include <ESP32Servo.h>
// ---------- choose the hardware ----------
#define USAR_DRV8833 0   // 0 = TB6612FNG, 1 = DRV8833
#define USAR_SERVO   1   // 0 = fixed sensor, facing forward
// ---------- pins (map from topic 1) ----------
const int PINO_PWMA = 25, PINO_AIN1 = 26, PINO_AIN2 = 27;  // left motor
const int PINO_PWMB = 33, PINO_BIN1 = 32, PINO_BIN2 = 4;   // right motor
const int PINO_STBY = 13;          // STBY (TB6612) or nSLEEP (DRV8833)
const int PINO_TRIG = 18, PINO_ECHO = 19;  // ECHO through 1k/2k divider
const int PINO_SERVO = 23;
const int PINO_BOTAO = 0;          // board BOOT button: pause / resume
// ---------- adjustments (topic 5) ----------
const int PWM_FREQ = 20000, PWM_BITS = 8;
const int VEL_MAX = 180;   // limit: around 6 V average with 2x 18650 (4x AA NiMH: 255)
const int VEL_MIN = 90;    // TT motor dead zone
const int VEL_FRENTE = 170, VEL_GIRO = 160;
const int AJUSTE_ESQ = 0, AJUSTE_DIR = 0;  // add +10 to the weaker motor
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 = right, -1 = left
bool pausado = true;         // starts stopped: press BOOT to move
float hist[3] = {400, 400, 400}; int iHist = 0;  // last 3 readings
// ---------- motors ----------
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 on both input pins
  ledcWrite(in2, vel < 0 ? forca : 0);
#else
  digitalWrite(in1, vel > 0 ? HIGH : LOW);   // TB6612: direction on IN, power on PWM
  digitalWrite(in2, vel < 0 ? HIGH : LOW);
  ledcWrite(pinoPwm, forca);
#endif
}
void andar(int esq, int dir) {   // -255 to 255 for each wheel
  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;    // no echo = nothing nearby (around 4 m)
}
float mediana3(float a, float b, float c) { return max(min(a, b), min(max(a, b), c)); }
float distanciaAndando() {               // 1 new reading + the 2 previous ones
  hist[iHist] = medirCm(); iHist = (iHist + 1) % 3;
  return mediana3(hist[0], hist[1], hist[2]);
}
float distanciaParado() {                // 3 new readings, 60 ms apart
  float a = medirCm(); delay(60);
  float b = medirCm(); delay(60);
  return mediana3(a, b, medirCm());
}
// ---------- state machine ----------
void mudar(Estado novo) {
  estado = novo; inicioEstado = millis();
  Serial.printf("-> %s\n", NOME[novo]);
}
void escolherLado() {                    // if the servo looks the wrong way, swap 20 and 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("right %.0f cm | left %.0f cm\n", dDir, dEsq);
  ladoGiro = (dDir >= dEsq) ? 1 : -1;
  tGiro = (max(dDir, dEsq) < DIST_PARAR_CM) ? 2 * T_GIRO_90 : T_GIRO_90;  // dead end: turn around
#else
  ladoGiro = -ladoGiro;                  // without a servo: alternate sides
  tGiro = T_GIRO_90;
#endif
}
void testar(char c) {                   // isolated tests through the Serial Monitor
  if (c == 's') Serial.printf("distance: %.1f cm\n", distanciaParado());
  if (c == 'e') { Serial.println("left motor, forward"); andar(VEL_FRENTE, 0); delay(1000); andar(0, 0); }
  if (c == 'd') { Serial.println("right motor, forward"); andar(0, VEL_FRENTE); delay(1000); andar(0, 0); }
#if USAR_SERVO
  if (c == 'c') { Serial.println("servo: right, left, forward"); 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 on a timer separate from the motors
  cabeca.setPeriodHertz(50);
  cabeca.attach(PINO_SERVO, 500, 2400);
  cabeca.write(90);
#endif
  Serial.println("Rover ready and PAUSED. Press BOOT to move.");
  Serial.println("Paused, test: s=sensor e=left motor d=right motor c=servo");
}
void loop() {
  static bool botaoAntes = HIGH;
  bool botao = digitalRead(PINO_BOTAO);
  if (botaoAntes == HIGH && botao == LOW) {      // just pressed
    pausado = !pausado; andar(0, 0); delay(250);   // 250 ms filters out button bounce
    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: one reading every 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();                              // robot stopped for about 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;         // discard old readings
        mudar(ANDAR);
      }
      break;
  }
}