#include <Arduino.h>
#include <SimpleFOC.h>
#include <SPI.h>

/* =========================================================
 * Makerbase MKS ESP32 FOC V2.0 —— **单轴**云台（AS5048A SPI 版）
 *
 * 由 SimpleFOC_ESP32FOC（双轴 AS5600 版）改造而来：
 *   1) 砍掉第二轴（M1/命令 A 的电机与驱动）—— 本机是单轴设备。
 *   2) 只换传感器层：MagneticSensorI2C(AS5600_I2C) → MagneticSensorSPI(AS5048_SPI)。
 *      原因：本机云台电机的编码器是 **AS5048A**（只有 SPI 和 PWM，**没有 I²C**）
 *      ⇒ 原版两次 I2C 扫描必然为空，电机 "sensor NOT found, skip init"。
 *
 * ★ 接线（与 v08 单轴版完全一致，全部取板载编码器座现成脚，免焊）
 *   CLK =GPIO18（座0 SCL_0）    MISO=GPIO19（座0 SDA_0）
 *   MOSI=GPIO23（座1 SDA_1）    CS  =GPIO5 （座1 SCL_1）
 *   3.3V / GND 从任一座取（本板无 5V 针脚；AS5048A 走 3V 模式需 pin11-pin12 短接）。
 *
 * ★ 限压（安全红线）：云台电机相阻 15.2Ω ⇒ 工作限压 **2.0V**；
 *   对齐期临时给 4.0V（SimpleFOC 会把 voltage_sensor_align 钳到 ≤ voltage_limit，
 *   钳到 2.0 会力矩不足 ⇒ initFOC 失败），对齐结束立刻收回 2.0V。
 *   原版写死 12.0V，对本机电机偏猛，不可沿用。
 *
 * 串口命令（115200，SimpleFOC Commander）：
 *   B = 本机唯一电机（沿用原版"上面电机 = B"的编号）
 *   A = 同一电机的**别名**（原版 A 指下面电机；单轴设备上两个前缀都指向这台，
 *       避免上位机发 A 时无响应）
 *   示例: BE1  BC1  B2   /   AE1  AC1  A2
 *
 * ★ 在线扫参（不用重编重烧，直接在同一个串口控制台敲；前缀 B，A 是别名）：
 *   BAI<v>  位置环 I  ← ★ 锁不住/差一点点到位，先调这个
 *   BAP<v>  位置环 P          BVP<v> / BVI<v>   速度环 P / I
 *   BLV<v>  位置模式最大角速度(=P_angle.limit) ★ 治超调的主旋钮，实测最优 1.5
 *   BLU<v>  电压限幅          BMG<n>  取一次状态(n: 0=目标 1=Vq 5=速度 6=角度 7=全部)
 *   BAF<v>  LPF_angle.Tf      BVF<v>  LPF_velocity.Tf
 *   回读：把值去掉即可，例如 BAI → "PID angle| I: 20.000"
 * ★ 判断"是没力还是环路没要力"：BMD1 打开监视（target/Vq/vel/angle 四列），BMD0 关。
 *   停住不动时 Vq 只有 0.1~0.4V ⇒ 环路没要力（缺 I）；
 *   Vq 顶到 ~2.0V 轴还不动 ⇒ 力矩不足（齿槽井/静摩擦）。
 *
 * ★★ 2026-09-22 起：AS5048A 读帧改为**带校验**（见下面 AS5048AGuarded）。
 *   病象「停住后细微抖动、频率不高」的真凶是编码器**单帧坏数据**
 *   （实测约 2 次/秒，幅度 1°~220°）被位置环当成真位置
 *   ⇒ 立刻输出一记力矩让轴抽一下。
 *   SimpleFOC 的 MagneticSensorSPI 丢掉了 AS5048A 的 bit15(偶校验)/bit14(EF)。
 *   串口敲 X 可看丢帧计数与主循环频率。
 * ========================================================= */

// ---------- 电机引脚 ----------
#define M0_IN1 32
#define M0_IN2 33
#define M0_IN3 25
#define EN_PIN 12

// ---------- 编码器 SPI 引脚 ----------
#define SPI_CLK  18
#define SPI_MISO 19
#define SPI_MOSI 23
#define PIN_CS    5

// ---------- 电源 / 限压 / 速度 ----------
#define SUPPLY_VOLTAGE      12.0f
#define VOLTAGE_LIMIT_RUN    2.0f   // 工作限压（云台电机安全值）
#define VOLTAGE_LIMIT_ALIGN  4.0f   // 仅对齐期，initFOC 需要足够力矩
#define MAX_TARGET_VELOCITY  8.0f   // 速度模式目标上限；同时是 P_angle.limit 的抗饱和边界
#define PWM_FREQUENCY        20000

// ---------- 开机默认模式 ----------
// 本机是单轴定位设备 ⇒ 默认位置(角度)模式。要回纯速度模式改 MotionControlType::velocity。
#define DEFAULT_MODE         MotionControlType::angle

// ---------- 位置环 / 速度环 参数（★ 有实测依据，别凭直觉改） ----------
/*  位置模式在 SimpleFOC 里是一条级联环：
 *      位置误差 --P_angle--> 速度指令 --PID_velocity--> voltage.q
 *
 *  第三方原值 PID_velocity.P = 0.05 ⇒ 速度环**几乎只有积分**：
 *  巡航时积分攒到 ~1V；外环要等到离目标 23° 才撤速度指令，
 *  而内环光靠积分需要约 0.5s 才把这个电压放掉 —— 这 0.5s 电机仍在
 *  以 8rad/s(≈458°/s) 往前冲 ⇒ **冲过头、来回摆几次、锁不住**。就是本次的病根。
 *
 *  本型号实测锚点（GM5208-12 + AS5048A，12N/14P，限压 2.0V）：
 *      PID_vel.P=0.5 / I=1.5        ⇒ 无超时、残差 ≤0.3°、5° 阶跃超调 ~22%
 *      P_angle.I=0.5 ⇒ 个别目标残差 0.70°（停在齿槽井里）；=20 ⇒ 三个目标全 ≤0.3°
 *   12N/14P ⇒ 齿槽井周期 = 360/lcm(12,14) = **4.29°**，
 *   ⇒ 小于一个井宽的位置误差会被井壁"卡住"，**位置环必须有 I** 才顶得出去。
 *      （这正是"锁不住/差一点点"的唯一有效旋钮。）
 *
 *  ★★ LPF_velocity.Tf 必须留在 0.10s（2026-09-22 实测，别改回去！）
 *  早期把"加大 LPF_velocity.Tf"列为反例，那是在治**小角度静差**的语境下：
 *  Tf 越大相位滞后越大 ⇒ 小误差下静态刚度略降。但本机的主诉是
 *  「停住后细微抖动」，它的机理完全不同：
 *     主循环 6.7kHz ⇒ 速度 = 约 150µs 内的位置差 / 150µs，
 *     而编码器只有 14 位 ⇒ **1 LSB 的位置变化 = ±147°/s 的原始速度**！
 *  Tf=0.03 时速度估计仍会被量化噪声推到 ~86°/s，经速度环 P=0.5/I=1.5
 *  放大成一记真力矩脉冲(Vq 达 0.72V)，把转子弹出去 ~1.3°，再花 10s 爬回。
 *  同一位置 45s 实测：
 *     Tf=0.03  |vel|max 85.97°/s  >20°/s 4 点  Vqpp 0.7222V  角度峰峰 1.51°
 *     Tf=0.10  |vel|max  1.56°/s  >20°/s 0 点  Vqpp 0.0421V  角度峰峰 0.15°  ← 选它
 *     Tf=0.20  |vel|max  0.50°/s  >20°/s 0 点  Vqpp 0.0347V  角度峰峰 0.18°
 *  100° 阶跃回归：超调 +3.93°(原 +4.52°)、到位 2.9s、残差 -0.025°、翻转 1(原 2)
 *  ⇒ 不但没变差，反而略好。0.20 再往上收益递减（且滞后更大），故取 0.10。
 *  仍然成立的反例：**只单调抬速度环 P**。
 */
#define P_ANGLE_P            20.0f
#define P_ANGLE_I            20.0f   // ★★ 消静差、顶出齿槽井
#define PID_VEL_P            0.5f    // ★ 原第三方 0.05，差 10 倍 ⇒ 内环刹不住
#define PID_VEL_I            1.5f
#define PID_VEL_RAMP         100.0f
#define LPF_VEL_TF           0.10f   // ★★ 压速度估计的量化尖峰，见上方实测（别改小）

/* ★★ 位置模式的最大角速度（= P_angle.limit，位置环积分的抗饱和边界）
 *
 * 这是"冲过头"的**主旋钮**。下面是本机实测（100° 阶跃，位置模式，每档都复位重跑）：
 *
 *   P_angle.limit   峰值超调    末态残差    末段抖动    翻转   峰值速度
 *   8.0 (原值)      +15.4/+15.7  +0.08/+0.19  0.36      9/7    6.4 rad/s
 *   1.5             + 5.1/+ 5.0  -0.07/-0.14  1.03/0.70  3     1.85 rad/s   ← 选它
 *   2.0             + 5.3        +0.08        2.11              3.26
 *   3.0             + 7.5        +0.41        2.15              3.60
 *   5.0             +14.7        -0.27        3.08       8      4.24
 *   （另外试过 P_angle.P 20→8：超调反而涨到 +11.6°、残差 -3.0° ⇒ **P 别动，保持 20**）
 *   （也试过 P_angle.I 20→5 保持全速：超调降到 +6.0° 但残差 +1.07°、抖动 3.3°、7s 没到位 ⇒ 不如限速）
 *
 * ⇒ 结论：内环在加速段会顶到 ~1.8V(≈电压限幅 2.0V)，**加速段是硬件饱和的**，
 *   光调增益救不了超调；把"要它跑多快"限住（1.5 rad/s ≈ 86°/s）才治本。
 *   代价：100° 行程 0.35s → 约 1.1s。要更快就把这个值往上调一档，超调同步变大。
 */
#define POS_SLEW_LIMIT       1.5f

// ---------- 传感器：AS5048A（SPI，14 位）—— ★ 带读帧校验 ----------
/* ★★ 为什么不直接用 SimpleFOC 的 MagneticSensorSPI
 *
 * AS5048A 返回的 16bit 帧结构：bit15=偶校验  bit14=错误标志EF  bit13..0=14bit角度。
 * 而 MagneticSensorSPI::read() 最后只有一句 `& 0x3FFF`，
 * **校验位和 EF 全被丢掉** ⇒ 一帧坏数据会被当成"真实位置"。
 *
 * 实测（345Hz 抓波形，_fast_trace.py / _fast_trace.csv，2026-09-22）：
 *   使能保持时单帧角度跳变 ≈ **2.0 次/秒**，幅度见过 ~1° 也见过 100°/162°/219°；
 *   电机失能（PWM 停）时只有 0.12 次/秒  ⇒ 与 PWM 开关耦合进 SPI 的干扰相关；
 *   跳变值出现过 0.0000（EF 帧的典型返回值）。
 * 位置环把一个假位置当成真值 ⇒ 立刻输出一记力矩 ⇒ 轴"抽一下"，
 * 现象正是「停住后细微抖动、频率不高」。
 *
 * 本类自己发命令帧、自己读回，四道防线（越靠前越"治本"）：
 *   ① **真·模 2π 归约**（v20 修的核心）：把原始角度对 2π 取整倍归约后再与
 *      上一连续值相减，永远取"最近的等价角"。老写法只做"一次 ±2π 修正"
 *      （if(d>π)d-=2π; if(d<-π)d+=2π），只在 |a-last|<3π 时才对；而 last 是
 *      **连续域**值（可达 ±200rad）、坏帧给的 a 却恒在 [0,2π) ⇒ 差值能到 200rad，
 *      一次修正不够 ⇒ u 落到**错误的圈数**，3 个这样的值进窗口就能把中值/ last
 *      整圈拽走 ⇒ shaft_angle 跳 360° 的整倍。改成真归约后坏帧误差被限在 ≤π，
 *      **再也动不了圈数**。
 *   ② **帧级步长合理性**（readValid，越界就重读）：轴在一个周期内物理上走不了
 *      AS5048_STEP_EPS（2°）⇒ 超限即假读数，立即重读（最多 3 次）。
 *      ★★ 绝不"沿用上一个输出"来冻结：v18 试过，实测是**严重回归** ——
 *         100s 保温里冒出 7 次 ±36~54° 暴走，Vq 打到 ±2V 限幅、|vel| 峰值 516°/s，
 *         而 v16/v17 同口径是 0 次。冻结输出让位置反馈停在旧值、位置环却继续加力
 *         ⇒ 正反馈自激。重读 3 次仍不合理才兜底采用，宁可接受一帧稍差的真值。
 *   ③ **死总线帧显式剔除**：MISO 恒低/恒高时整帧读回 0x0000 / 0xFFFF，
 *      而它们的**偶校验恰好都能通过**（1 的个数分别是 0 / 16，都是偶数）
 *      ⇒ 解码成 0° / 359.98° 的"合法"假角度，只查校验根本拦不住。显式剔除。
 *   ④ **5 点中值滤波**：连串坏帧(≤2 帧)无论跳多大都被中值直接挤掉
 *      （正常运动只是滞后 2 个控制周期 ≈0.30ms，可忽略）。兜底用。
 *   另有**现场记录**（只计数不干预）：中值输出与上一输出差 > 2.9° 时记 n_burst，
 *   并把现场(原始帧/解码值/展开值/中值/上一输出)存进 8 条环形缓冲，
 *   串口敲 X 打印 ⇒ 不必再靠推理猜坏帧长什么样。
 * ★ 初始化/对齐期 guard=false：对齐时轴会一次扫过约 51°，中值窗口会被灌坏。
 * ★★ 实测（2026-09-22，X 诊断 + 原始帧抓取；一次会话 1,730,271 帧）：
 *   - 本机 AS5048A 的 bit14(EF) **恒为 1**（100% 的帧）⇒ EF 不能当判据；
 *   - bit15 确是偶校验位，但**几乎从不失败**（219s 内只命中 2 次）⇒ 校验也抓不到坏帧；
 *   - 坏帧率 ≈ **0.25%**（6700Hz 下 ≈16 帧/秒）；
 *   - 好帧的低 14 位角度完全正确（实测 2963 ⇒ 65.10°，真值 65.11°）。
 *   ⇒ 唯一有效且无损的手段就是**中值滤波**（阈值兜底会漏掉 <2.9° 的坏帧）。
 * ★★ 为什么从 3 点改成 5 点：坏帧会**连续成串**。3 点中值只能吃掉 1 帧，
 *   遇到"2 连坏帧"时中值仍取到坏值 ⇒ 仍往外送 1 个假位置，
 *   实测抓到过一次 **53.9°** 的读数尖峰。5 点中值可吃掉 2 连坏帧，
 *   代价只是多滞后 1 个周期（0.15ms）。
 * 串口敲 X 看计数：SA=好帧 JU=被中值挤掉的帧 BS=连串穿透 PA=校验失败
 *                  DB=死总线帧  GL=步长越界重读  loop=主循环频率  另有连串穿透现场 EV
 */
#define AS5048_CPR       16384       // 2^14
#define AS5048_MED_N     5           // 中值窗口点数（★ 5 才吃得住 2 连坏帧）
#define AS5048_MED_EPS  0.005f      // 判定"这帧被挤掉"的阈值 ≈0.29°（计数用）
#define AS5048_BURST_EPS 0.05f      // 连串穿透的日志阈值 ≈2.9°（只记录，不拦截）
#define AS5048_STEP_EPS  0.035f     // 帧级步长合理上限 ≈2°（越界就重读，不冻结）
class AS5048AGuarded : public Sensor {
public:
  bool     guard  = false;           // 对齐完成后由 setup() 打开
  uint32_t n_ok = 0, n_jump = 0, n_par = 0, n_ef = 0, n_burst = 0, n_dead = 0, n_glitch = 0;
  // 连串穿透现场（X 命令打印）：触发瞬间的原始帧与各级中间量
  struct Ev { uint16_t rx; float a, u, m, last; uint8_t good; };
  Ev       ev[8] = {};
  uint8_t  ev_i  = 0;
  // 诊断（X 命令打印）
  uint16_t dbg_first[4] = {0, 0, 0, 0};
  uint16_t dbg_ring[16] = {0};
  uint16_t dbg_par = 0;
  uint8_t  dbg_i = 0;
  uint32_t n_frame = 0;
  bool diagPrimed() const { return primed; }

  explicit AS5048AGuarded(int cs_pin = PIN_CS) : cs(cs_pin) {}

  void init(SPIClass* sp = &SPI) {
    spi = sp;
    set = SPISettings(1000000, MSBFIRST, SPI_MODE1);   // AMS 上限 10MHz，1MHz 最稳
    pinMode(cs, OUTPUT);
    digitalWrite(cs, HIGH);
    spi->begin(SPI_CLK, SPI_MISO, SPI_MOSI, cs);
    Sensor::init();     // 基类把 full_rotations / angle_prev 对齐到当前位置
  }

protected:
  float getSensorAngle() override {
    uint16_t rx = 0;
    bool good = readValid(rx);
    float raw = (float)(rx & 0x3FFF) * (_2PI / (float)AS5048_CPR);
    if (!guard) { primed = false; return raw; }   // 初始化/对齐期不设防

    float a = good ? raw : (primed ? last : raw);          // 偶校验坏 => 沿用上次好值
    if (!primed) {
      primed = true; last = a;
      for (uint8_t i = 0; i < AS5048_MED_N; i++) h[i] = a;
      hi = 0;
      return a;
    }
    /* ★★ 真·模 2π 归约（v20 核心修复，见文件头 ①）
     * 老写法只做"一次 ±2π 修正"，|a-last| 超过 3π 时就会算错圈数。
     * 实测现场（X 的 EV 行）：真值 5.3452rad，读到 rx=4004(角度=4) ⇒
     * u=6.2847、last=12.5652 —— 圈数已被拽走。 */
    float d = a - last;
    d -= _2PI * roundf(d / _2PI);      // 归约到 (-π, π]，与 |d| 多大无关
    float u = last + d;

    h[hi % AS5048_MED_N] = u;
    hi++;
    if (hi < AS5048_MED_N) { last = u; return u; }         // 先把窗口填满

    // ★ 5 点中值：连串坏帧(≤2 帧)被挤掉；正常运动只滞后 2 周期(≈0.30ms)
    //   返回的永远是"窗口内真实采到过的值"，不做插值 ⇒ 不会引入虚假位置
    float m = median5(h[0], h[1], h[2], h[3], h[4]);
    if (fabsf(m - u) > AS5048_MED_EPS) n_jump++;           // 被挤掉的帧（计数）
    if (fabsf(m - last) > AS5048_BURST_EPS) {              // 连串穿透：只记录，不拦截
      n_burst++;
      Ev& e = ev[ev_i & 7]; ev_i++;                        // 记现场，供 X 打印
      e.rx = rx; e.a = a; e.u = u; e.m = m; e.last = last; e.good = good ? 1u : 0u;
    }
    last = m;
    n_ok++;
    return m;
  }

private:
  uint16_t frame() {
    uint16_t cmd = 0x3FFF | (1u << 14);                 // bit14=1 表示读
    if (__builtin_parity(cmd) & 0x1) cmd |= (1u << 15); // 补成偶校验
    spi->beginTransaction(set);
    digitalWrite(cs, LOW);
    spi->transfer16(cmd);
    digitalWrite(cs, HIGH);
    delayMicroseconds(50);                              // AMS 要求 CS 上升后 >=350ns
    digitalWrite(cs, LOW);
    uint16_t rx = spi->transfer16(0x0000);
    digitalWrite(cs, HIGH);
    spi->endTransaction();
    if (n_frame < 4) dbg_first[n_frame] = rx;
    n_frame++;
    dbg_ring[dbg_i & 15] = rx;
    dbg_i++;
    return rx;
  }

  /* 5 点取中（插入排序；5 个数最多 10 次比较） */
  static float median5(float a, float b, float c, float d, float e) {
    float v[5] = {a, b, c, d, e};
    for (int i = 1; i < 5; i++) {
      float k = v[i];
      int j = i - 1;
      while (j >= 0 && v[j] > k) { v[j + 1] = v[j]; j--; }
      v[j + 1] = k;
    }
    return v[2];
  }

  static bool parityOk(uint16_t rx) {
    return (__builtin_parity(rx) & 0x1) == 0;   // 偶校验：整帧 1 的个数须为偶数
  }

  /* ★★ 死总线帧：MISO 恒低/恒高时整帧读回 0x0000 / 0xFFFF。
   *    它们的偶校验**恰好都能通过**（1 的个数 0 / 16 都是偶数），
   *    只查校验根本拦不住 ⇒ 解码出 0° / 359.98° 的假角度，
   *    再被"跨 0/2π 展开"放大成 50° 级的记号翻转。必须显式剔除（v19 核心修复）。 */
  static bool deadBus(uint16_t rx) { return rx == 0x0000 || rx == 0xFFFF; }

  /* ★★ 帧的"步长合理性"：把原始角度按 2π 归约到 (-π, π] 与上一连续值比较。
   *    轴在一个控制周期内物理上走不了 AS5048_STEP_EPS（2°）：主循环 6.6kHz 下
   *    位置模式 0.013°/周期、速度模式上限 0.070°/周期，主循环即便卡 20ms 也才 1.7°
   *    ⇒ 超限就是假读数，**立即重读**（绝不冻结输出，理由见文件头 ②）。
   *    只在 guard 打开（对齐结束）后判：对齐期轴会大范围扫动。 */
  bool stepPlausible(uint16_t rx) {
    if (!primed || !guard) return true;
    float a = (float)(rx & 0x3FFF) * (_2PI / (float)AS5048_CPR);
    float d = a - last;
    d -= _2PI * roundf(d / _2PI);
    return fabsf(d) <= AS5048_STEP_EPS;
  }

  bool readValid(uint16_t& rx) {
    for (int k = 0; k < 3; k++) {                // 最多读 3 次，挑一帧"干净"的
      uint16_t r = frame();
      if (deadBus(r)) { n_dead++; dbg_par = r; continue; }  // 死总线：丢弃重读
      if (!parityOk(r)) {                                   // 校验不过：丢弃重读
        n_par++; dbg_par = r;
        if ((r >> 14) & 0x1) n_ef++;                        // EF 只统计，不判决
        continue;
      }
      rx = r;
      if (stepPlausible(r)) return true;
      n_glitch++;                                           // 越界：重读
    }
    return false;     // 三次都没拿到"干净"帧 ⇒ 调用方沿用 last，只冻结 1 帧
  }

  int         cs;
  SPIClass*   spi = nullptr;
  SPISettings set;
  bool        primed = false;
  float       last   = 0.0f;
  float       h[AS5048_MED_N] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f};  // 中值窗口（连续域）
  uint8_t     hi     = 0;
};

AS5048AGuarded sensor(PIN_CS);

// ---------- 电机 / 驱动 ----------
BLDCMotor motor = BLDCMotor(7);
BLDCDriver3PWM driver = BLDCDriver3PWM(M0_IN1, M0_IN2, M0_IN3, EN_PIN);

// ---------- Commander ----------
Commander command = Commander(Serial);

bool  g_foc_ok  = false;
float lastTgt   = 0.0f;

// 清速度环积分：SimpleFOC 2.2.x 的 PIDController 没有 reset()，用同样参数重建对象等价清零。
// 治「卡死后换向/停车半天才动、不松」。
void resetVelocityPID(BLDCMotor& m) {
  m.PID_velocity = PIDController(m.PID_velocity.P, m.PID_velocity.I, m.PID_velocity.D,
                                 m.PID_velocity.output_ramp, m.PID_velocity.limit);
}

void onMotor(char* cmd) {
  command.motor(&motor, cmd);
  float t = motor.target;
  if (fabsf(t - lastTgt) > 0.001f) resetVelocityPID(motor);
  lastTgt = t;
}

/* 串口敲 X：打印编码器丢帧计数（JU=被中值挤掉的坏帧）+ 主循环频率。 */
uint32_t g_loop_cnt = 0;
void onStats(char*) {
  static uint32_t last_cnt = 0, last_us = 0;
  uint32_t now = micros();
  uint32_t dc = g_loop_cnt - last_cnt, dt = now - last_us;
  float hz = dt ? (dc * 1e6f / (float)dt) : 0.0f;
  last_cnt = g_loop_cnt;
  last_us  = now;
  Serial.printf("SA%lu JU%lu BS%lu PA%lu EF%lu DB%lu GL%lu guard=%d loop=%.0fHz ang=%.4f\n",
                (unsigned long)sensor.n_ok,    (unsigned long)sensor.n_jump,
                (unsigned long)sensor.n_burst, (unsigned long)sensor.n_par,
                (unsigned long)sensor.n_ef,    (unsigned long)sensor.n_dead,
                (unsigned long)sensor.n_glitch,
                (int)sensor.guard, hz, motor.shaft_angle);
  Serial.printf("  FR %04X %04X %04X %04X lastPAR %04X primed %d\n",
                sensor.dbg_first[0], sensor.dbg_first[1],
                sensor.dbg_first[2], sensor.dbg_first[3],
                sensor.dbg_par, (int)sensor.diagPrimed());
  for (int k = 0; k < 16; k += 4)
    Serial.printf("  RG %04X %04X %04X %04X\n",
                  sensor.dbg_ring[k], sensor.dbg_ring[k + 1],
                  sensor.dbg_ring[k + 2], sensor.dbg_ring[k + 3]);
  /* 连串穿透现场：rx=原始帧 a=解码值 u=展开值 m=中值 last=上一输出 good=校验是否通过 */
  for (int k = 0; k < 8; k++) {
    const AS5048AGuarded::Ev& e = sensor.ev[k];
    if (!e.rx && !e.good) continue;
    Serial.printf("  EV rx=%04X good=%u | a=%.4f u=%.4f m=%.4f last=%.4f (rad)\n",
                  e.rx, (unsigned)e.good, e.a, e.u, e.m, e.last);
  }
}

/* 判断编码器是否真的有响应。
 * AS5048A 在 SPI 无响应时 MISO 恒定（读回全 0 或全 1）⇒ 角度锁死不变。
 * 连读 12 帧；若出现任何变化（真实噪声会让最低位抖动）即判为有响应。 */
bool sensorResponsive(Sensor& s) {
  float a0 = s.getAngle();
  for (int i = 0; i < 12; i++) {
    s.update();
    if (fabsf(s.getAngle() - a0) > 1e-5f) return true;
  }
  return false;
}

void setup() {
  Serial.begin(115200);
  delay(1000);

  // 关闭底层驱动报错刷屏
  esp_log_level_set("*", ESP_LOG_NONE);

  /* ---- 编码器 ---- */
  SPI.begin(SPI_CLK, SPI_MISO, SPI_MOSI, PIN_CS);
  sensor.init(&SPI);
  delay(50);

  bool ok = sensorResponsive(sensor);
  if (ok) {
    Serial.printf("SPI encoder: READY (CS%d) angle=%.4frad\n", PIN_CS, sensor.getAngle());
  } else {
    Serial.printf("SPI encoder: NO REPLY (CS%d) —— 查 CLK18/MISO19/MOSI23/CS5 与 3.3V/GND\n", PIN_CS);
  }

  /* ---- 驱动初始化前：三相 PWM 脚先设上拉输入，防上下桥直通 ---- */
  pinMode(32, INPUT_PULLUP); pinMode(33, INPUT_PULLUP); pinMode(25, INPUT_PULLUP);

  /* ---- 驱动 ---- */
  driver.enable_active_high = true;
  driver.voltage_power_supply = SUPPLY_VOLTAGE;
  driver.pwm_frequency = PWM_FREQUENCY;
  driver.voltage_limit = VOLTAGE_LIMIT_ALIGN;
  driver.init();

  motor.linkSensor(&sensor);
  motor.linkDriver(&driver);

  motor.foc_modulation = FOCModulationType::SinePWM;
  motor.controller = DEFAULT_MODE;

  // 速度环（位置模式的内环：必须够硬，否则外环撤指令后刹不住 ⇒ 冲过头）
  motor.PID_velocity.P = PID_VEL_P;
  motor.PID_velocity.I = PID_VEL_I;
  motor.PID_velocity.D = 0.0f;
  motor.PID_velocity.output_ramp = PID_VEL_RAMP;
  motor.LPF_velocity.Tf = LPF_VEL_TF;

  // 位置环（★ SimpleFOC 默认是 P=20 / **I=0**，I 不设就没有消除静差的能力）
  motor.P_angle.P = P_ANGLE_P;
  motor.P_angle.I = P_ANGLE_I;
  motor.P_angle.D = 0.0f;
  motor.P_angle.output_ramp = 0.0f;
  motor.LPF_angle.Tf = 0.0f;

  motor.voltage_limit  = VOLTAGE_LIMIT_ALIGN;   // 对齐期临时抬到 4.0
  motor.velocity_limit = MAX_TARGET_VELOCITY;

  // 强制每次上电重校（与 STM32 版一致）
  motor.voltage_sensor_align = VOLTAGE_LIMIT_ALIGN;
  motor.sensor_direction     = NOT_SET;
  motor.zero_electric_angle  = NOT_SET;

  Serial.println("Motor init...");

  if (ok) {
    motor.init();
    motor.voltage_sensor_align = VOLTAGE_LIMIT_ALIGN;  // init() 会钳到 voltage_limit，这里复原
    Serial.print("FOC init... ");
    g_foc_ok = motor.initFOC();
    Serial.println(g_foc_ok ? "OK" : "FAILED");
    motor.disable();
    sensor.guard = g_foc_ok;   // ★ 对齐结束才开读帧校验（对齐期轴会大范围扫动）
    Serial.printf("[SENSOR] median-%d + parity + deadbus + step-reject %s | eps %.3frad (%.2fdeg) step %.3frad (%.2fdeg)\n",
                  AS5048_MED_N, sensor.guard ? "ON" : "off",
                  AS5048_MED_EPS, AS5048_MED_EPS * 57.29578f,
                  AS5048_STEP_EPS, AS5048_STEP_EPS * 57.29578f);
  } else {
    Serial.println("encoder NO REPLY, skip init（电机不会动，但设备保持在线）");
  }

  /* ★ 对齐结束 ⇒ 把限压收回安全值。原版这里是 12V，对云台电机偏猛。 */
  motor.voltage_limit      = VOLTAGE_LIMIT_RUN;
  motor.PID_velocity.limit = VOLTAGE_LIMIT_RUN;  // ★ 速度环输出限幅才是真正限电压的那条
  Serial.printf("[CLAMP] align done -> voltage_limit=%.1fV\n", VOLTAGE_LIMIT_RUN);

  /* ★ motor.init() 内部会写 P_angle.limit = velocity_limit，这里覆盖成实测最优值：
     它就是位置环积分的**抗饱和边界**（外环速度指令最多到这么大，不会无限涨）。
     ★ 注意 velocity_limit 只被角度模式/开环到用，速度模式的目标**不受它限制**
       ⇒ 改这里不会影响速度模式的手感。 */
  motor.P_angle.limit = POS_SLEW_LIMIT;

  /* ★ 上电目标 = 当前位置：否则每次 BE1 使能时电机会"猛地跳回编码器零点"。 */
  if (g_foc_ok) motor.target = motor.shaftAngle();

  motor.monitor_downsample = 0;    // 监视默认关；用 BMD1 开、BMD0 关

  command.add('B', onMotor, (char*)"motor_main");
  command.add('A', onMotor, (char*)"motor_alias");
  command.add('X', onStats, (char*)"sensor_stats");

  Serial.println("========================================");
  Serial.printf("Ready. single-axis enc=AS5048A(SPI) cmd=A|B  mode=%s\n",
                (motor.controller == MotionControlType::angle) ? "POSITION(rad)" : "velocity(rad/s)");
  Serial.printf("  P_angle P=%.1f I=%.1f limit=%.1f | PID_vel P=%.2f I=%.1f limit=%.1fV BVF=%.2fs\n",
                motor.P_angle.P, motor.P_angle.I, motor.P_angle.limit,
                motor.PID_velocity.P, motor.PID_velocity.I, motor.PID_velocity.limit,
                motor.LPF_velocity.Tf);
  Serial.println("  POS unit = rad :  100deg = 1.745rad    (deg / 57.2958)");
  Serial.println("  ex: BE1  B1.745     /     AE1  A1.745");
  Serial.println("  tune live: BAI20 BAP20 BVP0.5 BVI1.5 BLV1.5 BLU2.0 BVF0.10 | watch: BMD1");
  Serial.printf("  fw v20: AS5048A median-%d + parity + deadbus + step-reject | vel LPF %.2fs | stats: X\n",
                AS5048_MED_N, LPF_VEL_TF);
  Serial.println("  measured @100deg: overshoot +3.93deg, residual -0.025deg, 1 flip");
  Serial.println("========================================");
}

void loop() {
  if (g_foc_ok) {
    motor.loopFOC();
    motor.move();
    motor.monitor();      // 默认惰性（monitor_downsample=0）；BMD1 打开
  } else {
    motor.voltage.q = 0;
    motor.voltage.d = 0;
    driver.setPwm(0, 0, 0);
  }
  command.run();
  g_loop_cnt++;
}