ESP32 Remote-Controlled Car

#include <Arduino.h>
#include <Adafruit_NeoPixel.h>
#include <WiFi.h>
#include <ESP32Servo.h>
#include "esp_camera.h"
#include "img_converters.h" // המרת תמונת המצלמה לפורמט JPEG
#include "esp_http_server.h"
#include "esp_wifi.h"
#include "esp_random.h"
#include "esp_err.h"
#include "esp_log.h"
#include <lwip/sockets.h>
#include <lwip/tcp.h>

#define PWDN_GPIO_NUM    -1
#define RESET_GPIO_NUM   -1
#define XCLK_GPIO_NUM    15
#define SIOD_GPIO_NUM     4
#define SIOC_GPIO_NUM     5
#define Y9_GPIO_NUM      16
#define Y8_GPIO_NUM      17
#define Y7_GPIO_NUM      18
#define Y6_GPIO_NUM      12
#define Y5_GPIO_NUM      10
#define Y4_GPIO_NUM       8
#define Y3_GPIO_NUM       9
#define Y2_GPIO_NUM      11
#define VSYNC_GPIO_NUM    6
#define HREF_GPIO_NUM     7
#define PCLK_GPIO_NUM    13

void applyLightsValues(bool on, uint8_t r, uint8_t g, uint8_t b);
esp_err_t indexHandler(httpd_req_t *request);
esp_err_t controlHandler(httpd_req_t *request);
esp_err_t statusHandler(httpd_req_t *request);
esp_err_t streamHandler(httpd_req_t *request);
bool startCamera();
bool startServer(httpd_handle_t *server, uint16_t port, const httpd_uri_t *uris, size_t uriCount);
bool startWebServers();

const int MOTOR_AIN1_PIN = 1;
const int MOTOR_AIN2_PIN = 2;
const int MOTOR_PWMA_PIN = 14;
const uint8_t MOTOR_LEDC_CHANNEL = 2;
const int SERVO_PIN = 21;

const bool LIGHTS_ENABLED = true;

const int FRONT_LEFT_LIGHT_PIN = 3;
const int FRONT_RIGHT_LIGHT_PIN = 41;
const int REAR_LEFT_LIGHT_PIN = 42;
const int REAR_RIGHT_LIGHT_PIN = 47;

const char *AP_NAME = "RC-CAR-CAM";
const char *AP_PASSWORD = "drivecar";
const uint16_t CONTROL_SERVER_PORT = 80;
const uint16_t STREAM_SERVER_PORT = 81;

const uint8_t DRIVE_POWER_SLOW = 105;
const uint8_t DRIVE_POWER_FAST = 255; // הספק מרבי מלא ברזולוציה של שמונה סיביות
const int STEER_LEFT = 55;
const int STEER_CENTRE = 90;
const int STEER_RIGHT = 125;
const unsigned long COMMAND_TIMEOUT_MS = 700;
const unsigned long RELEASE_RAMP_MS = 300;
const unsigned long MOTOR_DIR_DEADTIME_MS = 100;

Servo steering;
httpd_handle_t controlServer = nullptr;
httpd_handle_t streamServer = nullptr;
bool motorPwmReady = false;
bool cameraReady = false;
esp_err_t cameraError = ESP_ERR_INVALID_STATE;

portMUX_TYPE controlMux = portMUX_INITIALIZER_UNLOCKED;

volatile int pendingThrottle = 0;
volatile bool throttleDirty = false;
volatile int pendingSteer = 0;
volatile bool steerDirty = false;
volatile bool stopDirty = false;
volatile unsigned long long lastAcceptedTs = 0;
volatile bool needFreshDrive = true;
volatile bool needFreshSteer = true;
volatile unsigned long generation = 0;
volatile bool lightsOn = false;
volatile uint8_t lightR = 255, lightG = 255, lightB = 255;
volatile bool lightsDirty = false;

volatile unsigned long lastDriveCommandMs = 0;
volatile unsigned long lastSteerCommandMs = 0;
uint8_t currentDuty = 0;
int lastEnergizedDir = 0;
unsigned long motorOffMs = 0;
bool dirChangeWait = false;
int dirChangeTargetDir = 0;
uint8_t dirChangeTargetDuty = 0;
bool ramping = false;
unsigned long rampStartMs = 0;
uint8_t rampStartDuty = 0;

void writeDuty(uint8_t duty) {
  ledcWrite(MOTOR_PWMA_PIN, duty);
  currentDuty = duty;
}

void stopMotor() {
  writeDuty(0);
  digitalWrite(MOTOR_AIN1_PIN, LOW);
  digitalWrite(MOTOR_AIN2_PIN, LOW);
}

Adafruit_NeoPixel frontLeftLight(1, FRONT_LEFT_LIGHT_PIN, NEO_GRB + NEO_KHZ800);
Adafruit_NeoPixel frontRightLight(1, FRONT_RIGHT_LIGHT_PIN, NEO_GRB + NEO_KHZ800);
Adafruit_NeoPixel rearLeftLight(1, REAR_LEFT_LIGHT_PIN, NEO_GRB + NEO_KHZ800);
Adafruit_NeoPixel rearRightLight(1, REAR_RIGHT_LIGHT_PIN, NEO_GRB + NEO_KHZ800);
Adafruit_NeoPixel *const LIGHT_STRIPS[] = { &frontLeftLight, &frontRightLight, &rearLeftLight, &rearRightLight };

void applyLightsValues(bool on, uint8_t r, uint8_t g, uint8_t b) {
  if (!LIGHTS_ENABLED) return;
  for (Adafruit_NeoPixel *strip : LIGHT_STRIPS) {
    strip->setPixelColor(0, on ? strip->Color(r, g, b) : 0);
    strip->show();
  }
}

void startupFail(const char *stage) {
  digitalWrite(MOTOR_AIN1_PIN, LOW);
  digitalWrite(MOTOR_AIN2_PIN, LOW);
  if (motorPwmReady) ledcWrite(MOTOR_PWMA_PIN, 0);
  Serial.print("STARTUP FAILED at: "); Serial.println(stage);
  Serial.println("Car halted. Fix the cause and press reset.");
  while (true) {
    if (LIGHTS_ENABLED) { applyLightsValues(true, 255, 0, 0); delay(250); applyLightsValues(false, 0, 0, 0); delay(250); }
    else delay(1000);
  }
}

void disableNagle(httpd_req_t *request) {
  int fd = httpd_req_to_sockfd(request);
  if (fd >= 0) { int one = 1; setsockopt(fd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one)); }
}

const char INDEX_HTML[] PROGMEM = R"HTML(
<!doctype html><html><head><meta charset="utf-8"><meta name="viewport" content="width=device-width,initial-scale=1,user-scalable=no"><title>RC Car Camera</title><style>*{box-sizing:border-box;-webkit-tap-highlight-color:transparent}html,body{height:100%}body{margin:0;height:100vh;display:flex;flex-direction:column;background:#15191f;color:#fff;font:16px Arial;user-select:none;-webkit-user-select:none;touch-action:none;overflow:hidden}header{display:flex;justify-content:space-between;align-items:flex-start;padding:10px 14px}main{flex:1;display:flex;align-items:center;justify-content:center;gap:22px;padding:0 14px 14px;min-height:0}.panel{background:#1f242c;border-radius:12px;padding:8px 12px;min-width:132px}.label{font-size:12px;color:#b9c4d0;margin-bottom:4px}#connDot{display:inline-block;width:10px;height:10px;border-radius:50%;background:#e5484d;margin-right:6px}#connText{font-size:14px;white-space:nowrap}#lights .row{display:flex;align-items:center;gap:10px}#lights.disabled{opacity:.45}#lights.disabled::after{content:'not wired yet';font-size:11px;color:#b9c4d0}#lightOn{width:24px;height:24px;accent-color:#26a269}#lightColor{width:40px;height:30px;border:0;background:none;padding:0}.video{position:relative;width:min(58vw,120vh);aspect-ratio:4/3;max-height:100%;background:#000;border-radius:12px;overflow:hidden;display:flex;align-items:center;justify-content:center}.video img{position:absolute;inset:0;width:100%;height:100%;object-fit:contain;color:transparent}.video .placeholder{color:#7c8794;font-size:14px;text-align:center;padding:0 24px;line-height:1.5}.vslider{height:min(48vh,260px);width:56px;display:flex;align-items:center;justify-content:center}.vslider input{width:min(48vh,260px);transform:rotate(-90deg);accent-color:#2684ff}.hslider{display:flex;flex-direction:column;align-items:center;gap:6px}.hslider input{width:min(22vw,180px);accent-color:#2684ff}.ends{display:flex;justify-content:space-between;width:min(22vw,180px);font-size:13px;color:#b9c4d0}input[type=range]{height:36px;margin:0}</style></head><body><header><div class="panel" id="conn"><div class="label">Connection</div><span id="connDot"></span><span id="connText">Connecting…</span></div><div class="panel" id="lights"><div class="label">Lights</div><div class="row"><input type="checkbox" id="lightOn"><input type="color" id="lightColor" value="#ffffff"></div></div></header><main><div class="vslider"><input type="range" id="throttle" min="-100" max="100" value="0"></div><div class="video"><div class="placeholder" id="streamMsg">Connecting to camera stream…<br>If this stays, check the car is on and you are connected to RC-CAR-CAM.</div><img id="stream" alt=""></div><div class="hslider"><input type="range" id="steer" min="-100" max="100" value="0"><div class="ends"><span>L</span><span>R</span></div></div></main><script>
const $=i=>document.getElementById(i);
const cmd=u=>fetch(u).catch(()=>{});
let gen=-1;let tVal=0;const thr=$('throttle');
const ts=()=>Date.now();
const sendThrottle=fresh=>cmd('/control?cmd=throttle&v='+tVal+'&t='+ts()+'&f='+(fresh?1:0)+'&g='+gen);
thr.addEventListener('input',()=>{tVal=+thr.value;sendThrottle(true);});
setInterval(()=>{if(tVal!==0)sendThrottle(false);},200);
let sVal=0,sQueued=false;const str=$('steer');
const sendSteer=fresh=>cmd('/control?cmd=steer&v='+sVal+'&t='+ts()+'&f='+(fresh?1:0)+'&g='+gen);
str.addEventListener('input',()=>{sVal=+str.value;if(!sQueued){sQueued=true;setTimeout(()=>{sQueued=false;sendSteer(true);},50);}});
setInterval(()=>{if(sVal!==0)sendSteer(false);},200);
function releaseThrottle(){tVal=0;thr.value='0';cmd('/control?cmd=release&t='+ts());}
function releaseSteer(){sVal=0;str.value='0';cmd('/control?cmd=steer&v=0&t='+ts()+'&f=0');}
function releaseAll(){tPtr.clear();sPtr.clear();releaseThrottle();releaseSteer();}
const tPtr=new Set(),sPtr=new Set();
const bindHold=(el,ptrs,onRelease)=>{
  el.addEventListener('pointerdown',e=>{ptrs.add(e.pointerId);try{el.setPointerCapture(e.pointerId);}catch(_){}});
  const end=e=>{if(ptrs.delete(e.pointerId)&&ptrs.size===0)onRelease();};
  ['pointerup','pointercancel','lostpointercapture'].forEach(ev=>el.addEventListener(ev,end));
};
bindHold(thr,tPtr,releaseThrottle);
bindHold(str,sPtr,releaseSteer);
window.addEventListener('blur',releaseAll);
window.addEventListener('pagehide',releaseAll);
document.addEventListener('visibilitychange',()=>{if(document.hidden)releaseAll();});
function setConn(ok,text){$('connDot').style.background=ok?'#26a269':'#e5484d';$('connText').textContent=text;}
async function pollStatus(){try{const a=new AbortController();const t=setTimeout(()=>a.abort(),1500);const r=await fetch('/status',{signal:a.signal});clearTimeout(t);if(!r.ok)throw new Error('Status HTTP '+r.status);const s=await r.json();if(typeof s.gen==='number')gen=s.gen;cameraAvailable=s.camera_ready===true;if(s.clients===0){setConn(true,'Waiting for tablet');}else if(!cameraAvailable){setConn(true,'Wi-Fi connected · camera unavailable');streamMsg.style.display='';streamMsg.textContent='Camera unavailable: '+s.camera_error_name;streamImg.style.visibility='hidden';}else if(s.clients>0){if(s.rssi===null){setConn(true,s.clients+' clients · camera ready');}else{const q=s.rssi>=-55?'Excellent':s.rssi>=-65?'Good':s.rssi>=-75?'Fair':'Weak';setConn(true,s.rssi+' dBm · '+q+' · camera ready');}}else setConn(true,'Waiting for tablet');$('lights').classList.toggle('disabled',!s.lights_enabled);}catch(e){setConn(false,'No link');}}
setInterval(pollStatus,1000);pollStatus();
const lt=$('lightOn'),lc=$('lightColor');
function hx(v,i){return parseInt(v.substr(i,2),16);}
function sendLights(){cmd('/control?cmd=lights&on='+(lt.checked?1:0)+'&r='+hx(lc.value,1)+'&g='+hx(lc.value,3)+'&b='+hx(lc.value,5)+'&t='+ts());}
lt.addEventListener('change',sendLights);
lc.addEventListener('input',()=>{if(lt.checked)sendLights();});
const streamImg=$('stream'),streamMsg=$('streamMsg');
let cameraAvailable=null;
let streamRetry=0,streamTimer=null;
const connectStream=()=>{if(cameraAvailable===false)return;streamImg.style.visibility='visible';streamImg.src='http://'+location.hostname+':81/stream?r='+Date.now()};
streamImg.addEventListener('load',()=>{streamRetry=0;streamMsg.style.display='none'});
streamImg.addEventListener('error',()=>{if(cameraAvailable===false)return;streamImg.style.visibility='hidden';streamMsg.style.display='';streamMsg.textContent='Camera stream lost - reconnecting…';if(streamTimer)return;const wait=Math.min(500*(1<<Math.min(streamRetry,4)),8000);streamRetry++;streamTimer=setTimeout(()=>{streamTimer=null;connectStream()},wait)});
connectStream();
</script></body></html>
)HTML";

esp_err_t indexHandler(httpd_req_t *request) {
  httpd_resp_set_type(request, "text/html");
  return httpd_resp_send(request, INDEX_HTML, HTTPD_RESP_USE_STRLEN);
}

esp_err_t statusHandler(httpd_req_t *request) {
  wifi_sta_list_t stationList = {};
  esp_wifi_ap_get_sta_list(&stationList);
  int rssi = 0;
  bool matched = false;
  if (stationList.num == 1) { rssi = stationList.sta[0].rssi; matched = true; }
  char body[256];
  int length;
  if (matched) length = snprintf(body, sizeof(body), "{\"clients\":%d,\"rssi\":%d,\"lights_enabled\":%s,\"gen\":%lu,\"camera_ready\":%s,\"camera_error\":%d,\"camera_error_name\":\"%s\"}", stationList.num, rssi, LIGHTS_ENABLED ? "true" : "false", generation, cameraReady ? "true" : "false", static_cast<int>(cameraError), esp_err_to_name(cameraError));
  else length = snprintf(body, sizeof(body), "{\"clients\":%d,\"rssi\":null,\"lights_enabled\":%s,\"gen\":%lu,\"camera_ready\":%s,\"camera_error\":%d,\"camera_error_name\":\"%s\"}", stationList.num, LIGHTS_ENABLED ? "true" : "false", generation, cameraReady ? "true" : "false", static_cast<int>(cameraError), esp_err_to_name(cameraError));
  httpd_resp_set_type(request, "application/json");
  httpd_resp_set_hdr(request, "Cache-Control", "no-store");
  return httpd_resp_send(request, body, length);
}

esp_err_t controlHandler(httpd_req_t *request) {
  disableNagle(request);
  char query[96] = {0};
  char command[16] = {0};
  if (httpd_req_get_url_query_str(request, query, sizeof(query)) != ESP_OK ||
      httpd_query_key_value(query, "cmd", command, sizeof(command)) != ESP_OK) {
    httpd_resp_send_err(request, HTTPD_400_BAD_REQUEST, "Missing cmd");
    return ESP_FAIL;
  }
  const char *result = "OK";
  char tsBuffer[24] = {0};
  bool hasTs = httpd_query_key_value(query, "t", tsBuffer, sizeof(tsBuffer)) == ESP_OK;
  unsigned long long cmdTs = hasTs ? strtoull(tsBuffer, nullptr, 10) : 0;
  char freshBuffer[4] = {0};
  char genBuffer[12] = {0};

  bool fFlag = httpd_query_key_value(query, "f", freshBuffer, sizeof(freshBuffer)) == ESP_OK && freshBuffer[0] == '1';
  bool hasGen = httpd_query_key_value(query, "g", genBuffer, sizeof(genBuffer)) == ESP_OK;
  unsigned long cmdGen = hasGen ? strtoul(genBuffer, nullptr, 10) : 0;

  if (strcmp(command, "throttle") == 0 || strcmp(command, "steer") == 0) {
    char valueBuffer[8] = {0};
    if (httpd_query_key_value(query, "v", valueBuffer, sizeof(valueBuffer)) != ESP_OK) {
      httpd_resp_send_err(request, HTTPD_400_BAD_REQUEST, "Missing v");
      return ESP_FAIL;
    }
    int v = constrain(atoi(valueBuffer), -100, 100);
    portENTER_CRITICAL(&controlMux);
    bool freshGesture = fFlag && hasGen && cmdGen == generation;
    if (v != 0 && !hasTs) {
      result = "NEUTRAL";
    } else if (hasTs && cmdTs <= lastAcceptedTs) {
      result = "OLD";
    } else if (command[0] == 't') {
      if (v != 0 && needFreshDrive && !freshGesture) {
        result = "NEUTRAL";
      } else {
        pendingThrottle = v;
        throttleDirty = true;
        if (v != 0) { lastDriveCommandMs = millis(); if (freshGesture) needFreshDrive = false; }
        if (hasTs) lastAcceptedTs = cmdTs;
      }
    } else {
      if (v != 0 && needFreshSteer && !freshGesture) {
        result = "NEUTRAL";
      } else {
        pendingSteer = v;
        steerDirty = true;
        lastSteerCommandMs = (v != 0) ? millis() : 0;
        if (v != 0 && freshGesture) needFreshSteer = false;
        if (hasTs) lastAcceptedTs = cmdTs;
      }
    }
    portEXIT_CRITICAL(&controlMux);
  }
  else if (strcmp(command, "release") == 0) {
    portENTER_CRITICAL(&controlMux);
    if (hasTs && cmdTs <= lastAcceptedTs) result = "OLD";
    else { pendingThrottle = 0; throttleDirty = true; if (hasTs) lastAcceptedTs = cmdTs; }
    portEXIT_CRITICAL(&controlMux);
  }
  else if (strcmp(command, "stop") == 0) {
    portENTER_CRITICAL(&controlMux);
    if (hasTs && cmdTs <= lastAcceptedTs) result = "OLD";
    else { stopDirty = true; if (hasTs) lastAcceptedTs = cmdTs; }
    portEXIT_CRITICAL(&controlMux);
  }
  else if (strcmp(command, "lights") == 0) {
    char buffer[8] = {0};
    portENTER_CRITICAL(&controlMux);
    if (httpd_query_key_value(query, "on", buffer, sizeof(buffer)) == ESP_OK) lightsOn = atoi(buffer) != 0;
    if (httpd_query_key_value(query, "r", buffer, sizeof(buffer)) == ESP_OK) lightR = constrain(atoi(buffer), 0, 255);
    if (httpd_query_key_value(query, "g", buffer, sizeof(buffer)) == ESP_OK) lightG = constrain(atoi(buffer), 0, 255);
    if (httpd_query_key_value(query, "b", buffer, sizeof(buffer)) == ESP_OK) lightB = constrain(atoi(buffer), 0, 255);
    lightsDirty = true;
    portEXIT_CRITICAL(&controlMux);
  }
  else {
    httpd_resp_send_err(request, HTTPD_400_BAD_REQUEST, "Unknown cmd");
    return ESP_FAIL;
  }
  httpd_resp_set_type(request, "text/plain");
  return httpd_resp_sendstr(request, result);
}

esp_err_t streamHandler(httpd_req_t *request) {
  if (!cameraReady) {
    httpd_resp_set_status(request, "503 Service Unavailable");
    httpd_resp_set_type(request, "text/plain");
    return httpd_resp_sendstr(request, "Camera unavailable; car controls remain available.");
  }
  disableNagle(request);
  static const char *CONTENT_TYPE = "multipart/x-mixed-replace;boundary=frame";
  static const char *BOUNDARY = "\r\n--frame\r\n";
  char header[64];
  httpd_resp_set_type(request, CONTENT_TYPE);
  httpd_resp_set_hdr(request, "Cache-Control", "no-store");
  while (true) {
    camera_fb_t *frame = esp_camera_fb_get();
    if (frame == nullptr) {
      Serial.println("Camera frame acquisition failed; closing stream");
      return ESP_FAIL;
    }
    uint8_t *jpegBuffer = nullptr; // מצביע לזיכרון תמונת JPEG שיוקצה בהמרה
    size_t jpegLength = 0; // אורך התמונה המומרת בבתים
    bool converted = fmt2jpg(frame->buf, frame->len, frame->width, frame->height, PIXFORMAT_RGB565, 80, &jpegBuffer, &jpegLength); // המרת RGB565 באיכות שמונים
    esp_camera_fb_return(frame); // החזרת מסגרת המצלמה מיד לאחר סיום ההמרה
    if (!converted) { // טיפול בכישלון ההמרה ללא שימוש בתמונה לא תקינה
      free(jpegBuffer); // שחרור זיכרון שהוקצה גם אם ההמרה נכשלה
      return ESP_FAIL; // סגירת הזרם בלבד ללא עצירת השליטה ברכב
    } // סוף טיפול בכישלון ההמרה
    esp_err_t result = httpd_resp_send_chunk(request, BOUNDARY, strlen(BOUNDARY));
    if (result == ESP_OK) {
      int length = snprintf(header, sizeof(header), "Content-Type: image/jpeg\r\nContent-Length: %u\r\n\r\n", static_cast<unsigned>(jpegLength));
      result = httpd_resp_send_chunk(request, header, length);
    }
    if (result == ESP_OK) result = httpd_resp_send_chunk(request, reinterpret_cast<const char *>(jpegBuffer), jpegLength); // שליחת התמונה המומרת לדפדפן
    free(jpegBuffer); // שחרור תמונת JPEG גם במקרה של כישלון שליחה
    if (result != ESP_OK) return result;
  }
}

bool startCamera() {
  cameraReady = false;
  const bool hasPsram = psramFound();
  Serial.printf("Camera initialization started; PSRAM detected=%s, total=%u, free=%u bytes\n", hasPsram ? "yes" : "no", static_cast<unsigned>(ESP.getPsramSize()), static_cast<unsigned>(ESP.getFreePsram()));
  Serial.println("Camera: XCLK=15 SDA=4 SCL=5 D0..D7=11,9,8,10,12,18,17,16 VSYNC=6 HREF=7 PCLK=13; RGB565 10MHz LEDC channel/timer=0/0");
  esp_log_level_set("camera", ESP_LOG_INFO);
  camera_config_t config = {};
  config.ledc_channel = LEDC_CHANNEL_0;
  config.ledc_timer = LEDC_TIMER_0;
  config.pin_d0 = Y2_GPIO_NUM; config.pin_d1 = Y3_GPIO_NUM; config.pin_d2 = Y4_GPIO_NUM; config.pin_d3 = Y5_GPIO_NUM;
  config.pin_d4 = Y6_GPIO_NUM; config.pin_d5 = Y7_GPIO_NUM; config.pin_d6 = Y8_GPIO_NUM; config.pin_d7 = Y9_GPIO_NUM;
  config.pin_xclk = XCLK_GPIO_NUM; config.pin_pclk = PCLK_GPIO_NUM; config.pin_vsync = VSYNC_GPIO_NUM; config.pin_href = HREF_GPIO_NUM;
  config.pin_sccb_sda = SIOD_GPIO_NUM; config.pin_sccb_scl = SIOC_GPIO_NUM; config.pin_pwdn = PWDN_GPIO_NUM; config.pin_reset = RESET_GPIO_NUM;
  config.xclk_freq_hz = 10000000; // שעון מצלמה בתדר עשרה מגה הרץ
  config.pixel_format = PIXFORMAT_RGB565; // צילום בצבע גולמי לפני המרה ל־JPEG
  config.frame_size = FRAMESIZE_QVGA;
  config.jpeg_quality = 12;
  config.fb_count = 1; // שימוש במסגרת מצלמה אחת
  config.fb_location = CAMERA_FB_IN_PSRAM; // אחסון מסגרת בזיכרון החיצוני
  config.grab_mode = CAMERA_GRAB_LATEST; // שימוש במסגרת העדכנית ביותר
  Serial.printf("Camera buffers=%d location=%s free heap=%u\n", config.fb_count, "PSRAM", static_cast<unsigned>(ESP.getFreeHeap()));
  esp_err_t result = esp_camera_init(&config);
  cameraError = result;
  cameraReady = result == ESP_OK;
  Serial.printf("Camera initialization result: 0x%x (%s), ready=%s\n", static_cast<unsigned>(result), esp_err_to_name(result), cameraReady ? "true" : "false");
  if (cameraReady) {
    sensor_t *sensor = esp_camera_sensor_get();
    if (sensor) Serial.printf("Camera sensor PID=0x%04x VER=0x%02x MIDH=0x%02x MIDL=0x%02x\n", sensor->id.PID, sensor->id.VER, sensor->id.MIDH, sensor->id.MIDL);
  }
  return cameraReady;
}

bool startServer(httpd_handle_t *server, uint16_t port, const httpd_uri_t *uris, size_t uriCount) {
  httpd_config_t config = HTTPD_DEFAULT_CONFIG();
  config.server_port = port;
  config.ctrl_port = static_cast<uint16_t>(config.ctrl_port + (port - CONTROL_SERVER_PORT));
  config.lru_purge_enable = true;
  esp_err_t result = httpd_start(server, &config);
  Serial.printf("HTTP port %u start: 0x%x (%s)\n", port, static_cast<unsigned>(result), esp_err_to_name(result));
  if (result != ESP_OK) return false;
  for (size_t i = 0; i < uriCount; ++i) {
    result = httpd_register_uri_handler(*server, &uris[i]);
    if (result != ESP_OK) { Serial.printf("HTTP port %u route %s: %s\n", port, uris[i].uri, esp_err_to_name(result)); httpd_stop(*server); *server = nullptr; return false; }
  }
  return true;
}

bool startWebServers() {
  httpd_uri_t indexUri = {}; indexUri.uri = "/"; indexUri.method = HTTP_GET; indexUri.handler = indexHandler;
  httpd_uri_t controlUri = {}; controlUri.uri = "/control"; controlUri.method = HTTP_GET; controlUri.handler = controlHandler;
  httpd_uri_t statusUri = {}; statusUri.uri = "/status"; statusUri.method = HTTP_GET; statusUri.handler = statusHandler;
  httpd_uri_t streamUri = {}; streamUri.uri = "/stream"; streamUri.method = HTTP_GET; streamUri.handler = streamHandler;
  const httpd_uri_t controlUris[] = {indexUri, controlUri, statusUri};
  const httpd_uri_t streamUris[] = {streamUri};
  bool controlOk = startServer(&controlServer, CONTROL_SERVER_PORT, controlUris, 3);
  bool streamOk = startServer(&streamServer, STREAM_SERVER_PORT, streamUris, 1);
  Serial.printf("HTTP control=%s stream=%s\n", controlOk ? "ready" : "failed", streamOk ? "ready" : "failed");
  return controlOk;
}

void setup() {
  Serial.begin(115200);
  Serial.setDebugOutput(true);
  pinMode(MOTOR_AIN1_PIN, OUTPUT); pinMode(MOTOR_AIN2_PIN, OUTPUT);
  digitalWrite(MOTOR_AIN1_PIN, LOW); digitalWrite(MOTOR_AIN2_PIN, LOW);
  pinMode(MOTOR_PWMA_PIN, OUTPUT); digitalWrite(MOTOR_PWMA_PIN, LOW);
  generation = esp_random();
  WiFi.mode(WIFI_AP);
  if (!WiFi.softAP(AP_NAME, AP_PASSWORD)) startupFail("Wi-Fi access point");
  Serial.print("Wi-Fi AP RC-CAR-CAM started; control page: http://"); Serial.println(WiFi.softAPIP());
  startCamera();
  if (LIGHTS_ENABLED) { for (Adafruit_NeoPixel *strip : LIGHT_STRIPS) { strip->begin(); strip->show(); } }
  if (!ledcAttachChannel(MOTOR_PWMA_PIN, 20000, 8, MOTOR_LEDC_CHANNEL)) startupFail("motor PWM");
  motorPwmReady = true;
  stopMotor();
  steering.setPeriodHertz(50);
  if (!steering.attach(SERVO_PIN, 500, 2400)) startupFail("servo");
  steering.write(STEER_CENTRE);
  if (!startWebServers()) startupFail("control HTTP server");
  Serial.println("Startup complete; control loop running");
}

void loop() {
  unsigned long now = millis();
  bool applyStop = false, applyDrive = false, applySteer = false, applySteerCentre = false, applyLightsNow = false;
  bool lightsOnNow = false;
  uint8_t lightRNow = 255, lightGNow = 255, lightBNow = 255;
  int driveCmd = 0, steerCmd = 0;
  int rampDuty = -1;

  portENTER_CRITICAL(&controlMux);

  if (stopDirty) {
    stopDirty = false;
    throttleDirty = false;
    ramping = false;
    lastDriveCommandMs = 0;
    applyStop = true;
  } else if (throttleDirty) {
    throttleDirty = false;
    driveCmd = pendingThrottle;
    if (driveCmd != 0) { ramping = false; applyDrive = true; }
    else if (currentDuty == 0) { lastDriveCommandMs = 0; applyStop = true; }
    else { ramping = true; rampStartMs = now; rampStartDuty = currentDuty; }
  }
  if (steerDirty) { steerDirty = false; applySteer = true; steerCmd = pendingSteer; }
  if (lightsDirty) { lightsDirty = false; applyLightsNow = true; lightsOnNow = lightsOn; lightRNow = lightR; lightGNow = lightG; lightBNow = lightB; }

  if (!applyStop && !applyDrive) {
    if (ramping) {
      unsigned long elapsed = now - rampStartMs;
      if (elapsed >= RELEASE_RAMP_MS) { ramping = false; lastDriveCommandMs = 0; applyStop = true; }
      else rampDuty = rampStartDuty - (uint8_t)((unsigned long)rampStartDuty * elapsed / RELEASE_RAMP_MS);
    } else if (lastDriveCommandMs != 0 && now - lastDriveCommandMs > COMMAND_TIMEOUT_MS) {

      needFreshDrive = true;
      needFreshSteer = true;
      generation++;
      lastDriveCommandMs = 0;
      lastSteerCommandMs = 0;
      applyStop = true;
      applySteerCentre = true;
    }
  }
  if (!applySteerCentre && lastSteerCommandMs != 0 && now - lastSteerCommandMs > COMMAND_TIMEOUT_MS) {

    needFreshSteer = true;
    generation++;
    lastSteerCommandMs = 0;
    applySteerCentre = true;
  }
  portEXIT_CRITICAL(&controlMux);

  if (applyStop) {
    stopMotor();
    motorOffMs = now;
    dirChangeWait = false;
  } else {
    if (applyDrive) {
      int targetDir = driveCmd > 0 ? 1 : -1;
      int magnitude = abs(driveCmd);
      uint8_t targetDuty = DRIVE_POWER_SLOW + (uint8_t)((long)(DRIVE_POWER_FAST - DRIVE_POWER_SLOW) * magnitude / 100);
      bool reversal = lastEnergizedDir != 0 && targetDir != lastEnergizedDir;
      if (dirChangeWait) {
        if (targetDir == dirChangeTargetDir) {
          dirChangeTargetDuty = targetDuty;
        } else {

          dirChangeWait = false;
          digitalWrite(MOTOR_AIN1_PIN, targetDir > 0 ? HIGH : LOW);
          digitalWrite(MOTOR_AIN2_PIN, targetDir > 0 ? LOW : HIGH);
          writeDuty(targetDuty);
          lastEnergizedDir = targetDir;
        }
      } else if (reversal && (currentDuty != 0 || now - motorOffMs < MOTOR_DIR_DEADTIME_MS)) {

        if (currentDuty != 0) { writeDuty(0); motorOffMs = now; }
        digitalWrite(MOTOR_AIN1_PIN, LOW);
        digitalWrite(MOTOR_AIN2_PIN, LOW);
        dirChangeWait = true;
        dirChangeTargetDir = targetDir;
        dirChangeTargetDuty = targetDuty;
      } else {

        digitalWrite(MOTOR_AIN1_PIN, targetDir > 0 ? HIGH : LOW);
        digitalWrite(MOTOR_AIN2_PIN, targetDir > 0 ? LOW : HIGH);
        writeDuty(targetDuty);
        lastEnergizedDir = targetDir;
      }
    } else if (rampDuty >= 0) {
      writeDuty((uint8_t)rampDuty);
    }
    if (dirChangeWait && now - motorOffMs >= MOTOR_DIR_DEADTIME_MS) {

      dirChangeWait = false;
      digitalWrite(MOTOR_AIN1_PIN, dirChangeTargetDir > 0 ? HIGH : LOW);
      digitalWrite(MOTOR_AIN2_PIN, dirChangeTargetDir > 0 ? LOW : HIGH);
      writeDuty(dirChangeTargetDuty);
      lastEnergizedDir = dirChangeTargetDir;
    }
  }
  if (applySteerCentre) steering.write(STEER_CENTRE);
  else if (applySteer) steering.write(STEER_CENTRE + (int)((long)steerCmd * (STEER_RIGHT - STEER_CENTRE) / 100));
  if (applyLightsNow) applyLightsValues(lightsOnNow, lightRNow, lightGNow, lightBNow);
  delay(10);
}