二哈2 视觉跟随机器人：让六舵机机械臂的眼睛盯住目标（微控制器+伺服系统）

二哈2 视觉传感器装在机械臂夹爪上（眼在手上 eye-in-hand），让被追踪的物体始终保持在画面中心——一套"视觉 → 误差 → PID → 平滑伺服"完整闭环的桌面项目：micro:bit + 二哈2（HUSKYLENS2）+ 6×180°舵机。本版项目专注"目标跟随"，抓取闭环留作下一步。

二哈2 视觉跟随机器人：让六舵机机械臂的眼睛盯住目标（微控制器+伺服系统）_image_1.webp

一、项目简介

用 二哈2（DFRobot HUSKYLENS2） 做物体追踪，6 个 180° 舵机组成的桌面机械臂做执行器，实现：

目标跟随（视觉伺服）：物体在桌面上移动时，机械臂持续对准它，让物体保持画面中心；
一键操作：micro:bit 按 A 键对准测试、按 B 键开启/停止跟随；
平滑运动：所有舵机用"限速 + 限加速度"的梯形速度规划驱动，起步不猛冲、到位不猛停。

项目最大的特点是 相机装在夹爪上（眼在手上），不依赖固定相机标定，跟随靠"图像误差 → PID → 舵机增量"的闭环伺服直接完成；空间坐标系与正/逆运动学用一份 YAML 统一描述，先 Python 仿真验证，再移植到 micro:bit 实机跑通。

二、功能特性

开机自检：二哈2 初始化为物体追踪算法，舵机慢速回中位
按键控制：A = 对准测试，B = 跟随（再按停）；串口 f/x/s/h 同效
视觉跟随：双轴 PID（左右 yaw / 径向前后），像素中心保持画面中心
自适应增益：mm/px ≈ 0.6 × 物体实际高度 / 物体像素高度，越近步长越小、收尾越准
平滑轨迹：限速 120°/s、限加速度 150°/s²、制动距离减速（不急起/不急停）
防抖死区：偏差 ≤5px 停止修正，消除过零抖动
软件限位：关节 0~180° 检测，目标不可达时串口提示、平滑停住

三、硬件清单

部件	型号 / 参数	数量
主控	micro:bit V2	1
舵机驱动扩展板	DFRobot micro:bit 八路舵机驱动板（PCA9685，`Microbit_Motor` 库）	1
机械臂	DFRA6DOF 六自由度桌面机械臂（6×180° 舵机）	1
视觉传感器	二哈2 HUSKYLENS2（SEN0638），I2C 接口	1
追踪物	高约 20mm、有辨识度的物体	1
供电	舵机板独立 5~6V 供电（舵机电流大，别用主板 USB）	—

接线：二哈2 与舵机板都接 micro:bit 的 I2C（SDA=P20、SCL=P19，共地）；6 个舵机接驱动板 S1~~S6，对应 J1~~J6；相机固定在夹爪上，朝向跟随目标。
二哈2 视觉跟随机器人：让六舵机机械臂的眼睛盯住目标（微控制器+伺服系统）_image_2.webp

四、系统方案与原理

1. 坐标系与关节约定（YAML 为唯一事实来源）

世界系 table：桌面 Z=0，Z 向上，x 前、y 左（右手系）
基座系 base_j1：J1 旋转中心，相对桌面平移 [0,0,72]mm
TCP：夹爪两指中点；接近方向固定 [0,0,-1]（竖直向下）
连杆：上臂 90mm、前臂 90mm、腕 30mm、工具 10mm
关节角 0~180°、中心 90°；约定 yaw=θ1-90、ψ2=θ2-90、ψ3=ψ2+G(θ3-90)、ψt=ψ3+G(θ4-90)（G=FOLD_GAIN）

2. 正/逆运动学（跟随也要用）

正运动学：关节角 → TCP 桌面坐标，用于校验与固定观察高度
逆运动学：输入桌面点 (x,y,z)，固定爪朝下，二连杆 IK 求 J2/J3/J4（肘上/肘下两解选可用），J1 由方位角给出，J5 保持 90°、J6 控制张合

跟随中径向移动就是"目标半径 +Δr → IK → 关节角"，因此 IK 是跟随闭环的一部分；Python 仿真做了 300 组随机目标往返验证：FK(IK(目标)) ≈ 目标 误差 0，不可达目标正确拒绝。

3. 眼在手上的视觉伺服

相机随手臂移动，用闭环视觉伺服：

物体像素中心 (u,v) ──误差──> PID ──目标增量──> 平滑器 ──> 舵机
  水平偏移 du  → J1(yaw) 左右对正
  垂直偏移 dv  → 径向目标 Δr → IK → J2/J3/J4 前后对正

观察高度 z 固定（扫描高度 110mm），径向目标始终压在该高度上，避免平滑滞后导致高度漂移；半径同时限制在几何可达包络 [minR(z), maxR(z)] 内。

4. PID + 平滑运动（不急起 / 不急停）

PID 输出目标增量（yaw 度数、径向毫米），每 25ms 一个控制周期
平滑器做梯形速度规划：限速 120°/s、限加速度 150°/s²，起步速度按加速度爬升；剩余距离 ≤ 制动距离时目标速度归零按加速度减速
偏差 ≤5px 死区，对准后完全静止

五、程序实现

程序用 Mind+ / Arduino C 编写，核心结构：

setup()   按键注册(A对准/B跟随) → 二哈2初始化 → 舵机慢速回中
loop()    按键/串口分发 + 跟随非阻塞节拍伺服(25ms) + [cam] 打印
followControlAt(zFix)   眼在手上伺服：du→yaw，dv→径向，z固定
servoStepToward()       限速限加速度平滑逼近
eyeHandCenter()         扫描找物 + 居中伺服（对准测试 s）

完整程序

（以下为可直接编译的完整程序 dfra6dof_follow.ino，二哈2 使用官方扩展库 DFRobot_HuskylensV2）

/*!
 * DFRA6DOF 目标跟随 + 抓取（相机装在夹爪上 = 眼在手上）
 *
 * 新增：目标跟随模式（'f'）
 *   - 连续视觉伺服：物体像素中心 (u,v) 经 PID → 图像中心 (160,120)
 *       水平偏 du → PID → J1(yaw) 目标增量
 *       垂直偏 dv → PID → 径向目标增量（r + Δr，同高）→ IK → J2..J4 目标
 *   - 顺滑运动：所有关节用 梯形速度规划 流式逼近目标
 *       限最大速度 VMAX_DEG_S、限最大加速度 AMAX_DEG_S2
 *       （起步加速度受限→不急起；接近目标按制动距离减速→不急停）
 *   - 控制周期 25ms；丢目标时目标保持，平滑停住
 *
 * 其余：扫描+对准+抓取放置流程保留（同样走平滑器，全程无阶跃）。
 * 与 YAML/arm_kinematics.py 同一套约定（table/base_j1/TCP/接近方向 [0,0,-1]，
 * yaw=th1-90，psi2=th2-90，ψ3=ψ2+G(th3-90)，ψt=ψ3+G(th4-90)）。
 *
 * 命令：'f'跟随  'x'停止跟随  's'对准测试  'r'抓取-放置  'h'回中位
 * 安全：TODO 参数实测回填前不接真实舵机（YAML safety.note）；上电先回中位。
 */
#include <math.h>
#include <Microbit_Motor.h>
#include "DFRobot_HuskylensV2.h"

#if defined(ARDUINO_ARCH_NRF5) && defined(Serial1)
#define TRACE Serial1        // micro:bit 的 USB 串口；如果收不到打印就改 Serial
#else
#define TRACE Serial
#endif

// ---------- 结构参数（与 YAML 对应；实测后修改） ----------
const float J1_CENTER_H_MM = 72.0f;  // TODO: 桌面到 J1 旋转中心高度
const float L1_MM = 10.0f;           // TODO: J1->J2 垂直距离
const float L2_MM = 90.0f;           // TODO: 上臂
const float L3_MM = 90.0f;           // TODO: 前臂
const float L4_MM = 30.0f;           // TODO: 腕
const float L5_MM = 10.0f;           // TODO: 工具长(J5->TCP)
const float FOLD_GAIN  = 1.0f;       // TODO: 肘/腕折叠系数，实测 180° 折叠改 2.0
const float J_RANGE_LO = 0.0f, J_RANGE_HI = 180.0f;

// ---------- 跟随/轨迹参数 ----------
// HUSKYLENS2 屏幕为 640x480（协议文档：屏幕底边中点 (320,480)），中心 (320,240)
const float U_CENTER = 320.0f, V_CENTER = 240.0f;
const float OBJECT_HEIGHT_MM = 20.0f;   // TODO: 物体高度（用于 mm/px 自适应增益）
// 接近朝下约束：TCP z ≤132mm 才可爪朝下；扫描/跟随观察高度取 110
const float SCAN_R_MM = 110.0f, SCAN_H_MM = 110.0f;
const float SCAN_YAW_MIN = 20.0f, SCAN_YAW_MAX = 160.0f, SCAN_STEP = 5.0f;
const float GRIPPER_CLOSED = 0.0f, GRIPPER_OPEN = 180.0f;   // 跟随中 J6 保持张开

// ---------- 顺滑运动参数（梯形速度规划） ----------
const float CTRL_DT_MS = 25.0f;         // 控制周期 25ms（50Hz 左右）
const float VMAX_DEG_S = 120.0f;        // 关节最大速度（跟随模式）deg/s
const float AMAX_DEG_S2 = 150.0f;       // 关节最大加速度 deg/s²（不急起/不急停）
const float KV = 8.0f;                  // P 型速度参考增益 1/s
const float SETTLE_DEG = 0.5f;          // 到达判定
const int   MOVE_TIMEOUT_MS = 15000;

// ---------- PID 参数（实测微调） ----------
typedef struct {
  float kp, ki, kd;
  float iMax, outMax;
  float errPrev, i;
} Pid;
// yaw：纯 P（微分会放大像素噪声→正反馈发散；outMax=2°/步，慢而稳）
Pid pidYaw = {0.020f, 0.0f, 0.0f, 0.0f, 2.0f, 0, 0};
// radial：纯 P（outMax=10mm/步，避免目标跑在前引起满幅来回甩）
Pid pidRad = {0.080f, 0.0f, 0.0f, 0.0f, 10.0f, 0, 0};

// ---------- 伺服方向符号（用户实测确认：RAD=-1、YAW=-1 正确） ----------
const int SIGN_RAD = -1, SIGN_YAW = -1;
// 若只有上下或只有左右反，单独取反；相机横装导致 u/v 交换时，把 dv/du 对调

// ---------- 全局状态 ----------
HuskylensV2 huskylens;
Microbit_Motor motorbit;

float current[6];                // 当前命令角度（平滑器每周期更新）
float vel[6];                    // 关节速度状态 deg/s
const float HOME[6] = {90, 90, 90, 90, 90, 0};
float avgU = 0, avgV = 0;
unsigned long lastTrackMs = 0;

volatile bool followOn = false;
bool requestServoTest = false, requestHome = false;
bool requestFollow = false, requestStop = false;
unsigned long lastFollowMs = 0;              // 非阻塞跟随的节拍时间

// 按键回调（函数指针，供 onEvent 注册）
void buttonACallback();
void buttonBCallback();

#define DEG2RAD 0.01745329251f
#define RAD2DEG 57.2957795131f
inline float d2r(float d) { return d * DEG2RAD; }
inline float r2d(float r) { return r * RAD2DEG; }
static inline float clampf(float a, float lo, float hi) {
  return (a < lo) ? lo : (a > hi) ? hi : a;
}

// ---------- 正运动学 ----------
void fk(const float th[6], float tcp[3], float appr[3]) {
  float yaw = d2r(th[0] - 90.0f);
  float p2  = d2r(th[1] - 90.0f);
  float p3  = d2r(th[1] - 90.0f + FOLD_GAIN * (th[2] - 90.0f));
  float pt  = d2r(th[1] - 90.0f + FOLD_GAIN * (th[2] - 90.0f + th[3] - 90.0f));
  float px  = L2_MM*sinf(p2) + L3_MM*sinf(p3) + (L4_MM+L5_MM)*sinf(pt);
  float pz  = L2_MM*cosf(p2) + L3_MM*cosf(p3) + (L4_MM+L5_MM)*cosf(pt);
  float j2z = J1_CENTER_H_MM + L1_MM;
  tcp[0] = px*cosf(yaw);
  tcp[1] = px*sinf(yaw);
  tcp[2] = pz + j2z;
  appr[0] = sinf(pt)*cosf(yaw);
  appr[1] = sinf(pt)*sinf(yaw);
  appr[2] = cosf(pt);
}

bool inRange(const float th[6]) {
  for (int i = 0; i < 6; i++)
    if (th[i] < J_RANGE_LO - 1e-4f || th[i] > J_RANGE_HI + 1e-4f) return false;
  return true;
}

// ---------- 逆运动学（接近竖直向下） ----------
int ikPick(float xt, float yt, float zt, float th[6]) {
  if (fabsf(xt) < 1e-4f && fabsf(yt) < 1e-4f) return 1;
  float yawDeg = r2d(atan2f(yt, xt));
  if (yawDeg < -90.0f - 1e-3f || yawDeg > 90.0f + 1e-3f) return 1;
  th[0] = 90.0f + yawDeg;
  float px = hypotf(xt, yt);
  float pz = (zt - J1_CENTER_H_MM) - L1_MM;
  float wx = px, wz = pz + (L4_MM + L5_MM);
  float r  = hypotf(wx, wz);
  if (r > L2_MM + L3_MM + 1e-3f || r < 1e-3f) return 2;
  float beta = acosf(clampf((r*r + L2_MM*L2_MM - L3_MM*L3_MM) / (2.0f*r*L2_MM), -1.0f, 1.0f));
  float psiW = atan2f(wx, wz);
  for (int s = 0; s < 2; s++) {
    float sign = (s == 0) ? -1.0f : 1.0f;
    float psi2 = psiW + sign * beta;
    float d2x = sinf(psi2), d2z = cosf(psi2);
    float fx = wx - L2_MM*d2x, fz = wz - L2_MM*d2z;
    if (fabsf(fx) < 1e-4f && fabsf(fz) < 1e-4f) continue;
    float psi3 = atan2f(fx, fz);
    th[1] = 90.0f + r2d(psi2);
    th[2] = 90.0f + r2d(psi3 - psi2) / FOLD_GAIN;
    th[3] = 90.0f + r2d(3.14159265358979f - psi3) / FOLD_GAIN;
    th[4] = 90.0f;
    th[5] = GRIPPER_CLOSED;
    if (inRange(th)) return 0;
  }
  return 3;
}

// ---------- PID（含饱和时积分抑制，防 windup 过冲） ----------
float pidStep(Pid *p, float err) {
  p->i += err;
  p->i = clampf(p->i, -p->iMax, p->iMax);
  float d = err - p->errPrev;
  p->errPrev = err;
  float out = p->kp*err + p->ki*p->i + p->kd*d;
  float o = clampf(out, -p->outMax, p->outMax);
  if (fabsf(o) >= p->outMax) p->i *= 0.9f;   // 满幅输出时衰减积分，防止来回满幅推
  return o;
}

// ---------- 顺滑运动：每周期步进一次（梯形速度规划） ----------
// current 沿目标推进：限速、限加速度、制动距离减速 → 不急起/不急停
void servoStepToward(const float goal[6]) {
  unsigned long t0 = millis();
  static unsigned long last = 0;
  if (last == 0) last = millis();
  float dt = (float)(t0 - last) / 1000.0f;
  last = t0;
  dt = clampf(dt, 0.005f, 0.05f);                     // 防跳变
  float vMaxPer = VMAX_DEG_S * dt;                    // 每周期最大位移 deg
  float aMaxPer = AMAX_DEG_S2 * dt;                   // 每周期速度增量 deg/s
  float vCap = VMAX_DEG_S;

  for (int i = 0; i < 6; i++) {
    float err = goal[i] - current[i];
    // 参考速度（P 型，饱和到 vMax）
    float vRef = clampf(KV * err, -vCap, vCap);
    // 制动距离：需在当前速度下能停下；不足则目标速度归零
    float sStop = (vel[i] * fabsf(vel[i])) / (2.0f * AMAX_DEG_S2 + 1e-4f);
    if (fabsf(err) <= sStop + 0.2f) vRef = 0.0f;
    // 限加速度 → v 连续变化
    vel[i] += clampf(vRef - vel[i], -aMaxPer, aMaxPer);
    vel[i] = clampf(vel[i], -vCap, vCap);
    // 积分得位置增量（限幅）
    current[i] = clampf(current[i] + vel[i] * dt, 0.0f, 180.0f);
  }
  // 写舵机
  motorbit.servo(S1, (int)current[0]);
  motorbit.servo(S2, (int)current[1]);
  motorbit.servo(S3, (int)current[2]);
  motorbit.servo(S4, (int)current[3]);
  motorbit.servo(S5, (int)current[4]);
  motorbit.servo(S6, (int)current[5]);
}

// ---------- 阻塞式平滑移动到目标（可达判定） ----------
bool moveSmooth(const float goal[6], float settle = SETTLE_DEG) {
  unsigned long start = millis();
  bool ok = false;
  while (millis() - start < MOVE_TIMEOUT_MS) {
    servoStepToward(goal);
    bool done = true;
    for (int i = 0; i < 6; i++)
      if (fabsf(goal[i] - current[i]) > settle) done = false;
    if (done) { ok = true; break; }
    unsigned long used = millis() - start;
    if (used < (unsigned long)CTRL_DT_MS) delay((unsigned long)CTRL_DT_MS - used);
  }
  for (int i = 0; i < 6; i++) vel[i] = 0.0f;          // 到位清速
  return ok;
}

void goHome() {
  TRACE.println("[home] 回中位（平滑）");
  moveSmooth(HOME);
  float tcp[3], appr[3]; fk(current, tcp, appr);
  TRACE.print("home  TCP("); TRACE.print(tcp[0], 0); TRACE.print(",");
  TRACE.print(tcp[1], 0); TRACE.print(","); TRACE.print(tcp[2], 0); TRACE.println(")");
}

// ---------- 读取追踪（滑动滤波） ----------
bool trackCenter(float *u, float *v, float *w, float *h) {
  huskylens.getResult(ALGORITHM_OBJECT_TRACKING);
  if (!huskylens.available(ALGORITHM_OBJECT_TRACKING)) return false;
  Result *it = huskylens.getCachedCenterResult(ALGORITHM_OBJECT_TRACKING);
  float cx = (float)RET_ITEM_NUM(it, Result, xCenter);
  float cy = (float)RET_ITEM_NUM(it, Result, yCenter);
  avgU = 0.667f*avgU + 0.333f*cx;   // 轻滤波（同时抑制 PID 微分噪声）
  avgV = 0.667f*avgV + 0.333f*cy;
  *u = avgU; *v = avgV;
  *w = (float)RET_ITEM_NUM(it, Result, width);
  *h = (float)RET_ITEM_NUM(it, Result, height);
  lastTrackMs = millis();
  return true;
}

float mmPerPxFromHeight(float hPx) {
  return 0.6f * OBJECT_HEIGHT_MM / fmaxf(hPx, 4.0f);
}

// ---------- 可达半径包络（接近朝下 + ±90° 折叠约束） ----------
float minRadiusAt(float z) {
  float wz = z - (J1_CENTER_H_MM + L1_MM) + (L4_MM + L5_MM);   // 腕心相对肩高
  float t = 127.28f * 127.28f - wz * wz;    // 折叠≤90° ⇒ 腕心距肩 ≥ L2*√2
  if (t < 0) t = 0;
  return sqrtf(t);
}
float maxRadiusAt(float z) {
  float wz = z - (J1_CENTER_H_MM + L1_MM) + (L4_MM + L5_MM);
  float t = 180.0f * 180.0f - wz * wz;      // 臂全伸 180mm
  if (t < 0) t = 0;
  return sqrtf(t);
}

// ---------- 跟随控制：PID → 目标增量 → 平滑器（每周期调用一次） ----------
// zFix：观察高度 —— 径向目标固定压在该高度，防止平滑滞后导致 z 漂移
bool followControlAt(float zFix) {
  float u, v, w, h;
  if (!trackCenter(&u, &v, &w, &h)) {
    // 丢目标：保持目标（平滑器自然减速停住），积分慢衰减
    pidYaw.i *= 0.9f; pidRad.i *= 0.9f;
    return false;
  }
  float du = u - U_CENTER, dv = v - V_CENTER;
  // 死区：偏差小于 5px 不再修正，消除越过零点的来回摆动
  float outY = (fabsf(du) > 5.0f) ? pidStep(&pidYaw, du) : 0.0f;   // deg（J1 增量）
  float outR = (fabsf(dv) > 5.0f) ? pidStep(&pidRad, dv) : 0.0f;   // mm（径向增量）

  // yaw 目标
  float goal0 = clampf(current[0] + (float)SIGN_YAW * outY, 0.0f, 180.0f);

  // 径向目标：当前半径 + Δr，固定高度 zFix，半径限在可达包络内 → IK
  float tcp[3], appr[3]; fk(current, tcp, appr);
  float r = hypotf(tcp[0], tcp[1]);
  float ang = atan2f(tcp[1], tcp[0]);
  float r2 = clampf(r + (float)SIGN_RAD * outR, minRadiusAt(zFix), maxRadiusAt(zFix));

  // 调试打印（跟随/居中时每周期一行，判断方向与收敛）
  TRACE.print("[ctl] du="); TRACE.print((int)du); TRACE.print(" dv="); TRACE.print((int)dv);
  TRACE.print(" outY="); TRACE.print(outY, 1); TRACE.print(" outR="); TRACE.print(outR, 1);
  TRACE.print(" r="); TRACE.print((int)r); TRACE.print("/");
  TRACE.print((int)minRadiusAt(zFix)); TRACE.print("..");
  TRACE.println((int)maxRadiusAt(zFix));   // 看半径是否顶在限幅上（不可达特征）
  float goal2[6];
  for (int i = 0; i < 6; i++) goal2[i] = current[i];
  goal2[0] = goal0;
  goal2[5] = GRIPPER_OPEN;                       // 跟随中夹爪张开
  float th[6];
  if (ikPick(r2*cosf(ang), r2*sinf(ang), zFix, th) == 0) {
    goal2[1] = th[1]; goal2[2] = th[2]; goal2[3] = th[3];
    goal2[4] = 90.0f;
  }
  servoStepToward(goal2);                        // 平滑逼近（限速限加速度）
  return true;
}

// ---------- 开启跟随（非阻塞：实际控制在主 loop 里按节拍执行，
//          保证按键可随时打断/停止） ----------
void followLoop() {
  TRACE.println("[follow] 目标跟随开启（B 再按停止）");
  followOn = true;
}

// ---------- 扫描 + 居中伺服（抓取前，走平滑器） ----------
bool eyeHandCenter(float *ax, float *ay) {
  int scanDir = 1;
  bool found = false;
  for (int i = 0; i < 60 && !found; i++) {
    float u, v, w, h;
    if (trackCenter(&u, &v, &w, &h)) found = true;
    else {
      float tar[6];
      for (int k = 0; k < 6; k++) tar[k] = current[k];
      tar[0] += scanDir * SCAN_STEP;
      if (tar[0] > SCAN_YAW_MAX) { tar[0] = SCAN_YAW_MAX; scanDir = -1; }
      if (tar[0] < SCAN_YAW_MIN) { tar[0] = SCAN_YAW_MIN; scanDir = +1; }
      moveSmooth(tar);
      delay(80);
    }
  }
  if (!found) { TRACE.println("[eye] 扫描未找到物体"); return false; }

  // 居中（PID 短迭代收敛）
  for (int it = 0; it < 40; it++) {
    float u, v, w, h;
    if (!trackCenter(&u, &v, &w, &h)) break;
    float du = u - U_CENTER, dv = v - V_CENTER;
    if (fabsf(du) <= 8.0f && fabsf(dv) <= 8.0f) break;
    if (!followControlAt(SCAN_H_MM)) break;
  }
  float u, v, w, h;
  if (!trackCenter(&u, &v, &w, &h) ||
      fabsf(u - U_CENTER) > 8.0f || fabsf(v - V_CENTER) > 8.0f) {
    TRACE.println("[eye] 未收敛（目标可能不可达）");
    return false;
  }
  float tcp[3], appr[3]; fk(current, tcp, appr);
  *ax = tcp[0]; *ay = tcp[1];
  TRACE.print("[eye] 对准完成 @ "); TRACE.print(*ax, 0); TRACE.print(",");
  TRACE.print(*ay, 0); TRACE.print(" z="); TRACE.println(tcp[2], 0);
  return true;
}

// ---------- 对准测试 ----------
void servoTest() {
  float tar[6];
  if (ikPick(SCAN_R_MM, 0.0f, SCAN_H_MM, tar) == 0) moveSmooth(tar);
  float tx, ty;
  if (!eyeHandCenter(&tx, &ty)) {
    TRACE.println("[test] 未对准；若臂反向移动请翻转 SIGN_RAD/SIGN_YAW");
    return;
  }
  TRACE.println("[test] 对准成功");
}

// ---------- 主程序 ----------
// ---------- 按键回调（实际动作仍在主 loop 中执行，回调只置标志） ----------
void buttonACallback() {
  if (followOn) { followOn = false; return; }   // 跟随中按 A = 停止跟随
  requestServoTest = true;                      // 空闲按 A = 对准测试
}
void buttonBCallback() {
  if (followOn) { followOn = false; return; }   // 再按 B = 停止跟随
  requestFollow = true;                         // 空闲按 B = 开启跟随
}

// ---------- 主程序 ----------
void setup() {
  TRACE.begin(115200);
  // 按键：A=对准测试，B=跟随（再按停）—— Mind+ micro:bit 事件机制
  onEvent(ID_BUTTON_A, PRESS, buttonACallback);
  onEvent(ID_BUTTON_B, PRESS, buttonBCallback);

  Wire.begin();
  while (!huskylens.begin(Wire)) {
    delay(100);
    TRACE.println("[boot] 等待 HUSKYLENS2…");
  }
  huskylens.switchAlgorithm(ALGORITHM_OBJECT_TRACKING);
  delay(5000);
  TRACE.println("[boot] HUSKYLENS2 就绪（相机在夹爪上）");

  for (int i = 0; i < 6; i++) current[i] = 90.0f;
  TRACE.println("[boot] 上电回中位（平滑）");
  moveSmooth(HOME);

  TRACE.println("[boot] 按键：A对准 B跟随(再按停)；串口 f/x/s/r/h 同效");
}

void loop() {
  while (TRACE.available()) {
    char c = TRACE.read();
    if (c == 'f') requestFollow = true;
    if (c == 'x') requestStop = true;
    if (c == 's') requestServoTest = true;
    if (c == 'h') requestHome = true;
  }
  if (followOn) {
    // 跟随中：每 CTRL_DT_MS 执行一次伺服控制（非阻塞，按键可打断）
    if (requestStop || requestServoTest || requestHome) {
      followOn = false;
      TRACE.println("[follow] 已停止（平滑停住）");
    }
    unsigned long now = millis();
    if (now - lastFollowMs >= (unsigned long)CTRL_DT_MS) {
      lastFollowMs = now;
      followControlAt(SCAN_H_MM);
    }
    requestStop = requestServoTest = requestHome = false;
  } else {
    if (requestFollow) { requestFollow = false; followLoop(); }
    if (requestServoTest){ requestServoTest = false; servoTest(); }
    if (requestHome)     { requestHome = false;     goHome(); }
  }
  // 空闲打印（含跟随模式时也可观察）
  if (millis() - lastTrackMs > 500) {
    float u, v, w, h;
    if (trackCenter(&u, &v, &w, &h)) {
      TRACE.print("[cam] 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);
    }
  }
  delay(5);
}

二哈2 视觉跟随机器人：让六舵机机械臂的眼睛盯住目标（微控制器+伺服系统）_image_6.webp

使用方式（按键 / 串口均可）

操作	功能
按 A	对准测试：扫描找物 → 自动居中（验证方向）
按 B	开启跟随；再按一次停止
串口 `f` / `x`	开启跟随 / 停止跟随
串口 `s` / `h`	对准测试 / 回中位

关键参数表（按你的机械实测调整）

参数	值	说明
`U_CENTER / V_CENTER`	320 / 240	二哈2 屏幕是 640×480，图像中心不是 160/120！
`SIGN_RAD / SIGN_YAW`	-1 / -1	伺服方向符号（按安装实测）
`pidYaw`	kp=0.02, outMax=2°	纯 P，避免噪声放大
`pidRad`	kp=0.08, outMax=10mm	纯 P
`VMAX_DEG_S / AMAX_DEG_S2`	120 / 150	平滑限速、限加速度
`SCAN_R / SCAN_H`	110 / 110 mm	扫描半径/高度（高度 ≤132 才保证爪朝下）
`OBJECT_HEIGHT_MM`	20	用于 mm/px 自适应增益

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

YAML 语法坑：length_mm:90 少冒号后空格，解析直接失败——先修数据文件，再谈代码。
二哈2 图像中心是 640×480 的 (320,240)：按 320×240 的 (160,120) 设中心，垂直偏差恒偏 ~100px，怎么调都不收敛——这是"不收敛"最大的假象来源。
方向符号必须实测：SIGN_RAD/SIGN_YAW 由安装方向决定，先按 A 对准测试，看臂是"追着目标"还是"把目标赶出画面"。本案例实测 (-1,-1)。
PID 微分项在像素噪声上正反馈发散：加了 kd 后 du 从 ±15 放大到 ±200，振幅倍增。像素中心本来就在抖，微分=放大器。改用纯 P + 死区，一阶系统天然收敛。
积分饱和（windup）：输出长期顶限幅时积分继续累积，目标跑在平滑器前面 → 半径来回满幅甩。处理：饱和时积分衰减 + 收紧 outMax。
几何约束：观察高度 ≤132mm："中心90=直臂、±90°折叠"约定下，爪要朝下，腕心必须在肩上方 ≤90mm ⇒ TCP 高度 ≤132mm，扫描/平移高度取 110mm 即源于此；跟随可达半径约 107~166mm，物体拖出圈外就跟不上了。

七、实测效果与数据

Python 仿真（与实机同方程）验证控制律：

指标	结果
IK 往返	300 样本，误差 0
伺服收敛	26 周期（≈0.65s）到位，±2px 噪声下仍收敛
J1 峰值速度	37°/s（上限 120）
速度阶跃/周期	= 加速度上限 3.75°/s（无阶跃 → 不急起急停）

实机跟随：物体慢速拖动时 du 有界在 ±26px、dv 约 18px、半径稳定停在可达区，无发散、无满幅甩动；物体静止时进入死区完全不动。

跟随演示

八、下一步改进

抓取闭环（本版暂不实现）：对准后垂直下降 → 闭合 → 抬起 → 平移 → 放置，逆运动学部分已就绪，文章后续版本开放
实测肘/腕折叠行程，调 FOLD_GAIN 扩大可达范围
多目标 ID 锁定：只跟随指定物体
学习模式（learn）：让二哈2 只认要跟的物体

九、附录：文件清单

文件	作用
`dfra6dof_follow.ino`	跟随主程序（本文代码段，Mind+ 直接编译）
`dfra6dof_follow_standalone.ino`	单文件内联库版本（Mind+ 链接失败时的兜底）
`arm_kinematics.py` / `arm_tests.py`	Python 运动学模型 + 单关节/正逆解/限位测试
`simulate_pick_place.py` / `run_all.py`	轨迹仿真与一键全流程验证（含后续抓取段）