趙珩宇(Henry)

[撰寫中] ESP 32 小追蹤機器人實作

[操作教學撰寫中]

Arduino IDE中需要安裝ESP 32的開發版資料，因此需要在File（檔案） => Preferences（偏好設定）輸入https://dl.espressif.com/dl/package_esp32_index.json,https://jihulab.com/esp-mirror/espressif/arduino-esp32.git

程式碼：

#include <WiFi.h>
#include <AsyncTCP.h>
#include <ESPAsyncWebServer.h>
#include "esp_camera.h"
#include "FS.h"
#include "SD.h"
#include "SPI.h"
// --- 馬達與 PWM 腳位 ---
const int motorB_IA = D1; 
const int motorB_IB = D2; 
const int motorA_IA = D5; 
const int motorA_IB = D6; 
const int freq = 2000;         
const int pwmResolution = 8;   
const int ch_B_IA = 0; 
const int ch_B_IB = 1; 
const int ch_A_IA = 2; 
const int ch_A_IB = 3; 
// --- 相機腳位 (XIAO ESP32S3 Sense) ---
#define PWDN_GPIO_NUM     -1
#define RESET_GPIO_NUM    -1
#define XCLK_GPIO_NUM     10
#define SIOD_GPIO_NUM     40
#define SIOC_GPIO_NUM     39
#define Y9_GPIO_NUM       48
#define Y8_GPIO_NUM       11
#define Y7_GPIO_NUM       12
#define Y6_GPIO_NUM       14
#define Y5_GPIO_NUM       16
#define Y4_GPIO_NUM       18
#define Y3_GPIO_NUM       17
#define Y2_GPIO_NUM       15
#define VSYNC_GPIO_NUM    38
#define HREF_GPIO_NUM     47
#define PCLK_GPIO_NUM     13
// --- SD 卡腳位 ---
#define SD_SCK  7
#define SD_MISO 8
#define SD_MOSI 9
#define SD_CS   21
const char* ssid = "XIAO_RC_CAR";
const char* password = "";
AsyncWebServer server(80);
AsyncWebSocket ws("/ws");
WiFiServer streamServer(81);
// --- 系統狀態與 PD 控制變數 ---
bool isAutoMode = false;
float targetOffset = 0.0;
unsigned long lastCommandTime = 0;
unsigned long lastLogTime = 0;
// 【修正】移除 const，讓參數可透過網頁動態調整
float Kp = 180.0;  // 比例常數 (決定轉向的基礎力度)
float Kd = 80.0;   // 微分常數 (負責踩煞車，抑制震盪)
float lastError = 0.0;
int currentLeftPWM = 0;
int currentRightPWM = 0;
// --- 網頁前端 HTML (加入動態調參拉桿) ---
const char index_html[] PROGMEM = R"rawliteral(
<!DOCTYPE HTML><html>
<head>
  <title>Hybrid Control & SD Log</title>
  <meta name="viewport" content="width=device-width, initial-scale=1, maximum-scale=1, user-scalable=no">
  <meta charset="UTF-8">
  <style>
    body { font-family: Arial; text-align: center; background-color: #222; color: #fff; margin:0; padding: 10px; overflow: hidden; }
    #stream { display: none; } 
    #videoCanvas { width: 100%; max-width: 400px; border-radius: 10px; margin-bottom: 10px; box-shadow: 0 4px 8px rgba(0,0,0,0.5); cursor: crosshair; }
    #joystick { background-color: #333; border-radius: 10px; touch-action: none; box-shadow: inset 0 0 10px rgba(0,0,0,0.8); display: block; margin: 0 auto; }
    .mode-btn { padding: 10px 15px; font-size: 16px; margin: 5px; cursor: pointer; border-radius: 5px; border: none; background-color: #555; color: white; }
    .active-mode { background-color: #0f8b8d; font-weight: bold; }
    #status { color: #f0ad4e; font-size: 14px; margin: 5px; height: 20px; }
    
    /* 動態調參介面樣式 */
    #pd-controls { background-color: #333; padding: 15px; border-radius: 10px; margin: 10px auto; max-width: 300px; text-align: left; display: none; box-shadow: inset 0 0 8px rgba(0,0,0,0.5); }
    #pd-controls label { font-size: 14px; color: #ddd; font-weight: bold; }
    #pd-controls input[type=range] { width: 100%; margin-top: 8px; margin-bottom: 15px; }
    .val-display { color: #0f8b8d; float: right; }
  </style>
</head>
<body>
  <h2>Hybrid Control System</h2>
  
  <div>
    <button id="btnJoy" class="mode-btn active-mode" onclick="setMode('JOYSTICK')">Manual (Joystick)</button>
    <button id="btnColor" class="mode-btn" onclick="setMode('COLOR')">Auto (PD Track)</button>
  </div>
  <p id="status">Mode: Joystick</p>
  <img src="" id="stream" crossorigin="anonymous">
  <canvas id="videoCanvas" width="320" height="240"></canvas>
  
  <div id="pd-controls">
    <label>Kp (Turn Force): <span id="kp-val" class="val-display">180</span></label><br>
    <input type="range" id="kp-slider" min="0" max="400" step="5" value="180" oninput="updatePD()">
    
    <label>Kd (Damping): <span id="kd-val" class="val-display">80</span></label><br>
    <input type="range" id="kd-slider" min="0" max="400" step="5" value="80" oninput="updatePD()">
  </div>
  <div id="joystick-container">
    <canvas id="joystick" width="240" height="240"></canvas>
  </div>
  <script>
    const streamImg = document.getElementById('stream');
    const videoCanvas = document.getElementById('videoCanvas');
    const vCtx = videoCanvas.getContext('2d', { willReadFrequently: true });
    
    let currentMode = 'JOYSTICK';
    let targetColor = null; 
    var gateway = `ws://${window.location.hostname}/ws`;
    var websocket;
    streamImg.src = `http://${window.location.hostname}:81/stream`;
    
    function initWebSocket() { websocket = new WebSocket(gateway); }
    initWebSocket();
    // 發送 PD 參數到 ESP32
    function updatePD() {
      let kp = document.getElementById('kp-slider').value;
      let kd = document.getElementById('kd-slider').value;
      document.getElementById('kp-val').innerText = kp;
      document.getElementById('kd-val').innerText = kd;
      if (websocket && websocket.readyState === WebSocket.OPEN) {
        websocket.send(`PD,${kp},${kd}`);
      }
    }
    function setMode(mode) {
      currentMode = mode;
      document.getElementById('btnJoy').classList.remove('active-mode');
      document.getElementById('btnColor').classList.remove('active-mode');
      
      if (mode === 'JOYSTICK') {
        document.getElementById('btnJoy').classList.add('active-mode');
        document.getElementById('joystick-container').style.display = 'block';
        document.getElementById('pd-controls').style.display = 'none';
        document.getElementById('status').innerText = 'Mode: Joystick';
        if (websocket && websocket.readyState === WebSocket.OPEN) websocket.send("Joy,0,0");
      } else if (mode === 'COLOR') {
        document.getElementById('btnColor').classList.add('active-mode');
        document.getElementById('joystick-container').style.display = 'none';
        document.getElementById('pd-controls').style.display = 'block';
        document.getElementById('status').innerText = 'Click on the video to select a color to track!';
        targetColor = null; 
      }
    }
    function processVideoFrame() {
      if (streamImg.naturalWidth > 0) {
        vCtx.drawImage(streamImg, 0, 0, videoCanvas.width, videoCanvas.height);
        if (currentMode === 'COLOR' && targetColor !== null) {
          trackColor();
        }
      }
      requestAnimationFrame(processVideoFrame);
    }
    setTimeout(() => { requestAnimationFrame(processVideoFrame); }, 1000);
    videoCanvas.addEventListener('click', function(e) {
      if (currentMode !== 'COLOR') return;
      let rect = videoCanvas.getBoundingClientRect();
      let scaleX = videoCanvas.width / rect.width;
      let scaleY = videoCanvas.height / rect.height;
      let x = (e.clientX - rect.left) * scaleX;
      let y = (e.clientY - rect.top) * scaleY;
      
      let pixel = vCtx.getImageData(x, y, 1, 1).data;
      targetColor = { r: pixel[0], g: pixel[1], b: pixel[2] };
      document.getElementById('status').innerText = `Tracking Color: RGB(${targetColor.r}, ${targetColor.g}, ${targetColor.b})`;
    });
    let lastTrackTime = 0;
    function trackColor() {
      let frameData = vCtx.getImageData(0, 0, videoCanvas.width, videoCanvas.height);
      let data = frameData.data;
      let sumX = 0, sumY = 0, count = 0;
      let threshold = 50; 
      
      for (let i = 0; i < data.length; i += 4) {
        let r = data[i], g = data[i+1], b = data[i+2];
        let diff = Math.sqrt(Math.pow(r - targetColor.r, 2) + Math.pow(g - targetColor.g, 2) + Math.pow(b - targetColor.b, 2));
        if (diff < threshold) {
          let pixelIndex = i / 4;
          let x = pixelIndex % videoCanvas.width;
          let y = Math.floor(pixelIndex / videoCanvas.width);
          sumX += x;
          sumY += y;
          count++;
        }
      }
      let now = Date.now();
      if (now - lastTrackTime > 66) { 
        if (count > 50) { 
          let centerX = sumX / count;
          let offset = (centerX - (videoCanvas.width / 2)) / (videoCanvas.width / 2);
          
          if (websocket && websocket.readyState === WebSocket.OPEN) {
            websocket.send(`Offset,${offset.toFixed(3)}`);
          }
          vCtx.beginPath();
          vCtx.arc(centerX, sumY/count, 10, 0, 2 * Math.PI);
          vCtx.fillStyle = 'red';
          vCtx.fill();
        } else {
          if (websocket && websocket.readyState === WebSocket.OPEN) {
            websocket.send("Offset,0.0");
          }
        }
        lastTrackTime = now;
      }
    }
    // 虛擬搖桿邏輯
    const canvasJoy = document.getElementById('joystick');
    const ctxJoy = canvasJoy.getContext('2d');
    const centerJoy = { x: canvasJoy.width / 2, y: canvasJoy.height / 2 };
    const maxRadius = 80; 
    let isDragging = false;
    let lastSendTime = 0;
    function drawJoystick(x, y) {
      ctxJoy.clearRect(0, 0, canvasJoy.width, canvasJoy.height);
      ctxJoy.beginPath();
      ctxJoy.arc(centerJoy.x, centerJoy.y, maxRadius, 0, Math.PI * 2);
      ctxJoy.fillStyle = 'rgba(255, 255, 255, 0.1)';
      ctxJoy.fill();
      ctxJoy.beginPath();
      ctxJoy.arc(x, y, 30, 0, Math.PI * 2);
      ctxJoy.fillStyle = '#0f8b8d';
      ctxJoy.fill();
    }
    drawJoystick(centerJoy.x, centerJoy.y);
    function updatePosition(e) {
      if (!isDragging || currentMode !== 'JOYSTICK') return;
      let rect = canvasJoy.getBoundingClientRect();
      let clientX = e.touches ? e.touches[0].clientX : e.clientX;
      let clientY = e.touches ? e.touches[0].clientY : e.clientY;
      let px = clientX - rect.left;
      let py = clientY - rect.top;
      let dx = px - centerJoy.x;
      let dy = py - centerJoy.y;
      let distance = Math.sqrt(dx * dx + dy * dy);
      if (distance > maxRadius) {
        px = centerJoy.x + (dx * maxRadius / distance);
        py = centerJoy.y + (dy * maxRadius / distance);
      }
      drawJoystick(px, py);
      let joyX = (px - centerJoy.x) / maxRadius; 
      let joyY = -(py - centerJoy.y) / maxRadius;
      let speed = joyY * 255;
      let turn = joyX * 255;
      let leftSpeed = speed + turn;
      let rightSpeed = speed - turn;
      leftSpeed = Math.max(-255, Math.min(255, leftSpeed));
      rightSpeed = Math.max(-255, Math.min(255, rightSpeed));
      let now = Date.now();
      if (now - lastSendTime > 50) {
        if (websocket && websocket.readyState === WebSocket.OPEN) {
          websocket.send(`Joy,${Math.round(leftSpeed)},${Math.round(rightSpeed)}`);
        }
        lastSendTime = now;
      }
    }
    canvasJoy.onmousedown = canvasJoy.ontouchstart = function(e) { if(currentMode === 'JOYSTICK') { isDragging = true; updatePosition(e); }};
    canvasJoy.onmousemove = canvasJoy.ontouchmove = function(e) { if(isDragging) e.preventDefault(); updatePosition(e); };
    window.onmouseup = window.ontouchend = function(e) {
      if(isDragging) {
        isDragging = false;
        drawJoystick(centerJoy.x, centerJoy.y); 
        if (websocket && websocket.readyState === WebSocket.OPEN) {
          websocket.send("Joy,0,0"); 
        }
      }
    };
  </script>
</body>
</html>
)rawliteral";
// --- 影像串流任務 ---
#define PART_BOUNDARY "123456789000000000000987654321"
static const char* _STREAM_CONTENT_TYPE = "multipart/x-mixed-replace;boundary=" PART_BOUNDARY;
static const char* _STREAM_BOUNDARY = "\r\n--" PART_BOUNDARY "\r\n";
static const char* _STREAM_PART = "Content-Type: image/jpeg\r\nContent-Length: %u\r\n\r\n";
void streamTask(void *pvParameters) {
  while (true) {
    WiFiClient client = streamServer.available();
    if (client) {
      client.print("HTTP/1.1 200 OK\r\n");
      client.print("Access-Control-Allow-Origin: *\r\n"); 
      client.print("Content-Type: ");
      client.print(_STREAM_CONTENT_TYPE);
      client.print("\r\n\r\n");
      
      camera_fb_t * fb = NULL;
      while (client.connected()) {
        fb = esp_camera_fb_get();
        if (!fb) { delay(100); continue; }
        client.print(_STREAM_BOUNDARY);
        client.printf(_STREAM_PART, fb->len);
        client.write(fb->buf, fb->len);
        client.print("\r\n");
        esp_camera_fb_return(fb); 
        vTaskDelay(15 / portTICK_PERIOD_MS); 
      }
      client.stop();
    }
    delay(10); 
  }
}
// --- 初始化相機 ---
void setupCamera() {
  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 = 20000000; 
  config.pixel_format = PIXFORMAT_JPEG;
  config.frame_size = FRAMESIZE_QVGA; 
  config.jpeg_quality = 20; 
  config.fb_count = 1; 
  esp_err_t err = esp_camera_init(&config);
  if (err != ESP_OK) {
    Serial.printf("相機初始化失敗: 0x%x\n", err);
    return;
  }
  sensor_t * s = esp_camera_sensor_get();
  s->set_vflip(s, 1); 
}
// --- 初始化 SD 卡與標題 (加入 Kp 與 Kd 欄位) ---
void initSDCard() {
  SPI.begin(SD_SCK, SD_MISO, SD_MOSI, SD_CS);
  if (!SD.begin(SD_CS, SPI)) {
    Serial.println("SD 卡掛載失敗！請檢查是否插入。");
    return;
  }
  Serial.println("SD 卡掛載成功！");
  
  File file = SD.open("/datalog.csv", FILE_READ);
  if (!file) {
    file = SD.open("/datalog.csv", FILE_WRITE);
    if (file) {
      file.println("Time(ms),Mode,Target_Offset,PWM_L,PWM_R,Free_Heap(KB),Kp,Kd");
      file.close();
    }
  } else {
    file.close();
  }
}
// --- 寫入數據到 SD 卡 ---
void logDataToSD() {
  File file = SD.open("/datalog.csv", FILE_APPEND);
  if (file) {
    String modeStr = isAutoMode ? "AUTO(PD)" : "MANUAL";
    String logString = String(millis()) + "," + modeStr + "," + String(targetOffset, 3) + "," + 
                       String(currentLeftPWM) + "," + String(currentRightPWM) + "," + 
                       String(ESP.getFreeHeap() / 1024.0) + "," + String(Kp, 1) + "," + String(Kd, 1);
    file.println(logString);
    file.close();
  }
}
// --- 馬達 PWM 輸出 ---
void setMotorsPWM(int leftSpeed, int rightSpeed) {
  currentLeftPWM = leftSpeed;
  currentRightPWM = rightSpeed;
  
  if (leftSpeed >= 0) {
    ledcWrite(ch_A_IA, leftSpeed);
    ledcWrite(ch_A_IB, 0);
  } else {
    ledcWrite(ch_A_IA, 0);
    ledcWrite(ch_A_IB, -leftSpeed); 
  }
  if (rightSpeed >= 0) {
    ledcWrite(ch_B_IA, rightSpeed);
    ledcWrite(ch_B_IB, 0);
  } else {
    ledcWrite(ch_B_IA, 0);
    ledcWrite(ch_B_IB, -rightSpeed);
  }
}
// --- 解析 WebSocket (加入 PD 調參指令) ---
void handleWebSocketMessage(void *arg, uint8_t *data, size_t len) {
  AwsFrameInfo *info = (AwsFrameInfo*)arg;
  if (info->final && info->index == 0 && info->len == len && info->opcode == WS_TEXT) {
    data[len] = 0;
    String cmd = (char*)data;
    
    if (cmd.startsWith("Joy,")) {
      isAutoMode = false;
      int firstComma = cmd.indexOf(',');
      int secondComma = cmd.indexOf(',', firstComma + 1);
      if (firstComma > 0 && secondComma > 0) {
        int left = cmd.substring(firstComma + 1, secondComma).toInt();
        int right = cmd.substring(secondComma + 1).toInt();
        setMotorsPWM(left, right);
      }
      lastCommandTime = millis();
    } 
    else if (cmd.startsWith("Offset,")) {
      isAutoMode = true;
      targetOffset = cmd.substring(7).toFloat();
      lastCommandTime = millis();
    }
    // 【新增】接收動態 PD 參數
    else if (cmd.startsWith("PD,")) {
      int firstComma = cmd.indexOf(',');
      int secondComma = cmd.indexOf(',', firstComma + 1);
      if (firstComma > 0 && secondComma > 0) {
        Kp = cmd.substring(firstComma + 1, secondComma).toFloat();
        Kd = cmd.substring(secondComma + 1).toFloat();
        Serial.printf("Updated PD: Kp=%.1f, Kd=%.1f\n", Kp, Kd);
      }
    }
  }
}
void onEvent(AsyncWebSocket *server, AsyncWebSocketClient *client, AwsEventType type, void *arg, uint8_t *data, size_t len) {
  if (type == WS_EVT_DATA) handleWebSocketMessage(arg, data, len);
}
void setup() {
  Serial.begin(115200);
  delay(500);
  ledcSetup(ch_B_IA, freq, pwmResolution);
  ledcSetup(ch_B_IB, freq, pwmResolution);
  ledcSetup(ch_A_IA, freq, pwmResolution);
  ledcSetup(ch_A_IB, freq, pwmResolution);
  ledcAttachPin(motorB_IA, ch_B_IA);
  ledcAttachPin(motorB_IB, ch_B_IB);
  ledcAttachPin(motorA_IA, ch_A_IA);
  ledcAttachPin(motorA_IB, ch_A_IB);
  
  setMotorsPWM(0, 0); 
  
  initSDCard();
  setupCamera();
  WiFi.softAP(ssid, password);
  Serial.print("AP IP: "); Serial.println(WiFi.softAPIP());
  ws.onEvent(onEvent);
  server.addHandler(&ws);
  server.on("/", HTTP_GET, [](AsyncWebServerRequest *request){
    request->send_P(200, "text/html", index_html);
  });
  server.begin();
  streamServer.begin();
  xTaskCreatePinnedToCore(streamTask, "streamTask", 8192, NULL, 1, NULL, 1);
  
  Serial.println("System Ready! PD Control Loop Starting...");
}
void loop() {
  ws.cleanupClients();
  unsigned long now = millis();
  if (now - lastCommandTime > 500) {
    if (currentLeftPWM != 0 || currentRightPWM != 0) {
      setMotorsPWM(0, 0);
    }
    isAutoMode = false;
  } 
  else if (isAutoMode) {
    static unsigned long lastPDTime = 0;
    if (now - lastPDTime >= 50) {
      float error = targetOffset; 
      float derivative = error - lastError;
      
      int baseSpeed = 160; 
      int turn = (Kp * error) + (Kd * derivative);
      
      int leftSpeed = baseSpeed + turn;
      int rightSpeed = baseSpeed - turn;
      
      leftSpeed = constrain(leftSpeed, -255, 255);
      rightSpeed = constrain(rightSpeed, -255, 255);
      
      if (targetOffset == 0.0) {
        setMotorsPWM(0, 0);
      } else {
        setMotorsPWM(leftSpeed, rightSpeed);
      }
      
      lastError = error;
      lastPDTime = now;
    }
  }
  // 紀錄頻率：每 200ms 寫入一筆
  if (now - lastLogTime >= 200) {
    logDataToSD();
    lastLogTime = now;
  }
}
未知 的大頭貼

發表者：趙珩宇(Henry)

師大數學系肄業 師大科技系畢業 師大科技碩畢業 師大科技博肄業 實習於台北市立金華國中 過去為《Make》特約採訪編輯 曾任職台北市立永春高中以及台北市立金華國中 現為台北市立建國高中生活科技教師以及台北市數位實驗高中生活科技教師 喜歡學習新東西、穿梭於臺北自造者空間，以別的學科眼中的不務正業(玩一堆奇怪的東西)為榮 採訪： * 會「玩」指尖陀螺不稀奇，會「做」指尖陀螺才厲害。（親子天下，2017） * 風靡全球的指尖陀螺 老師把生技課變有趣了。（教育電台，2017） * 永春高中超大電路板　教室人人都看得見（蘋果日報）
 https://www.facebook.com/henry.d.chao mail:henrychao521@gmail.com

發表留言