Build your smart robot

// Hardware Academy course robot
// ESP32 DevKitC + TB6612FNG + 2x N20 encoder gearmotors + HC-SR04 on an SG90 + 2x TCRT5000.
// Target: Arduino IDE 2.x with the esp32 boards package 3.x (board: ESP32 Dev Module), saved as robot/robot.ino.
// Verified October 2026: compiles with no warnings (-Wall -Wextra) on arduino-cli 1.5.1 with esp32 core 3.3.12, FQBN esp32:esp32:esp32.
// Core 3.x API: ledcAttach / ledcWrite take a pin (on core 2.x use ledcSetup + ledcAttachPin).
// Every type is declared before the first function, so the IDE's auto-generated prototypes compile;
// the same file also builds as a plain C++ main.cpp, for example in PlatformIO with an Arduino-ESP32 3.x platform.
// Serial commands at 115200 baud: g = obstacle avoidance, l = line following, s = stop.

#include <Arduino.h>
#include <esp_system.h>

// ---------- Pin map (matches the wiring diagram) ----------
constexpr uint8_t PIN_PWMA = 25, PIN_AIN1 = 26, PIN_AIN2 = 27;  // left motor
constexpr uint8_t PIN_PWMB = 13, PIN_BIN1 = 4, PIN_BIN2 = 16;   // right motor
constexpr uint8_t PIN_STBY = 17;                                // 10k pull-down to GND (the chip has 200k inside)
constexpr uint8_t PIN_ENC_LA = 18, PIN_ENC_LB = 19;
constexpr uint8_t PIN_ENC_RA = 32, PIN_ENC_RB = 33;
constexpr uint8_t PIN_TRIG = 14, PIN_ECHO = 34;                 // ECHO through 1k/2k divider
constexpr uint8_t PIN_SERVO = 23;
constexpr uint8_t PIN_LINE_L = 36, PIN_LINE_R = 39;             // ADC1, safe with Wi-Fi on
constexpr uint8_t PIN_VBAT = 35;                                // 100k/47k divider from the pack

// ---------- Robot constants ----------
constexpr float WHEEL_DIAMETER_MM = 43.0f;
constexpr float TICKS_PER_REV = 1050.0f;          // 7 pulses per motor turn x 150:1 (rising edges of A)
constexpr float MM_PER_TICK = PI * WHEEL_DIAMETER_MM / TICKS_PER_REV;  // 0.1287 mm
constexpr float WHEEL_BASE_MM = 120.0f;           // calibrate with a square test
constexpr float MAX_SPEED_MM_S = 225.0f;          // 100 rpm x pi x 43 mm / 60 at 6 V
constexpr float MOTOR_RATED_V = 6.0f;
constexpr float VBAT_DIVIDER = 147.0f / 47.0f;
constexpr float VBAT_CUTOFF = 6.4f;               // 3.2 V per cell
constexpr uint32_t PWM_FREQ = 20000;              // 20 kHz: above hearing
constexpr uint8_t PWM_BITS = 10;                  // duty 0..1023
constexpr uint32_t LOOP_US = 10000;               // 100 Hz control loop
constexpr uint32_t PING_MS = 60;                  // HC-SR04 needs ~60 ms between pings
constexpr uint32_t TELEMETRY_MS = 100;            // 10 Hz
constexpr int LINE_THRESHOLD = 2000;              // 12-bit ADC: calibrate on your floor and tape

// ---------- Behavior states ----------
// Declared before any function: the Arduino IDE inserts its generated prototypes (such as
// setAvoidState(AvoidState)) above the first function, so these types must already exist there.
enum class Mode { STOPPED, AVOID, LINE };
enum class AvoidState { CRUISE, LOOK, TURN };

// ---------- Motor: one TB6612 channel ----------
class Motor {
 public:
  Motor(uint8_t pwm, uint8_t in1, uint8_t in2, bool reversed)
      : pwm_(pwm), in1_(in1), in2_(in2), reversed_(reversed) {}

  void begin() {
    pinMode(in1_, OUTPUT);
    pinMode(in2_, OUTPUT);
    ledcAttach(pwm_, PWM_FREQ, PWM_BITS);
    set(0.0f, 1.0f);
  }

  // cmd in -1..1; maxDuty (0..1) limits the average motor voltage to its rating
  void set(float cmd, float maxDuty) {
    cmd = constrain(cmd, -1.0f, 1.0f);
    if (reversed_) cmd = -cmd;
    const uint32_t duty = (uint32_t)(fabsf(cmd) * maxDuty * ((1 << PWM_BITS) - 1));
    if (duty == 0) {                                // IN1 = IN2 = LOW: coast
      digitalWrite(in1_, LOW);
      digitalWrite(in2_, LOW);
    } else {
      digitalWrite(in1_, cmd > 0 ? HIGH : LOW);
      digitalWrite(in2_, cmd > 0 ? LOW : HIGH);
    }
    ledcWrite(pwm_, duty);
  }

 private:
  uint8_t pwm_, in1_, in2_;
  bool reversed_;
};

Motor leftMotor(PIN_PWMA, PIN_AIN1, PIN_AIN2, false);
Motor rightMotor(PIN_PWMB, PIN_BIN1, PIN_BIN2, true);  // mounted mirrored

// ---------- Encoders: interrupt on A, direction from B ----------
volatile int32_t ticksL = 0, ticksR = 0;
portMUX_TYPE encMux = portMUX_INITIALIZER_UNLOCKED;

void IRAM_ATTR isrLeft() {
  portENTER_CRITICAL_ISR(&encMux);
  ticksL += digitalRead(PIN_ENC_LB) ? -1 : 1;
  portEXIT_CRITICAL_ISR(&encMux);
}

void IRAM_ATTR isrRight() {
  portENTER_CRITICAL_ISR(&encMux);
  ticksR += digitalRead(PIN_ENC_RB) ? 1 : -1;      // mirrored motor counts the other way
  portEXIT_CRITICAL_ISR(&encMux);
}

void readTicks(int32_t &l, int32_t &r) {
  portENTER_CRITICAL(&encMux);
  l = ticksL;
  r = ticksR;
  portEXIT_CRITICAL(&encMux);
}

// ---------- Ultrasonic: non-blocking echo timing ----------
volatile uint32_t echoStartUs = 0, echoWidthUs = 0;
volatile bool echoReady = false;
float distanceCm = -1.0f;                           // last reading; -1 = unknown (no echo or silent sensor)
uint8_t missedPings = 0;                            // consecutive pings without a valid echo
uint32_t lastEchoMs = 0;
bool sonarDead = true;                              // no echo pulse at all for 250 ms: wire off or module dead

void IRAM_ATTR isrEcho() {
  const uint32_t now = micros();
  if (digitalRead(PIN_ECHO)) {
    echoStartUs = now;
  } else {
    echoWidthUs = now - echoStartUs;
    echoReady = true;
  }
}

void startPing() {
  digitalWrite(PIN_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(PIN_TRIG, LOW);
}

void updateDistance() {
  if (echoReady) {
    echoReady = false;
    const uint32_t w = echoWidthUs;
    lastEchoMs = millis();
    if (w > 0 && w < 23200) {                                // 58 us per cm: a real echo within 4 m
      distanceCm = w / 58.0f;
      missedPings = 0;
    } else {                                                 // ~38 ms pulse = no echo came back. An angled
      distanceCm = -1.0f;                                    // wall can reflect the ping away, so this is
      if (missedPings < 255) missedPings++;                  // UNKNOWN, never free space.
    }
  }
  sonarDead = millis() - lastEchoMs > 250;                   // not even a timeout pulse
  if (sonarDead) distanceCm = -1.0f;
}

// ---------- Servo on LEDC: 50 Hz, 14-bit ----------
void servoWriteDeg(float deg) {
  deg = constrain(deg, 0.0f, 180.0f);
  const float us = 500.0f + deg * (1900.0f / 180.0f);       // 0.5 to 2.4 ms
  ledcWrite(PIN_SERVO, (uint32_t)(us / 20000.0f * 16383.0f));
}

// ---------- Line sensors and battery ----------
int lineRawL = 0, lineRawR = 0;
bool onLineL = false, onLineR = false;
float vbat = 8.0f;

void readSensors() {
  lineRawL = analogRead(PIN_LINE_L);                         // higher over black tape
  lineRawR = analogRead(PIN_LINE_R);
  onLineL = lineRawL > LINE_THRESHOLD;
  onLineR = lineRawR > LINE_THRESHOLD;
  const float v = analogReadMilliVolts(PIN_VBAT) / 1000.0f * VBAT_DIVIDER;
  vbat = 0.95f * vbat + 0.05f * v;                           // low-pass: motor current makes it noisy
}

// ---------- Odometry and wheel speed ----------
float poseX = 0, poseY = 0, poseTh = 0;                      // mm, mm, rad
float speedL = 0, speedR = 0;                                // mm/s
int32_t lastTicksL = 0, lastTicksR = 0;

void updateOdometry(float dt) {
  int32_t l, r;
  readTicks(l, r);
  const float dsL = (l - lastTicksL) * MM_PER_TICK;
  const float dsR = (r - lastTicksR) * MM_PER_TICK;
  lastTicksL = l;
  lastTicksR = r;
  speedL = 0.7f * speedL + 0.3f * (dsL / dt);                // ~17 ticks per 10 ms at full speed
  speedR = 0.7f * speedR + 0.3f * (dsR / dt);
  const float ds = 0.5f * (dsL + dsR);
  const float dth = (dsR - dsL) / WHEEL_BASE_MM;
  poseX += ds * cosf(poseTh + 0.5f * dth);
  poseY += ds * sinf(poseTh + 0.5f * dth);
  poseTh = atan2f(sinf(poseTh + dth), cosf(poseTh + dth));
}

// ---------- Per-wheel speed loop: feedforward + PI ----------
struct SpeedPI {
  float kp = 0.002f;                                         // command per mm/s of error
  float ki = 0.010f;                                         // command per (mm/s x s)
  float integral = 0;
  float update(float target, float measured, float dt) {
    if (target == 0.0f) { integral = 0; return 0.0f; }       // stop means stop
    const float err = target - measured;
    integral = constrain(integral + err * dt, -40.0f, 40.0f);  // anti-windup: |ki x integral| <= 0.4
    return target / MAX_SPEED_MM_S + kp * err + ki * integral;
  }
};
SpeedPI piL, piR;

// ---------- Behaviors ----------
Mode mode = Mode::STOPPED;
AvoidState avoidState = AvoidState::CRUISE;
uint32_t stateSinceMs = 0;
float lookLeftCm = 0, lookRightCm = 0, turnStartTh = 0, turnTarget = 0;
float lastLineSide = 1;                                      // +1: line last seen on the left
uint32_t lineSeenMs = 0;

void setAvoidState(AvoidState s) {
  avoidState = s;
  stateSinceMs = millis();
}

// Each behavior writes target wheel speeds in mm/s.
void obstacleAvoid(float &vL, float &vR) {
  const uint32_t inState = millis() - stateSinceMs;
  switch (avoidState) {
    case AvoidState::CRUISE: {
      servoWriteDeg(90);
      if (sonarDead) {                                       // no sonar at all: never drive blind
        vL = vR = 0;
        return;
      }
      if (missedPings >= 3) {                                // ~180 ms without an echo: stop and look around
        setAvoidState(AvoidState::LOOK);
        vL = vR = 0;
        return;
      }
      if (distanceCm < 0.0f) {                               // one or two missed echoes: creep, don't assume free
        vL = vR = 50.0f;
        return;
      }
      if (distanceCm < 25.0f) {                              // obstacle: stop and look around
        setAvoidState(AvoidState::LOOK);
        vL = vR = 0;
        return;
      }
      const float k = constrain((distanceCm - 25.0f) / 55.0f, 0.3f, 1.0f);  // slow down near things
      vL = vR = 160.0f * k;
      return;
    }
    case AvoidState::LOOK:                                   // stop, look left, look right
      vL = vR = 0;
      if (inState < 450) {
        servoWriteDeg(160);
        lookLeftCm = distanceCm;                             // last reading before 450 ms wins; unknown (-1) loses
      } else if (inState < 900) {
        servoWriteDeg(20);
        lookRightCm = distanceCm;
      } else {
        servoWriteDeg(90);
        turnStartTh = poseTh;
        turnTarget = (lookLeftCm >= lookRightCm ? 1.0f : -1.0f) * HALF_PI;   // + = turn left
        setAvoidState(AvoidState::TURN);
      }
      return;
    case AvoidState::TURN: {
      const float turned = atan2f(sinf(poseTh - turnStartTh), cosf(poseTh - turnStartTh));
      if (fabsf(turned) >= 0.95f * fabsf(turnTarget) || inState > 2500) {   // done, or timed out
        setAvoidState(AvoidState::CRUISE);
        vL = vR = 0;
        return;
      }
      const float s = turnTarget > 0 ? 1.0f : -1.0f;
      vL = -s * 90.0f;                                       // spin in place
      vR = s * 90.0f;
      return;
    }
  }
}

void lineFollow(float &vL, float &vR) {
  // The sensors sit side by side about 14 mm apart: both over 19 mm tape when centered.
  if (onLineL && !onLineR) lastLineSide = 1;
  if (onLineR && !onLineL) lastLineSide = -1;
  if (onLineL || onLineR) {
    lineSeenMs = millis();
    const float e = (onLineL ? 1.0f : 0.0f) - (onLineR ? 1.0f : 0.0f);  // +1: steer left
    vL = 120.0f - 70.0f * e;
    vR = 120.0f + 70.0f * e;
  } else if (millis() - lineSeenMs < 3000) {
    vL = -60.0f * lastLineSide;                              // spin toward the last-seen side
    vR = 60.0f * lastLineSide;
  } else {
    vL = vR = 0;                                             // give up after 3 s
  }
  if (sonarDead || (distanceCm >= 0.0f && distanceCm < 15.0f)) {
    vL = vR = 0;                                             // never drive into something, or blind
  } else if (distanceCm < 0.0f) {
    vL *= 0.5f;                                              // no echo: unknown, so slow down
    vR *= 0.5f;
  }
}

// ---------- 100 Hz control step ----------
float cmdL = 0, cmdR = 0;

void controlStep(float dt) {
  updateOdometry(dt);
  readSensors();
  if (vbat < VBAT_CUTOFF && mode != Mode::STOPPED) {
    mode = Mode::STOPPED;
    Serial.println("battery low: stopped");
  }
  float vL = 0, vR = 0;
  if (mode == Mode::AVOID) obstacleAvoid(vL, vR);
  if (mode == Mode::LINE) lineFollow(vL, vR);
  cmdL = piL.update(vL, speedL, dt);
  cmdR = piR.update(vR, speedR, dt);
  const float maxDuty = min(1.0f, MOTOR_RATED_V / max(vbat, 1.0f));  // 71% on a full pack
  leftMotor.set(cmdL, maxDuty);
  rightMotor.set(cmdR, maxDuty);
}

// ---------- Serial commands and telemetry ----------
void handleSerial() {
  while (Serial.available()) {
    switch (Serial.read()) {
      case 'g': mode = Mode::AVOID; setAvoidState(AvoidState::CRUISE); break;
      case 'l': mode = Mode::LINE; lineSeenMs = millis(); break;
      case 's': mode = Mode::STOPPED; break;
      default: break;
    }
  }
}

void printTelemetry() {
  int32_t l, r;
  readTicks(l, r);
  // CSV: t_ms,mode,vbat,ticksL,ticksR,speedL,speedR,x_mm,y_mm,theta_deg,dist_cm,lineL,lineR,cmdL,cmdR
  Serial.printf("%lu,%d,%.2f,%ld,%ld,%.0f,%.0f,%.0f,%.0f,%.1f,%.1f,%d,%d,%.2f,%.2f\n",
                (unsigned long)millis(), (int)mode, vbat, (long)l, (long)r, speedL, speedR,
                poseX, poseY, poseTh * RAD_TO_DEG, distanceCm, lineRawL, lineRawR, cmdL, cmdR);
}

// ---------- setup and loop ----------
uint32_t lastLoopUs = 0, lastPingMs = 0, lastTelemetryMs = 0;

void setup() {
  Serial.begin(115200);
  pinMode(PIN_STBY, OUTPUT);
  digitalWrite(PIN_STBY, LOW);                               // driver off until everything is set up

  leftMotor.begin();
  rightMotor.begin();

  pinMode(PIN_ENC_LA, INPUT_PULLUP);
  pinMode(PIN_ENC_LB, INPUT_PULLUP);
  pinMode(PIN_ENC_RA, INPUT_PULLUP);
  pinMode(PIN_ENC_RB, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(PIN_ENC_LA), isrLeft, RISING);
  attachInterrupt(digitalPinToInterrupt(PIN_ENC_RA), isrRight, RISING);

  pinMode(PIN_TRIG, OUTPUT);
  digitalWrite(PIN_TRIG, LOW);
  pinMode(PIN_ECHO, INPUT);                                  // GPIO 34 is input-only, no pull-up
  attachInterrupt(digitalPinToInterrupt(PIN_ECHO), isrEcho, CHANGE);

  ledcAttach(PIN_SERVO, 50, 14);
  servoWriteDeg(90);

  vbat = analogReadMilliVolts(PIN_VBAT) / 1000.0f * VBAT_DIVIDER;
  Serial.printf("reset reason: %s, battery %.2f V\n",
                esp_reset_reason() == ESP_RST_BROWNOUT ? "BROWNOUT" : "normal", vbat);

  digitalWrite(PIN_STBY, HIGH);                              // H-bridges live, duty still zero
  lastLoopUs = micros();
  Serial.println("ready: g = obstacle avoid, l = line follow, s = stop");
}

void loop() {
  handleSerial();
  updateDistance();
  const uint32_t nowMs = millis();
  if (nowMs - lastPingMs >= PING_MS) {
    lastPingMs = nowMs;
    startPing();
  }
  if (micros() - lastLoopUs >= LOOP_US) {                    // fixed 100 Hz, no delay() anywhere
    lastLoopUs += LOOP_US;
    controlStep(LOOP_US / 1e6f);
  }
  if (nowMs - lastTelemetryMs >= TELEMETRY_MS) {
    lastTelemetryMs = nowMs;
    printTelemetry();
  }
}