智慧生活科技專業社群: [遙控甲蟲] 用Web控制甲蟲

Board：ESP32 Wrover Module
Arduino程式：
/*
  ESP32-CAM Web 控制（影像 + 搖桿 + 夾爪）
  優化重點：
  1) 控制與串流分成兩個 HTTP Server：
     - 控制頁 + /set 在 port 80
     - MJPEG /stream 在 port 81（避免串流阻塞控制）
  2) 影像改用較順設定：QVGA + 較高壓縮 + grab latest
  3) 伺服 PWM 固定用 LEDC channel 1~3（避開相機用 channel 0）
  4) 控制端傳送頻率調穩（避免爆量 fetch 造成卡頓）

  AP 模式：
    http://192.168.4.1/        控制頁
    http://192.168.4.1:81/stream  影像串流

  /set?fb=1500&rl=1500&t=1500（1000~2000, 1500 中立）
*/

#include <WiFi.h>
#include "esp_camera.h"
#include "esp_http_server.h"
#include "soc/soc.h"
#include "soc/rtc_cntl_reg.h"
#include <esp_wifi.h>

// ========= 伺服/馬達腳位 =========
static const int TongsPin  = 14;  // 夾爪
static const int WheelRPin = 12;  // 右輪
static const int WheelLPin = 13;  // 左輪

// ========= LEDC channels（避開 0；相機用 channel 0） =========
static const int CH_TONGS  = 1;
static const int CH_WHEELR = 2;
static const int CH_WHEELL = 3;

// ========= WiFi AP 設定 =========
static const char *ssid = "2AC0";
static const char *password = "12345678";

// ========= 控制狀態（1000~2000, 1500中立） =========
volatile int web_fb = 1500; // 前後
volatile int web_rl = 1500; // 左右
volatile int web_t  = 1500; // 夾爪
volatile unsigned long lastCmdMs = 0;

// ========= failsafe / deadband =========
static const uint32_t FAILSAFE_MS = 2000; // 2 秒無指令回中
static const int DEADBAND = 5;            // FB/RL 小於 5 當作 0

// ========= 相機翻轉設定 =========
int vFlip = 0;    // 1=上下翻轉
int hMirror = 0;  // 1=左右鏡像

// ========= 串流設定 =========
#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";

// 兩個 server：控制(80) / 串流(81)
httpd_handle_t control_httpd = NULL;
httpd_handle_t stream_httpd  = NULL;

sensor_t *s = nullptr;

// ========= 伺服 PWM（微秒→duty，50Hz） =========
static inline void servo_us_ch(int ch, int us) {
  us = constrain(us, 400, 2600);
  const int RES = 16;
  const uint32_t maxDuty = (1UL << RES) - 1; // 65535
  uint32_t duty = (uint32_t)((us / 20000.0) * maxDuty); // 50Hz => 20000us
  ledcWriteChannel(ch, duty);
}

static inline void servo_angle_ch(int ch, int angle) {
  angle = constrain(angle, 0, 180);
  int us = map(angle, 0, 180, 500, 2500); // 行程緊可改 600~2400
  servo_us_ch(ch, us);
}

// ========= 初始化伺服 =========
void initServo() {
  ledcAttachChannel(TongsPin,  50, 16, CH_TONGS);
  ledcAttachChannel(WheelRPin, 50, 16, CH_WHEELR);
  ledcAttachChannel(WheelLPin, 50, 16, CH_WHEELL);

  servo_angle_ch(CH_TONGS,  90);
  servo_angle_ch(CH_WHEELR, 90);
  servo_angle_ch(CH_WHEELL, 90);
}

// ========= 相機設定（AI Thinker ESP32-CAM 常見腳位） =========
void setupCam() {
  camera_config_t config;
  config.ledc_channel = LEDC_CHANNEL_0;
  config.ledc_timer   = LEDC_TIMER_0;

  config.pin_d0 = 5;
  config.pin_d1 = 18;
  config.pin_d2 = 19;
  config.pin_d3 = 21;
  config.pin_d4 = 36;
  config.pin_d5 = 39;
  config.pin_d6 = 34;
  config.pin_d7 = 35;

  config.pin_xclk = 0;
  config.pin_pclk = 22;
  config.pin_vsync = 25;
  config.pin_href  = 23;
  config.pin_sscb_sda = 26;
  config.pin_sscb_scl = 27;

  config.pin_pwdn  = 32;
  config.pin_reset = -1;

  config.xclk_freq_hz = 20000000;
  config.pixel_format = PIXFORMAT_JPEG;

  // ===== 影像速度優化（建議值）=====
  // QQVGA(160x120) 幀率會明顯比 VGA 好很多
  config.frame_size   = FRAMESIZE_QQVGA; // 160x120;
  // 數字越大壓縮越高、越省 CPU/頻寬（畫質會下降一些）
  config.jpeg_quality = 18;
  // 沒 PSRAM 的情況用 1；有 PSRAM 可用 2
  config.fb_count     = psramFound() ? 2 : 1;

#if defined(CAMERA_GRAB_LATEST)
  config.grab_mode = CAMERA_GRAB_LATEST; // 優先送最新畫面，降低延遲
#endif
#if defined(CAMERA_FB_IN_PSRAM)
  if (psramFound()) config.fb_location = CAMERA_FB_IN_PSRAM;
#endif

  esp_err_t err = esp_camera_init(&config);
  if (err != ESP_OK) {
    Serial.printf("Camera init failed: 0x%x\n", err);
    return;
  }

  s = esp_camera_sensor_get();
  s->set_brightness(s, 1);
  s->set_contrast(s,   1);
  s->set_saturation(s, 1);
  s->set_wb_mode(s,    0);

  s->set_vflip(s, vFlip);
  s->set_hmirror(s, hMirror);
  s->set_framesize(s, FRAMESIZE_QQVGA);

  Serial.println("Camera Setup OK");
}

// ========= MJPEG 串流 handler（跑在 port 81 的 server） =========
static esp_err_t stream_handler(httpd_req_t *req) {
  camera_fb_t *fb = NULL;
  esp_err_t res = ESP_OK;
  size_t jpg_len = 0;
  uint8_t *jpg_buf = NULL;
  char part_buf[64];

  httpd_resp_set_type(req, _STREAM_CONTENT_TYPE);
  httpd_resp_set_hdr(req, "Access-Control-Allow-Origin", "*");

  while (true) {
    fb = esp_camera_fb_get();
    if (!fb) {
      Serial.println("Camera capture failed");
      res = ESP_FAIL;
    } else {
      if (fb->format != PIXFORMAT_JPEG) {
        bool ok = frame2jpg(fb, 80, &jpg_buf, &jpg_len);
        esp_camera_fb_return(fb);
        fb = NULL;
        if (!ok) {
          Serial.println("JPEG compression failed");
          res = ESP_FAIL;
        }
      } else {
        jpg_len = fb->len;
        jpg_buf = fb->buf;
      }
    }

    if (res == ESP_OK) {
      size_t hlen = snprintf(part_buf, sizeof(part_buf), _STREAM_PART, jpg_len);
      res = httpd_resp_send_chunk(req, part_buf, hlen);
    }
    if (res == ESP_OK) res = httpd_resp_send_chunk(req, (const char *)jpg_buf, jpg_len);
    if (res == ESP_OK) res = httpd_resp_send_chunk(req, _STREAM_BOUNDARY, strlen(_STREAM_BOUNDARY));

    if (fb) {
      esp_camera_fb_return(fb);
      fb = NULL;
      jpg_buf = NULL;
    } else if (jpg_buf) {
      free(jpg_buf);
      jpg_buf = NULL;
    }

    if (res != ESP_OK) break;

    // 讓出 CPU（避免把控制端擠爆）
    vTaskDelay(1);
  }
  return res;
}

// ========= 控制頁 HTML =========
static const char INDEX_HTML[] PROGMEM = R"HTML(
<!doctype html>
<html lang="zh-TW">
<head>
<meta charset="utf-8">
<meta name="viewport" content="width=device-width,initial-scale=1">
<title>ESP32-CAM Web 控制</title>
<style>
  body{font-family:Arial,Helvetica,sans-serif;background:#f5f5f5;margin:0;padding:14px}
  .wrap{max-width:920px;margin:0 auto}
  .card{background:#fff;border-radius:12px;padding:12px;margin-bottom:12px;box-shadow:0 2px 10px rgba(0,0,0,.06)}
  .row{display:flex;gap:12px;flex-wrap:wrap}
  .col{flex:1;min-width:280px}
  img{width:100%;border-radius:10px;background:#000}
  .joy{width:260px;height:260px;border-radius:16px;background:#eee;position:relative;touch-action:none;user-select:none}
  .dot{width:28px;height:28px;border-radius:50%;background:#333;position:absolute;left:50%;top:50%;transform:translate(-50%,-50%)}
  button{padding:10px 12px;border:0;border-radius:10px;background:#2d7ef7;color:#fff;font-size:16px}
  button.gray{background:#6c757d}
  button.red{background:#d9534f}
  .btns{display:flex;gap:10px;flex-wrap:wrap;margin-top:10px}
  .hint{color:#555;font-size:14px;line-height:1.5}
  .val{font-family:ui-monospace,Consolas,monospace}
</style>
</head>
<body>
<div class="wrap">
  <div class="card">
    <div class="row">
      <div class="col">
        <div class="hint">影像（port 81 /stream）</div>
        <img id="cam" src="" alt="stream">
      </div>
      <div class="col">
        <div class="hint">搖桿控制（差速混控）：<span class="val" id="vv">fb=1500 rl=1500</span></div>
        <div class="joy" id="joy"><div class="dot" id="dot"></div></div>
        <div class="btns">
          <button id="stop" class="red">停止</button>
          <button id="center" class="gray">歸中</button>
        </div>

        <div class="hint" style="margin-top:10px">
          夾爪：<span class="val" id="tv">t=1500</span>
        </div>
        <div class="btns">
          <button id="open">上舉</button>
          <button id="close" class="gray">下壓</button>
        </div>
      </div>
    </div>
  </div>

  <div class="card hint">
    操作：拖曳搖桿控制前後/左右；放開回中。若 WiFi 抖動，2 秒無指令會自動停止。
  </div>
</div>

<script>
let fb=1500, rl=1500, t=1500;

// 控制送出頻率：建議 40~60ms（約 16~25Hz）
const SEND_INTERVAL_MS = 50;
let lastSend=0;

function clamp(x,a,b){return Math.max(a,Math.min(b,x));}

function send(force=false){
  const now=Date.now();
  if(!force && (now-lastSend < SEND_INTERVAL_MS)) return;
  lastSend=now;

  document.getElementById('vv').textContent = `fb=${fb} rl=${rl}`;
  document.getElementById('tv').textContent = `t=${t}`;

  fetch(`http://${location.host}/set?fb=${fb}&rl=${rl}&t=${t}`, {cache:"no-store"})
    .catch(()=>{});
}

function moveDot(nx, ny){
  const joy = document.getElementById('joy');
  const dot = document.getElementById('dot');
  const w = joy.clientWidth, h = joy.clientHeight;
  const cx = w/2, cy = h/2;
  const r  = Math.min(w,h)*0.42;
  dot.style.left = (cx + nx*r) + 'px';
  dot.style.top  = (cy + ny*r) + 'px';
}

function setCenter(){
  fb=1500; rl=1500;
  moveDot(0,0);
  send(true);
}
function setStop(){
  fb=1500; rl=1500; t=1500;
  moveDot(0,0);
  send(true);
}

const joy = document.getElementById('joy');

function calcFromEvent(e){
  const rect = joy.getBoundingClientRect();
  const x = (e.clientX - rect.left) - rect.width/2;
  const y = (e.clientY - rect.top)  - rect.height/2;
  const r = Math.min(rect.width, rect.height)*0.42;

  let nx = clamp(x / r, -1, 1);
  let ny = clamp(y / r, -1, 1);

  // 建議先用 600，太敏感再降；太鈍再升
  fb = Math.round(1500 + (-ny)*600);
  rl = Math.round(1500 + ( nx)*600);

  fb = clamp(fb,1000,2000);
  rl = clamp(rl,1000,2000);

  moveDot(nx, ny);
  send(false);
}

let active=false;

joy.addEventListener('pointerdown', (e)=>{
  active=true;
  joy.setPointerCapture(e.pointerId);
  calcFromEvent(e);
});
joy.addEventListener('pointermove', (e)=>{
  if(!active) return;
  calcFromEvent(e);
});
joy.addEventListener('pointerup', ()=>{
  active=false;
  setCenter();
});
joy.addEventListener('pointercancel', ()=>{
  active=false;
  setCenter();
});

document.getElementById('stop').onclick   = setStop;
document.getElementById('center').onclick = setCenter;

// 上舉 / 下壓（仍用 t=2000 / 1000，角度在 ESP32 端映射）
document.getElementById('open').onclick = ()=>{
  t=2000; send(true);
};
document.getElementById('close').onclick = ()=>{
  t=1000; send(true);
};

// 影像改連 port 81
document.getElementById('cam').src = `http://${location.hostname}:81/stream`;

moveDot(0,0);
send(true);
</script>
</body>
</html>
)HTML";

// ========= handler：/ =========
static esp_err_t index_handler(httpd_req_t *req) {
  httpd_resp_set_type(req, "text/html; charset=utf-8");
  return httpd_resp_send(req, INDEX_HTML, HTTPD_RESP_USE_STRLEN);
}

// ========= handler：/set =========
static esp_err_t set_handler(httpd_req_t *req) {
  char qs[128];
  char v[16];

  int fb = web_fb, rl = web_rl, tt = web_t;

  if (httpd_req_get_url_query_str(req, qs, sizeof(qs)) == ESP_OK) {
    if (httpd_query_key_value(qs, "fb", v, sizeof(v)) == ESP_OK) fb = atoi(v);
    if (httpd_query_key_value(qs, "rl", v, sizeof(v)) == ESP_OK) rl = atoi(v);
    if (httpd_query_key_value(qs, "t",  v, sizeof(v)) == ESP_OK) tt = atoi(v);
  }

  fb = constrain(fb, 1000, 2000);
  rl = constrain(rl, 1000, 2000);
  tt = constrain(tt, 1000, 2000);

  web_fb = fb;
  web_rl = rl;
  web_t  = tt;
  lastCmdMs = millis();

  // Debug
  // Serial.printf("SET fb=%d rl=%d t=%d\n", web_fb, web_rl, web_t);

  httpd_resp_set_type(req, "text/plain");
  httpd_resp_set_hdr(req, "Access-Control-Allow-Origin", "*");
  return httpd_resp_sendstr(req, "OK");
}

// ========= 啟動控制 Server（port 80） =========
void startControlServer() {
  httpd_config_t config = HTTPD_DEFAULT_CONFIG();
  config.server_port = 80;
  config.ctrl_port   = 32768;   // 第二個 server 需要不同 ctrl_port
  config.max_open_sockets = 6;

  httpd_uri_t uri_index = { .uri="/", .method=HTTP_GET, .handler=index_handler, .user_ctx=NULL };
  httpd_uri_t uri_set   = { .uri="/set", .method=HTTP_GET, .handler=set_handler, .user_ctx=NULL };

  if (httpd_start(&control_httpd, &config) == ESP_OK) {
    httpd_register_uri_handler(control_httpd, &uri_index);
    httpd_register_uri_handler(control_httpd, &uri_set);
  }
  Serial.println("Control server: port 80 ( / , /set )");
}

// ========= 啟動串流 Server（port 81） =========
void startStreamServer() {
  httpd_config_t config = HTTPD_DEFAULT_CONFIG();
  config.server_port = 81;
  config.ctrl_port   = 32769;
  config.max_open_sockets = 3;

  httpd_uri_t uri_stream = { .uri="/stream", .method=HTTP_GET, .handler=stream_handler, .user_ctx=NULL };

  if (httpd_start(&stream_httpd, &config) == ESP_OK) {
    httpd_register_uri_handler(stream_httpd, &uri_stream);
  }
  Serial.println("Stream server: port 81 ( /stream )");
}

void setup() {
  WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0);
  Serial.begin(115200);

  // WiFi 穩定度/延遲優化
  WiFi.mode(WIFI_AP);
  WiFi.setSleep(false);
  esp_wifi_set_ps(WIFI_PS_NONE);
  WiFi.softAP(ssid, password, 1, false, 4); // channel=1, max conn=4
  WiFi.setTxPower(WIFI_POWER_19_5dBm);

  IPAddress IP = WiFi.softAPIP();
  Serial.print("AP IP: http://");
  Serial.println(IP);

  setupCam();
  initServo();

  startControlServer();
  startStreamServer();

  // 關掉板上閃光燈（通常 GPIO4）
  pinMode(4, OUTPUT);
  digitalWrite(4, LOW);

  lastCmdMs = millis();
}

void loop() {
  // failsafe
  if (millis() - lastCmdMs > FAILSAFE_MS) {
    web_fb = 1500;
    web_rl = 1500;
    web_t  = 1500;
  }

  // FB/RL 映射到 -90~90
  int FB = map(web_fb, 1000, 2000, -90, 90);
  int RL = map(web_rl, 1000, 2000, -90, 90);

  if (abs(FB) < DEADBAND) FB = 0;
  if (abs(RL) < DEADBAND) RL = 0;

  int RWheel = 90 + FB + RL;
  int LWheel = 90 - FB + RL;

  RWheel = constrain(RWheel, 0, 180);
  LWheel = constrain(LWheel, 0, 180);

  // 夾爪：上舉=120度（t=2000）、下壓=75度（t=1000）
  int tongAngle = map(web_t, 1000, 2000, 75, 120);

  servo_angle_ch(CH_TONGS,  tongAngle);
  servo_angle_ch(CH_WHEELR, RWheel);
  servo_angle_ch(CH_WHEELL, LWheel);

  // 控制迴圈不需要太慢，縮短 delay 讓操控更跟手
  delay(5);
}
智慧生活科技專業社群

2026年2月4日星期三

[遙控甲蟲] 用Web控制甲蟲

沒有留言:

張貼留言

2026年2月4日 星期三

[遙控甲蟲] 用Web控制甲蟲

沒有留言:

張貼留言

2026年2月4日星期三
