小智AI语音打靶机器人：掌控板 + 二哈2

说一句话就能指挥的自动打靶系统：小智AI（语音/文本）→ MCP WebSocket → 掌控板(ESP32) → 二哈2视觉 → 水平舵机对准 → 电磁继电器扣扳机。目标出现时自动搜索、锁定在屏幕中心、连发 3 次；语音还能让炮塔"转到90度""向左10度""射击"。

一、项目简介

用 掌控板（ESP32，Arduino IDE 编程） 做主控，二哈2（DFRobot SEN0638） 做视觉，1 个 180° 舵机做水平炮塔（倒立安装，行程 10°~110°、中位 50°），电磁继电器控制扳机机构，接入 小智AI 的 MCP 服务实现语音指挥：

• 自动打靶：舵机来回扫描 → 二哈2 检测到目标 → 斜率标定后自动对准 → 目标锁定在屏幕水平中心 → 连续射击 3 次 → 保持；目标大幅移动/丢失后重新锁定
• 语音指挥：对小智AI 说"开始搜索""停止""射击""转到90度""向左10度""向右一点""现在什么状态"
• 双保险控制：按键 A=搜索开停、B=手动射击；串口命令全功能

项目核心价值不在"射击"本身，而在一套从"图像稳定锁定"到"语音工具调用"的完整闭环，以及大量实测调出来的细节：640×480 图像中心、ID 锁定、滑动滤波、舵机-相机斜率标定（倒装自动适配）、滞回死区、每发后重瞄。

二、功能特性

• 视觉锁定：二哈2 物体追踪 + ID 锁定单一目标 + EMA 滑动滤波 + 残影过滤（w<30px 视为丢失）
• 自标定：首次检测自动测"舵机每转1°目标在画面移动多少像素"（slope，px/°），倒立装方向自动适配
• 位置伺服：目标角 = 舵机角 − du/slope，直接算"把目标移到中心"的角度，不猜符号
• 锁定射击：死区 50px 进入锁定 → 稳定 10 帧连发 → 每发后重新瞄准；滞回带 50~70px 防边界抖动；3 发上限，3 发后保持不扫不射；大幅移动/丢失重新锁定
• 语音工具：search_shoot / fire / servo_abs / servo_rel / status
• 安全：软件限位 10°~110°；开火前确认目标居中且稳定

三、硬件清单

部件	型号 / 参数	数量
主控	掌控板（Maker Learning Board，ESP32-WROOM）	1
视觉	二哈2 HUSKYLENS2（SEN0638），I2C	1
炮塔	180° 舵机 ×1（倒立安装，行程 10~110°，中位 50°）	1
扳机	电磁继电器模块（P1/IO32 控制）	1
扳机机构	触发装置（继电器输出驱动）	1
供电	掌控板 5V/USB；舵机与继电器/扳机机构必须分开供电（见"坑6"）	—

接线：二哈2 接 I2C（SDA=IO23，SCL=IO22，原理图 V2.0.3 确认）；舵机信号 P8/IO26；继电器 P1/IO32；按键 A=IO0、B=IO2。

四、系统方案与原理

1. 视觉与图像坐标

• 二哈2 屏幕是 640×480，水平中心是 320，不是 160！协议文档写明"屏幕底边中点 (320,480)"
• 误差 du = u − 320；目标在左 du<0，在右 du>0

2. 舵机-相机联动斜率标定（自标定，倒装自动适配）

舵机倒立装 = 角度增大对应实际转向相反（镜像）。所以不猜方向符号，首次检测时实测映射：

朝中心方向探 3° → 等 400ms 滤波稳定 → slope = Δu / Δdeg  （px/°，正负含安装方向）

之后瞄准直接用位置伺服：目标角 = 舵机角 − du / slope（每周期限幅 1°），不再依赖方向常量。

3. 锁定-连发-保持（状态机）

搜索扫描(10°~110°来回) → 检测到目标 → 斜率标定 → 位置伺服补瞄
  → |du|≤50 进入锁定 → 稳定10帧 → 开火(第N/3)
      开火后：解锁+重新标定（每发都"重新瞄准"，更稳）
  → 50<|du|≤70 滞回带：保持锁定、不累计、不抖动
  → |du|>70 补瞄追回；3发完成 → 保持（不追不射）
  → |du|>100 或丢失 → 重新锁定（新会话，可再3发）

4. 小智AI 语音（MCP 工具）

工具	对话示例	动作
search_shoot	"开始搜索"/"停止搜索"	开/停自动打靶循环
fire	"射击"/"开火"	继电器吸合 250ms 扣扳机
servo_abs	"转到90度"	舵机绝对角度
servo_rel	"向左10度"/"向右一点"	相对转动
status	"现在什么状态"	是否搜索/舵机角/目标偏差/丢失

五、程序实现

用 Arduino IDE 编写，单文件（完整代码见下文代码块，esp32_huskylens2_shooter_mcp.ino）：

• trackU()：ID锁定 + EMA滤波 + 残影过滤
• stepShooter()：标定 → 位置伺服 → 锁定 → 连发3次 → 保持（核心状态机）
• triggerFire()：继电器扣扳机
• MCP 工具注册 + WiFi 连接 + 按键/串口命令

完整程序

/*!
 * 二哈2 自动瞄准射击 —— 小智AI(MCP/WebSocket) 对话控制版
 * 掌控板(ESP32) + Arduino IDE，结构参考 WebSocketMCP 官方例程
 *
 * 对话能力（通过 MCP 工具注册，小智AI 语音/文本可调用）：
 *   search_shoot  开始/停止“搜索目标→瞄准→自动射击”循环
 *   fire          射击一次（扣扳机）
 *   servo_abs     转到指定绝对角度（对话：“转到90度”“指向左边目标”由 AI 折成角度）
 *   servo_rel     相对转动（对话：“向左一点”“向右10度”）
 *   status        查询状态（是否在搜索、舵机角度、目标偏差）
 *
 * 硬件引脚（掌控板 V2.0.3 + 实机确认）：
 *   二哈2   I2C: SDA=IO23, SCL=IO22
 *   水平舵机 P8/IO26（LEDC 50Hz，500~2500µs）
 *   电磁继电器 P1/IO32（吸合 FIRE_ON_MS 扣扳机）
 *   按键 A=IO0（搜索开/停） 按键 B=IO2（手动射击）
 *   注意：IO2 已被按键B占用，状态LED默认禁用（STATUS_LED_PIN=-1）
 *
 * 行为说明：
 *   - 对话“相对/绝对转动”会暂停自动搜索循环（避免抢舵机），
 *     需要继续自动打靶时再说“开始搜索”
 *   - 舵机角度限位 SERVO_MIN~SERVO_MAX（当前 10~110°，中心 50°）
 *   - SERVO_TURN_DIR 决定“向右”是角度增大还是减小（安装方向，实测取反）
 */
#include <WiFi.h>
#include <WebSocketMCP.h>
#include <ArduinoJson.h>
#include <Wire.h>
#include "DFRobot_HuskylensV2.h"

#define TRACE Serial

/********** 配置项（沿用小智AI例程） ***********/
// WiFi设置
const char* WIFI_SSID = "sxs";
const char* WIFI_PASS = "smj080823";

// WebSocket MCP服务器地址
const char* MCP_ENDPOINT = "wss://api.xiaozhi.me/mcp/?token=eyJhbGciOiJFUzI1NiIsInR5cCI6IkpXVCJ9.eyJ1c2VySWQiOjEyODIzNSwiYWdlbnRJZCI6MTg4Mjc1LCJlbmRwb2ludElkIjoiYWdlbnRfMTg4Mjc1IiwicHVycG9zZSI6Im1jcC1lbmRwb2ludCIsImlhdCI6MTc5MDk4NDc0OSwiZXhwIjoxODIyNTQyMzQ5fQ.whdG6Stnn7bwcy8ISd69GekCIZFlHLsZeQEOL2NEP_7DIea0D3L0e0ntthSDkg5xDO6nYM1ab9fsaR-zlwTmZg";

#define DEBUG_SERIAL Serial
#define DEBUG_BAUD_RATE 115200

#define STATUS_LED_PIN -1    // 状态LED（IO2被按键B占用，默认禁用；可改空闲引脚）

// ---------- 引脚（实机确认） ----------
#define I2C_SDA     23
#define I2C_SCL     22
#define SERVO_PIN   26      // P8
#define RELAY_PIN   32      // P1
#define BTN_A_PIN   0       // 按键A（按下=低电平）
#define BTN_B_PIN   2       // 按键B（按下=低电平）

// ---------- LEDC 兼容层（esp32 v2/v3） ----------
#if !defined(ESP_ARDUINO_VERSION_MAJOR) || ESP_ARDUINO_VERSION_MAJOR < 3
static inline bool pwmAttach(uint8_t pin, uint32_t freq, uint8_t res, uint8_t ch = 0) {
  (void)pin; ledcSetup(ch, freq, res); ledcAttachPin(pin, ch); return true;
}
static inline void pwmWriteF(uint8_t pin, uint32_t duty, uint8_t ch = 0) {
  (void)pin; ledcWrite(ch, duty);
}
#else
static inline bool pwmAttach(uint8_t pin, uint32_t freq, uint8_t res, uint8_t ch = 0) {
  (void)ch; return ledcAttach(pin, freq, res);
}
static inline void pwmWriteF(uint8_t pin, uint32_t duty, uint8_t ch = 0) {
  (void)ch; ledcWrite(pin, duty);
}
#endif

// ---------- 控制参数（实测微调） ----------
const float SCAN_STEP_DEG = 0.5f;    // 搜索扫描步长(°/25ms≈20°/s)；想更慢调 0.25，更快调 1.0
const int   SERVO_DIR     = 1;          // 实机日志确认：目标在左(du<0)→deg应减小；原 -1 使目标越推越偏并卡限位，已改回 +1
const int   SERVO_TURN_DIR = 1;         // TODO: 对话“向右”=角度增大？反了改 -1
const float SERVO_MIN     = 5.0f, SERVO_MAX = 110.0f, SERVO_CENTER = 50.0f;   // MIN 10→5：之前目标在左会被限位卡死
const float AIM_OFFSET_PX = 0.0f;       // 相机中心与枪口机械偏置（像素）
const float KP            = 0.0002f;      // du(px) → 舵机角(deg)
const float OUT_MAX_DEG   = 1.0f;       // 瞄准单周期最大转角(≈40°/s)，保证 servoDeg≈实物
const float DEADBAND_PX   = 50.0f;      // 进入死区（锁定/开火判据），可放宽到 60~70
const float LOCK_EXIT_PX  = 70.0f;      // 滞回退出带：锁定保持到此，超出才补瞄（防边界抖动）
const float FIRE_TOL_PX   = 50.0f;      // 开火允许偏差（与 DEADBAND 一致）
const float RESUME_PX     = 100.0f;     // 射3发后：|du|>100px 视为目标大幅移动→重新锁定(可再射)
const int   FIRE_STABLE   = 10;         // 居中连续稳定帧数
const float FIRE_SIZE_MIN_PX = 0.0f;    // 目标宽度阈值（够近才开火；0=不启用）
const int   RELAY_ACTIVE  = 1;          // 继电器触发电平：1=高电平吸合，0=低电平吸合
const int   FIRE_ON_MS    = 250;        // 扣扳机吸合时长(ms)
const float CTRL_DT_MS    = 25.0f;      // 控制周期
const float U_CENTER      = 320.0f;     // 二哈2 图像中心（640×480！）

/********** 全局变量 ***********/
WebSocketMCP mcpClient;
HuskylensV2 huskylens;

bool wifiConnected = false;
bool mcpConnected = false;
bool running = false;                 // 搜索-射击循环开关
int  fireStable = 0;
int  fireCount = 0;                   // 本次锁定已射击次数（上限3）
bool holdPrinted = false;             // [hold] 提示只打印一次
bool locked = false;                  // 锁定标志：锁定后舵机绝对不动，连发3次
float servoDeg = SERVO_CENTER;
int  scanDir = 1;
float lastDu = 0.0f;
bool lastLost = true;
unsigned long lastCtrl = 0;

/********** 函数声明 ***********/
void setupWifi();
void registerMcpTools();
void onMcpConnectionChange(bool connected);
void triggerFire();
void servoWrite(float deg);
void relaySet(bool on);
bool trackU(float *u, float *w);
void stepShooter();
void blinkLed(int times, int delayMs);

// ---------- 舵机 ----------
void servoInit() {
  bool ok = pwmAttach(SERVO_PIN, 50, 16, 0);
  TRACE.print("[boot] LEDC舵机挂载: ");
  TRACE.println(ok ? "成功" : "失败!");
}
void servoMicros(uint32_t us) { pwmWriteF(SERVO_PIN, us * 65536UL / 20000UL, 0); }
void servoWrite(float deg) {
  deg = (deg < SERVO_MIN) ? SERVO_MIN : (deg > SERVO_MAX) ? SERVO_MAX : deg;
  servoDeg = deg;
  servoMicros((uint32_t)(500.0f + deg / 180.0f * 2000.0f));
}

// ---------- 继电器（扣扳机） ----------
void relaySet(bool on) {
  digitalWrite(RELAY_PIN, on ? RELAY_ACTIVE : !RELAY_ACTIVE);
}
void triggerFire() {
  TRACE.println("[fire] 扣扳机！");
  relaySet(true);
  delay(FIRE_ON_MS);
  relaySet(false);
}

// ---------- 二哈2 读取（u 做滑动滤波，抑制像素抖动；输出目标屏幕位置） ----------
bool trackU(float *u, float *v, float *w, float *h) {
  huskylens.getResult(ALGORITHM_OBJECT_TRACKING);
  if (!huskylens.available(ALGORITHM_OBJECT_TRACKING)) return false;
  // 锁定单一目标 ID：防止追踪框在多目标/场景边缘之间跳（w/h 乱跳的根因）
  static int16_t lockId = -1;
  Result *it;
  if (lockId >= 0) {
    it = huskylens.getCachedResultByID(ALGORITHM_OBJECT_TRACKING, lockId);
    if (!it) { lockId = -1; return false; }   // 目标消失 → 解锁，等下一个
  } else {
    it = huskylens.getCachedCenterResult(ALGORITHM_OBJECT_TRACKING);
    lockId = (int16_t)RET_ITEM_NUM(it, Result, ID);
  }
  // RET_ITEM_NUM 宏内部 static_cast<Result*>(func)，不接受 const 指针
  float tw = (float)RET_ITEM_NUM(it, Result, width);
  if (tw < 30.0f) return false;              // 边角残影/目标离开视野 → 视为丢失，触发重扫
  float cx = (float)RET_ITEM_NUM(it, Result, xCenter);
  static bool uInit = false;
  static float avgU = 0;
  if (!uInit) { avgU = cx; uInit = true; }   // 首帧直接采用
  avgU = 0.7f * avgU + 0.3f * cx;            // 滤波（30% 新值，忽略框跳变）
  *u = avgU;
  *v = (float)RET_ITEM_NUM(it, Result, yCenter);
  *w = tw;
  *h = (float)RET_ITEM_NUM(it, Result, height);
  return true;
}

// ---------- 搜索-瞄准-射击 单步（斜率校准定方向 + 位置伺服锁定，已验证可稳定居中） ----------
void stepShooter() {
  float u, v, w, h;
  static int8_t calibPhase = 0;      // 0=未校准 1=探步 2=等滤波稳定 3=已校准
  static float uBefore = 0, degBefore = 0;
  static unsigned long calibT = 0;
  static float slope = 0;            // 映射斜率 px/deg（含安装方向正负，倒装自动适配）

  if (!trackU(&u, &v, &w, &h)) {
    lastLost = true; lastDu = 0.0f;
    fireStable = 0; calibPhase = 0; slope = 0;
    fireCount = 0; holdPrinted = false; locked = false;   // 目标丢失 → 新会话，计数清零
    servoWrite(servoDeg + scanDir * SCAN_STEP_DEG);   // 扫描 10°~110°
    if (servoDeg >= SERVO_MAX) scanDir = -1;
    if (servoDeg <= SERVO_MIN) scanDir = +1;
    return;
  }
  lastLost = false;
  float du = u - (U_CENTER + AIM_OFFSET_PX);
  lastDu = du;
  float ad = fabsf(du);
  // 目标屏幕位置 + 误差 + 舵机角 + 稳定帧 + 射击次数 + 是否居中
  TRACE.print("[aim] u="); TRACE.print((int)u);
  TRACE.print(" v="); TRACE.print((int)v);
  TRACE.print(" w="); TRACE.print((int)w);
  TRACE.print(" h="); TRACE.print((int)h);
  TRACE.print(" du="); TRACE.print((int)du);
  TRACE.print(" deg="); TRACE.print((int)servoDeg);
  TRACE.print(" stable="); TRACE.print(fireStable);
  TRACE.print(" shots="); TRACE.print(fireCount);
  TRACE.print(" center="); TRACE.println((ad <= DEADBAND_PX) ? "yes" : "no");

  // ---------- 斜率校准：测“每° u 变化多少像素”（一次定型，等滤波稳定） ----------
  if (calibPhase == 0) {                    // 朝中心方向探 3°，留出行程
    degBefore = servoDeg;
    uBefore = u;
    servoWrite(servoDeg + ((servoDeg >= SERVO_CENTER) ? -3.0f : 3.0f));
    calibT = millis();
    calibPhase = 1;
    TRACE.println("[aim] 校准中…");
    return;
  }
  if (calibPhase == 1) {
    if (millis() - calibT < 400) return;    // 等滤波稳定
    float dDeg = servoDeg - degBefore;
    if (fabsf(dDeg) < 0.5f) {               // 探到限位没动 → 反向再探
      degBefore = servoDeg; uBefore = u;
      servoWrite(servoDeg + ((servoDeg >= SERVO_CENTER) ? 3.0f : -3.0f));
      calibT = millis();
      TRACE.println("[aim] 探到限位，反向再探");
      return;
    }
    slope = (u - uBefore) / dDeg;           // px/deg
    calibPhase = 3;
    TRACE.print("[aim] 方向已校准 slope="); TRACE.print(slope, 3);
    TRACE.println(" px/°");
    return;
  }

  bool sizeOk = (FIRE_SIZE_MIN_PX <= 0.0f) || (w >= FIRE_SIZE_MIN_PX);

  // ---------- 锁定 + 滞回死区 + 每发后重瞄 ----------
  // 进死区=DEADBAND(50)，退出带=LOCK_EXIT(70)：50~70 滞回带内保持锁定不抖动
  if (!locked && ad <= DEADBAND_PX) {            // 首次进入 → 锁定
    locked = true;
    fireStable = 0;
    TRACE.println("[lock] 目标锁定，开始连发");
  }
  if (locked) {
    if (fireCount >= 3) {                        // 3发完成：保持（不追不射）
      if (!holdPrinted) { TRACE.println("[hold] 3发完成，保持不动"); holdPrinted = true; }
      if (ad > RESUME_PX) {                      // 大幅移动 → 新会话
        locked = false; fireCount = 0; calibPhase = 0; slope = 0;
        holdPrinted = false;
        TRACE.println("[lock] 目标大幅移动，重新锁定");
      }
      return;                                    // 保持：不射不追
    }
    if (ad > LOCK_EXIT_PX) {                     // 超出滞回退出带 → 解锁补瞄（保留slope）
      locked = false;
      TRACE.println("[aim] 出滞回带，补瞄");
    } else if (ad <= DEADBAND_PX) {
      fireStable++;                              // 死区内：累计稳定帧（舵机不动）
      if (fireStable >= FIRE_STABLE && sizeOk) {
        fireStable = 0;
        fireCount++;
        TRACE.print("[确认] 第"); TRACE.print(fireCount);
        TRACE.print("/3 u="); TRACE.print((int)u);
        TRACE.print(" du="); TRACE.print((int)du); TRACE.println("，开火！");
        triggerFire();
        locked = false; calibPhase = 0;          // 每发后重新瞄准（更稳，连发稍慢）
        if (fireCount >= 3 && !holdPrinted) {
          TRACE.println("[hold] 3发完成，保持不动");
          holdPrinted = true;
        }
      }
      return;                                    // 锁定保持：舵机不动
    } else {
      fireStable = 0;                            // 滞回带 50~70：保持锁定、不累计
      return;                                    // 舵机不动
    }
    // ad > LOCK_EXIT → 已解锁，落到下方补瞄
  }

  fireStable = 0;
  if (fabsf(slope) < 0.3f) { calibPhase = 0; return; }   // 斜率异常重校
  // ---------- 位置伺服（未锁定时补瞄；每发后重新校准再瞄准下一发） ----------
  float targetDeg = servoDeg - du / slope;
  float d = targetDeg - servoDeg;
  if (d > OUT_MAX_DEG) d = OUT_MAX_DEG;
  if (d < -OUT_MAX_DEG) d = -OUT_MAX_DEG;
  servoWrite(servoDeg + d);
}

/********** 主程序 ***********/
void setup() {
  DEBUG_SERIAL.begin(DEBUG_BAUD_RATE);
  DEBUG_SERIAL.println("\n\n[二哈2射击·小智AI] 初始化...");

  pinMode(RELAY_PIN, OUTPUT);
  pinMode(BTN_A_PIN, INPUT_PULLUP);
  pinMode(BTN_B_PIN, INPUT_PULLUP);
#if STATUS_LED_PIN >= 0
  pinMode(STATUS_LED_PIN, OUTPUT);
  digitalWrite(STATUS_LED_PIN, LOW);
#endif
  servoInit();
  relaySet(false);
  // 上电摇摆自检：舵机应肉眼可见 30→70→50 扫一遍（验证驱动与机械）
  servoWrite(30.0f); delay(400);
  servoWrite(70.0f); delay(400);
  servoWrite(SERVO_CENTER);

  Wire.begin(I2C_SDA, I2C_SCL);
  while (!huskylens.begin(Wire)) {
    delay(100);
    DEBUG_SERIAL.println("[boot] 等待 HUSKYLENS2…");
  }
  huskylens.switchAlgorithm(ALGORITHM_OBJECT_TRACKING);
  delay(5000);
  DEBUG_SERIAL.println("[boot] 二哈2 就绪，物体追踪");

  setupWifi();
  if (mcpClient.begin(MCP_ENDPOINT, onMcpConnectionChange)) {
    DEBUG_SERIAL.println("[boot] MCP 客户端初始化成功");
  } else {
    DEBUG_SERIAL.println("[boot] MCP 客户端初始化失败!");
  }
  DEBUG_SERIAL.println("[boot] 按键A=搜索开/停，B=手动射击；对话可控制搜索/射击/角度");
}

void loop() {
  mcpClient.loop();

  // 按键轮询（30ms 防抖）
  static bool lastA = true, lastB = true;
  static unsigned long debA = 0, debB = 0;
  bool a = digitalRead(BTN_A_PIN), b = digitalRead(BTN_B_PIN);
  if (!a && lastA && (millis() - debA) > 30) { debA = millis(); running = !running; if (running) fireStable = 0; }
  if (!b && lastB && (millis() - debB) > 30) { debB = millis(); triggerFire(); }
  lastA = a; lastB = b;

  // 串口命令（快捷方式）
  while (TRACE.available()) {
    char c = TRACE.read();
    if (c == 'a') { running = !running; if (running) fireStable = 0; }
    if (c == 'b') triggerFire();
    if (c == 'h') { running = false; servoWrite(SERVO_CENTER); TRACE.println("[cmd] 回中 50°"); }
    if (c == '1') { running = false; servoWrite(30.0f); TRACE.println("[cmd] 转到 30°"); }
    if (c == '2') { running = false; servoWrite(50.0f); TRACE.println("[cmd] 转到 50°"); }
    if (c == '3') { running = false; servoWrite(70.0f); TRACE.println("[cmd] 转到 70°"); }
    if (c == 's') { DEBUG_SERIAL.print("running="); DEBUG_SERIAL.print(running ? "yes" : "no");
                    DEBUG_SERIAL.print(" deg="); DEBUG_SERIAL.print((int)servoDeg);
                    DEBUG_SERIAL.print(" du="); DEBUG_SERIAL.print((int)lastDu);
                    DEBUG_SERIAL.print(" lost="); DEBUG_SERIAL.println(lastLost ? "yes" : "no"); }
    if (c == 'x') running = false;
  }

  // 空闲时打印目标位置（不搜索也能确认目标是否静止）
  static unsigned long lastIdleP = 0;
  if (!running && (millis() - lastIdleP > 500)) {
    lastIdleP = millis();
    float u, v, w, h;
    if (trackU(&u, &v, &w, &h)) {
      TRACE.print("[idle] u="); TRACE.print((int)u);
      TRACE.print(" v="); TRACE.print((int)v);
      TRACE.print(" w="); TRACE.print((int)w);
      TRACE.print(" h="); TRACE.println((int)h);
    } else {
      TRACE.println("[idle] 无目标");
    }
  }

  // 搜索-射击循环（非阻塞 25ms）
  if (running) {
    unsigned long now = millis();
    if (now - lastCtrl >= (unsigned long)CTRL_DT_MS) {
      lastCtrl = now;
      stepShooter();
    }
  } else {
    relaySet(false);
  }

  // 状态LED（STATP_LED_PIN>=0 时）
#if STATUS_LED_PIN >= 0
  if (!wifiConnected)      blinkLed(1, 100);
  else if (!mcpConnected)  blinkLed(1, 500);
  else                     digitalWrite(STATUS_LED_PIN, HIGH);
#endif
  delay(1);
}

// ---------- WiFi ----------
void setupWifi() {
  DEBUG_SERIAL.print("[WiFi] 连接到 ");
  DEBUG_SERIAL.println(WIFI_SSID);
  WiFi.mode(WIFI_STA);
  WiFi.begin(WIFI_SSID, WIFI_PASS);
  int attempts = 0;
  while (WiFi.status() != WL_CONNECTED && attempts < 20) {
    delay(500);
    DEBUG_SERIAL.print(".");
    attempts++;
  }
  if (WiFi.status() == WL_CONNECTED) {
    wifiConnected = true;
    DEBUG_SERIAL.println();
    DEBUG_SERIAL.println("[WiFi] 连接成功! IP: " + WiFi.localIP().toString());
  } else {
    wifiConnected = false;
    DEBUG_SERIAL.println();
    DEBUG_SERIAL.println("[WiFi] 连接失败! 将继续尝试...");
  }
}

// ---------- MCP 工具注册（连接成功后） ----------
void registerMcpTools() {
  DEBUG_SERIAL.println("[MCP] 注册工具...");

  mcpClient.registerTool(
    "search_shoot",
    "开始或停止“搜索目标→自动对准→自动射击”循环。动作 start 或 stop。",
    "{\"properties\":{\"action\":{\"title\":\"动作\",\"type\":\"string\",\"enum\":[\"start\",\"stop\"]}},\"required\":[\"action\"],\"title\":\"searchShootArguments\",\"type\":\"object\"}",
    [](const String& args) {
      DynamicJsonDocument doc(256);
      if (deserializeJson(doc, args)) {
        return WebSocketMCP::ToolResponse("{\"success\":false,\"error\":\"参数错误\"}", true);
      }
      String action = doc["action"].as<String>();
      if (action == "start") { running = true; fireStable = 0; }
      else if (action == "stop") { running = false; }
      String r = "{\"success\":true,\"running\":" + String(running ? "true" : "false") + "}";
      return WebSocketMCP::ToolResponse(r);
    });
  DEBUG_SERIAL.println("[MCP] 工具 search_shoot 已注册");

  mcpClient.registerTool(
    "fire",
    "射击一次：继电器吸合扣扳机。",
    "{\"properties\":{},\"title\":\"fireArguments\",\"type\":\"object\"}",
    [](const String&) {
      triggerFire();
      return WebSocketMCP::ToolResponse("{\"success\":true}");
    });
  DEBUG_SERIAL.println("[MCP] 工具 fire 已注册");

  mcpClient.registerTool(
    "servo_abs",
    "把水平舵机转到指定的绝对角度(0~180)。",
    "{\"properties\":{\"angle\":{\"title\":\"角度\",\"type\":\"number\"}},\"required\":[\"angle\"],\"title\":\"servoAbsArguments\",\"type\":\"object\"}",
    [](const String& args) {
      DynamicJsonDocument doc(256);
      if (deserializeJson(doc, args)) {
        return WebSocketMCP::ToolResponse("{\"success\":false,\"error\":\"参数错误\"}", true);
      }
      float ang = doc["angle"].as<float>();
      running = false;
      bool atLimit = (ang < SERVO_MIN || ang > SERVO_MAX);
      servoWrite(ang);
      TRACE.print("[servo_abs] ang="); TRACE.print(ang, 1);
      TRACE.print(" -> "); TRACE.println((int)servoDeg);
      String r = "{\"success\":true,\"angle\":" + String(servoDeg) +
                 (atLimit ? ",\"at_limit\":true" : "") + "}";
      return WebSocketMCP::ToolResponse(r);
    });
  DEBUG_SERIAL.println("[MCP] 工具 servo_abs 已注册");

  mcpClient.registerTool(
    "servo_rel",
    "水平舵机相对转动：direction 为 left 或 right，degrees 为度数（如向左10度）。",
    "{\"properties\":{\"direction\":{\"title\":\"方向\",\"type\":\"string\",\"enum\":[\"left\",\"right\"]},\"degrees\":{\"title\":\"度数\",\"type\":\"number\"}},\"required\":[\"direction\"],\"title\":\"servoRelArguments\",\"type\":\"object\"}",
    [](const String& args) {
      DynamicJsonDocument doc(256);
      if (deserializeJson(doc, args)) {
        return WebSocketMCP::ToolResponse("{\"success\":false,\"error\":\"参数错误\"}", true);
      }
      String dir = doc["direction"].as<String>();
      float d = doc["degrees"].as<float>();
      if (d <= 0) d = 5.0f;            // 没给度数默认 5°（“向左一点”）
      if (dir == "right") d = -d;      // 方向符号由 SERVO_TURN_DIR 定：1=向右=角度增大
      d *= (float)SERVO_TURN_DIR;
      running = false;                 // 手动转动暂停自动循环
      float oldDeg = servoDeg;
      float target = oldDeg + d;
      bool atLimit = (target < SERVO_MIN || target > SERVO_MAX);
      servoWrite(target);              // 内部限位到 SERVO_MIN~SERVO_MAX
      TRACE.print("[servo_rel] dir="); TRACE.print(dir);
      TRACE.print(" d="); TRACE.print(d, 1);
      TRACE.print(" old="); TRACE.print((int)oldDeg);
      TRACE.print(" new="); TRACE.println((int)servoDeg);
      String r = "{\"success\":true,\"angle\":" + String(servoDeg) +
                 (atLimit ? ",\"at_limit\":true" : "") + "}";
      return WebSocketMCP::ToolResponse(r);
    });
  DEBUG_SERIAL.println("[MCP] 工具 servo_rel 已注册");

  mcpClient.registerTool(
    "status",
    "查询射击系统状态：是否在搜索、舵机角度、目标水平偏差、是否丢失目标。",
    "{\"properties\":{},\"title\":\"statusArguments\",\"type\":\"object\"}",
    [](const String&) {
      String r = "{\"success\":true,\"running\":" + String(running ? "true" : "false") +
                 ",\"angle\":" + String(servoDeg) +
                 ",\"du\":" + String(lastDu) +
                 ",\"lost\":" + String(lastLost ? "true" : "false") + "}";
      return WebSocketMCP::ToolResponse(r);
    });
  DEBUG_SERIAL.println("[MCP] 工具 status 已注册");
  DEBUG_SERIAL.println("[MCP] 工具注册完成，共" + String(mcpClient.getToolCount()) + "个");
}

// ---------- MCP 连接回调 ----------
void onMcpConnectionChange(bool connected) {
  mcpConnected = connected;
  if (connected) {
    DEBUG_SERIAL.println("[MCP] 已连接到MCP服务器");
    registerMcpTools();
  } else {
    DEBUG_SERIAL.println("[MCP] 与MCP服务器断开连接");
  }
}

// ---------- 状态LED（非阻塞闪烁，example 风格） ----------
void blinkLed(int times, int delayMs) {
#if STATUS_LED_PIN >= 0
  static int blinkCount = 0;
  static unsigned long lastBlinkTime = 0;
  static bool ledState = false;
  static int lastTimes = 0;
  if (times == 0) { digitalWrite(STATUS_LED_PIN, LOW); blinkCount = 0; lastTimes = 0; return; }
  if (lastTimes != times) { blinkCount = 0; lastTimes = times; ledState = false; lastBlinkTime = millis(); }
  unsigned long now = millis();
  if (blinkCount < times * 2) {
    if (now - lastBlinkTime > delayMs) {
      lastBlinkTime = now;
      ledState = !ledState;
      digitalWrite(STATUS_LED_PIN, ledState ? HIGH : LOW);
      blinkCount++;
    }
  } else {
    digitalWrite(STATUS_LED_PIN, LOW);
    blinkCount = 0;
    lastTimes = 0;
  }
#endif
}

使用方式

方式	说明
语音	对小智AI 说口令（见"四.4"）
按键	A=搜索开停，B=手动射击
串口	a搜索 b射击 h回中50° s状态 x停止

关键参数

参数	值	说明
DEADBAND_PX	50	锁定进入/开火判据
LOCK_EXIT_PX	70	滞回退出带（超出补瞄）
FIRE_STABLE	10	每发稳定帧（10帧≈250ms）
FIRE_MAX	3	单会话连发上限
RESUME_PX	100	3发后"大幅移动"解锁阈值
OUT_MAX_DEG	1.0	每周期最大转角 40°/s

演示视频

六、调试实录与踩坑（重点干货）

1. 640×480 图像中心：按 320×240 的 (160,120) 设中心，垂直/水平偏差恒偏 ~100px，怎么调都不收敛。协议文档"底边中点(320,480)"才是真相。
2. 二哈2 库宏要求非 const 指针：RET_ITEM_NUM 内部 static_cast<Result*>(func)，传 const Result* 会编译报错。
3. 舵机倒立装=方向镜像：爬山式"偏差变大就反向"在滤波滞后下左右猛甩、落不到中心 → 改成实测斜率标定 + 位置伺服，一次定型、不再猜符号。
4. "锁定后不追"的卡死：目标漂出死区（50）但没到大幅（100）时，既不开火也不补瞄。解决：滞回带（进 50/出 70），漂移 50~70 保持锁定不乱抖，>70 补瞄追回。
5. 每发后重新瞄准：连发间重新标定+补瞄，每一发都"干净锁定"，不累积误差。
6. 开火瞬间舵机停转（最重要的硬件坑）：继电器+扳机机构与舵机共电源，开火吸合瞬间大电流把电压拉低 → 舵机掉力矩/失步。解决：继电器/扳机机构独立供电，只共地；舵机独立 5~6V/2A + 470µF 电容；FIRE_ON_MS 250→120。
7. arduino-esp32 v3 改了 LEDC API：ledcSetup/ledcAttachPin 已删除，新接口 ledcAttach(pin, freq, res) + ledcWrite(pin, duty)（按引脚）。代码里做了 v2/v3 兼容层。
8. ID 锁定 + EMA 滤波 + 残影过滤：追踪框在多目标/场景边缘乱跳、目标跑出视野只剩 4px 残影——分别用 ID 锁定单目标、u 滑动滤波（0.7/0.3）、w<30 视为丢失解决。

七、实测表现

典型日志（锁定连发）：

[aim] 方向已校准 slope=2.100 px/°
[lock] 目标锁定，开始连发
[aim] u=320 du=1 stable=3 center=yes
[确认] 第1/3 u=320，开火！
[aim] 校准中…            ← 每发后重新瞄准
[lock] 目标锁定，开始连发
[确认] 第2/3 …
[确认] 第3/3 …
[hold] 3发完成，保持不动
[hold] 目标大幅移动，重新锁定   ← 目标挪走后重新打

八、文件清单

文件	作用
esp32_huskylens2_shooter_mcp.ino	完整主程序（本文代码块）
esp32_huskylens2_centerlock.ino	最小版：只"搜索-锁定居中"（不含射击/网络）
小智AI自定义角色介绍_移动打靶机器人.md	角色人设与工具说明（含安全规则）

安全提醒：这是射击装置。务必先空枪测试（扳机机构不装弹）验证舵机与继电器；开火方向面对人/动物时拒绝；继电器与扳机机构独立供电、与系统共地；儿童使用需成人陪同。所有"实测后调整"的参数（舵机行程、中位、死区、连发数）请按你的机械结构实测。

本文由完整实测调试过程整理：从 640×480 视觉中心、斜率自标定、锁定-连发-滞回状态机，到供电坑与 LEDC v3 迁移。所有参数为本机示例值，请按你的设备实测微调。