ESP32 避障小車：用超音波做一台會自己閃開障礙物的車

/*
 * TinyBot Forge — ESP32 避障小車
 * https://tinybotforge.com
 *
 * 硬體：ESP32 DevKit V1、HC-SR04 超音波感測器、L298N 馬達驅動板、兩顆 TT 直流馬達
 * 環境：Arduino IDE 2.x + ESP32 開發板套件 3.x（使用 ledcAttach / ledcWrite 新版 API）
 *
 * 注意：HC-SR04 的 ECHO 腳輸出 5V，ESP32 腳位只能承受 3.3V，
 *       ECHO 必須經過分壓電阻（1kΩ + 2kΩ）再接到 GPIO 19。
 */

// ---------- 腳位 ----------
const int PIN_TRIG = 18;   // HC-SR04 TRIG
const int PIN_ECHO = 19;   // HC-SR04 ECHO（經分壓）

const int PIN_ENA = 25;    // 左馬達速度（PWM）
const int PIN_IN1 = 26;    // 左馬達方向
const int PIN_IN2 = 27;
const int PIN_ENB = 33;    // 右馬達速度（PWM）
const int PIN_IN3 = 32;    // 右馬達方向
const int PIN_IN4 = 13;

// ---------- 參數（可依你的車調整） ----------
const int PWM_FREQ = 1000;      // PWM 頻率 1 kHz
const int PWM_BITS = 8;         // 解析度 8 bit → 速度 0–255
const int SPEED_CRUISE = 170;   // 直走速度
const int SPEED_TURN   = 160;   // 轉彎速度
const float STOP_CM    = 20.0;  // 小於這個距離就閃避
const unsigned long BACK_MS = 350;  // 倒車時間
const unsigned long TURN_MS = 450;  // 轉彎時間

// ---------- 馬達 ----------
// speed：-255（全速後退）到 255（全速前進）
void setMotor(int pinEn, int pinA, int pinB, int speed) {
  speed = constrain(speed, -255, 255);
  digitalWrite(pinA, speed > 0 ? HIGH : LOW);
  digitalWrite(pinB, speed < 0 ? HIGH : LOW);
  ledcWrite(pinEn, abs(speed));
}

void drive(int left, int right) {
  setMotor(PIN_ENA, PIN_IN1, PIN_IN2, left);
  setMotor(PIN_ENB, PIN_IN3, PIN_IN4, right);
}

void stopCar() { drive(0, 0); }

// ---------- 超音波測距 ----------
// 回傳公分；超過量測範圍回傳 999
float readOnceCm() {
  digitalWrite(PIN_TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(PIN_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(PIN_TRIG, LOW);
  unsigned long us = pulseIn(PIN_ECHO, HIGH, 25000UL);  // 25 ms ≈ 4 公尺
  if (us == 0) return 999.0;
  return us / 58.0;  // 聲速換算：來回 1 cm ≈ 58 µs
}

// 量三次取中位數，濾掉偶發的錯誤讀數
float readDistanceCm() {
  float a = readOnceCm(); delay(10);
  float b = readOnceCm(); delay(10);
  float c = readOnceCm();
  if (a > b) { float t = a; a = b; b = t; }
  if (b > c) { float t = b; b = c; c = t; }
  if (a > b) { float t = a; a = b; b = t; }
  return b;
}

// ---------- 閃避 ----------
void avoid() {
  stopCar();
  delay(100);
  drive(-SPEED_CRUISE, -SPEED_CRUISE);   // 倒車
  delay(BACK_MS);

  drive(SPEED_TURN, -SPEED_TURN);        // 先試右轉
  delay(TURN_MS);
  stopCar();
  delay(80);
  if (readDistanceCm() > STOP_CM * 1.5) return;

  drive(-SPEED_TURN, SPEED_TURN);        // 右邊也擋住，改往左轉兩倍時間
  delay(TURN_MS * 2);
  stopCar();
  delay(80);
}

void setup() {
  Serial.begin(115200);

  pinMode(PIN_TRIG, OUTPUT);
  pinMode(PIN_ECHO, INPUT);
  pinMode(PIN_IN1, OUTPUT);
  pinMode(PIN_IN2, OUTPUT);
  pinMode(PIN_IN3, OUTPUT);
  pinMode(PIN_IN4, OUTPUT);

  ledcAttach(PIN_ENA, PWM_FREQ, PWM_BITS);
  ledcAttach(PIN_ENB, PWM_FREQ, PWM_BITS);

  stopCar();
  Serial.println("TinyBot 避障小車：2 秒後出發");
  delay(2000);
}

void loop() {
  float cm = readDistanceCm();
  Serial.printf("距離 %.1f cm\n", cm);

  if (cm < STOP_CM) {
    avoid();
  } else {
    drive(SPEED_CRUISE, SPEED_CRUISE);
  }
  delay(40);
}