|
#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++;
|
|
}
|