项目概述
该项目使用 NXP FRDM-MCXN236 与 Zephyr，按 SimpleFOC 的 estimated current（估算电流）模式实现了一个 FOC 力反馈旋钮，支持 5 种不同的手感：自由、阻尼、齿槽、限位、回中，通过 USB HID 连接到电脑实现音量旋钮或滚轮。
控制方案：控制环由 MRT 中断定节拍跑在 10 kHz、单次更新 35.5 µs，min-max 注入写成三路 40 kHz 占空比；没有电流采样，电流由模型估算，闭环止于 AS5600 的角度。
手感与安全：自由、阻尼、齿槽、限位、回中五种手感都是位置与转速的一条函数，三级钳位按“模式 → 阻尼 → 限速”依次收紧，另外角度过期 50 ms 或转速持续 200 ms 过快都会停机。
USB 交互：一个 HID 接口、两个报告（音量位与鼠标形状的滚轮），每 10° 行程一步、上限 200 步/秒（约 330 rpm），SW2 切换功能、SW3 轮换手感、OLED 显示状态。
硬件平台
整套系统由一块 FRDM-MCXN236、一块 SimpleFOC Mini 驱动板、一台 2804 云台无刷电机（外挂AS5600 磁编码器）和一块 SSD1306 128×64 OLED 组成。
SimpleFOC Mini 驱动板没有电流采样，所以只能实现电压模式的 FOC，在这个项目里我参考 SimpleFOC 的估算电流模式，通过电机电感、相电阻来估算电流。
FRDM-MCXN236 自带的 USB，可以实现 USB HID 接口， 给电脑报告音量加/减位以及滚轮。电机轴每转过 10°（可配置）输出一步，每 5 ms 发一次报告。SW2 可以在音量与滚轮之间切换，SW3 轮换五种手感，OLED 显示当前手感、关键参数、以及 USB 功能与故障计数。
系统架构
系统自下而上可以分成感知、控制、执行三层。
1、感知层：AS5600，由独立的编码器线程以 1 kHz 采样。每采一次，线程把原始角度换算成连续机械角、电角度（乘极对数、减去标定得到的零点偏移）、转速（一阶滤波得到）发布到共享状态里。
2、控制层：唯一的闭环，跑在最内层。控制环线程优先级最高，节拍由 MRT 通道 0 的中断给出：中断给一个容量为 1 的信号量，唤醒控制环线程，这样控制环就可以用固定的频率（10Khz）进行。每个节拍里，控制环线程获取当前的状态数据，计算电角速度，再按样本年龄把角度外推到“现在”。接着根据手感定律输出一 q 轴电流(mA)；用相电阻、Kv、电感这组模型参数把电流换算成 dq 两轴电压，做逆 Park 得到静止两轴电压；调 modulate_ab()，min-max 注入把电压变成三个千分比占空比；最后写桥臂寄存器。
3、执行层：SimpleFOC Mini 驱动板与电机，通过 EN 引脚和三个 eFlexPWM 通道（40 kHz 载波）驱动，驱动板把它变成三相电压，定子磁场就落在比转子 d 轴超前 90 电角度的方向上。机械闭环把执行层接回感知层：电机的轴上装着磁铁，磁铁的角度被 AS5600 读到，于是控制层下一拍拿到的样本就是刚才那次转矩的结果。这条回路里没有电流采样：电流是由模型估算出来的值，闭环真正闭合在角度上。
旁路共三条：
1、USB 泵线程：读 enc 里的机械角，按行程每 10° 发一步 HID 报告，与手感完全无关，所以驱动关着、用手转动电机时宿主机照样收到音量或滚轮事件。
2、主循环：把编码器、驱动、手感、USB 四份状态拼成四行字符串，只在和上一帧不同时更新 OLED，平时只是睡醒比较一次。
3、输入线程：用按键 SW3 切换手感、SW2 切换 USB 功能。
三相 PWM 与驱动使能
在设备树 overlay 添加 EN 引脚和 3 个 PWM：
/ {
zephyr,user {
/* 高有效：高电平放开驱动输出，低电平让输出高阻。 */
driver-en-gpios = <&gpio2 0 GPIO_ACTIVE_HIGH>;
/* 三相按 A、B、C 排。真正的周期在 pwm_setup() 里算。 */
pwms = <&flexpwm1_pwm0 0 0 PWM_POLARITY_NORMAL>,
<&flexpwm1_pwm0 1 0 PWM_POLARITY_NORMAL>,
<&flexpwm1_pwm1 0 0 PWM_POLARITY_NORMAL>;
};
};
&pinctrl {
foc_pwm0: foc_pwm0 {
group0 {
pinmux = <PWM1_A0_PIO3_12>,
<PWM1_B0_PIO2_7>;
slew-rate = "fast";
drive-strength = "low";
};
};
foc_pwm1: foc_pwm1 {
group0 {
pinmux = <PWM1_A1_PIO3_14>;
slew-rate = "fast";
drive-strength = "low";
};
};
};
接着把两个子模块打开。nxp,prescaler = <1> 让计数器跑在 150 MHz 总线时钟上；nxp,reload = "full-cycle" 让占空比写入在计数器回零时整体生效：
&flexpwm1_pwm0 {
status = "okay";
pinctrl-0 = <&foc_pwm0>;
pinctrl-names = "default";
nxp,prescaler = <1>;
nxp,reload = "full-cycle";
};
&flexpwm1_pwm1 {
status = "okay";
pinctrl-0 = <&foc_pwm1>;
pinctrl-names = "default";
nxp,prescaler = <1>;
nxp,reload = "full-cycle";
};
prj.conf 配置 CONFIG_PWM，开启 Zephyr 的 PWM 驱动：
# 三相 PWM，来自 eFlexPWM1 的子模块 0 和 1。
CONFIG_PWM=y
EN 引脚
EN 必须在 PWM 驱动配置它的引脚之前就被拉低。否则在这两个 init 之间，MCU 的 PWM 脚可能短暂输出高电平，这里用Zephyr 的 init 优先级控制这个启动顺序：
/* 在 GPIO 控制器（CONFIG_GPIO_INIT_PRIORITY）之后、PWM 驱动
* （CONFIG_PWM_INIT_PRIORITY）之前，于是 PWM 引脚还在配置时桥臂就是关的。
*/
#define DRIVER_EN_INIT_PRIORITY 45
const struct gpio_dt_spec driver_en =
GPIO_DT_SPEC_GET(DT_PATH(zephyr_user), driver_en_gpios);
static int driver_en_init(void)
{
if (!gpio_is_ready_dt(&driver_en)) {
printk("Driver EN pin is not available\n");
return -ENODEV;
}
/* 低电平让输出高阻 */
return gpio_pin_configure_dt(&driver_en, GPIO_OUTPUT_INACTIVE);
}
SYS_INIT(driver_en_init, POST_KERNEL, DRIVER_EN_INIT_PRIORITY);
占空比用千分比存放在 duty_permille[]，pwm_apply_all() 把所有相位写下去：
static int pwm_apply(unsigned int phase)
{
uint32_t pulse;
if (phase >= PHASE_COUNT || pwm_period[phase] == 0U) {
return -EINVAL;
}
pulse = (uint32_t)(((uint64_t)pwm_period[phase] * duty_permille[phase]) /
DUTY_FULL);
if (pulse > pwm_period[phase]) {
pulse = pwm_period[phase];
}
/* 计数已知，就走 pwm_set_cycles()，跳过它内部的纳秒换算。 */
return pwm_set_cycles(phases[phase].dev, phases[phase].channel,
pwm_period[phase], pulse, phases[phase].flags);
}
int pwm_apply_all(void)
{
for (unsigned int i = 0; i < PHASE_COUNT; i++) {
int ret = pwm_apply(i);
if (ret < 0) {
printk("Failed to set phase %u: %d\n", i, ret);
return ret;
}
}
return 0;
}
AS5600 角度采样
Zephyr 已经有 AS5600 驱动，可以直接使用：
CONFIG_SENSOR=y
CONFIG_AMS_AS5600=y
CONFIG_SENSOR_LOG_LEVEL_ERR=y
因为 OLED 用了板子的 LPI2C2，所以编码器另开 flexcomm5，走 Arduino D14/D15，并把速率提到 400 kHz:
&flexcomm5_lpi2c5 {
clock-frequency = <I2C_BITRATE_FAST>;
as5600: as5600@36 {
compatible = "ams,as5600";
reg = <0x36>;
};
这里硬件有点小问题，Zephyr 的 AS5600 驱动会一直提示磁铁太弱，但实际测试是能用的。所以我配置了 CONFIG_SENSOR_LOG_LEVEL_ERR，不打印 warning 日志，然后读取数据的时候要读原始数据，忽略掉 STATUS 状态。
整套固件只有一个传感器：角度。旋钮的定律要微分它，控制环要用它放置磁场，USB 报告要数它的变化。所以它被单独对待：自己的 I2C 总线、自己的线程、自己的时间戳来源。
采样线程本身很短：按周期醒来调用 encoder_poll()，落后了就重新对齐时间基准而不是累积补偿。连续失败 5 次后把周期拉长到 1 秒，避免在 115200 的串口上刷屏：
static void encoder_thread(void *p1, void *p2, void *p3)
{
int64_t next = k_uptime_get();
ARG_UNUSED(p1);
ARG_UNUSED(p2);
ARG_UNUSED(p3);
while (1) {
int64_t now;
if (!encoder_active) {
k_msleep(50);
next = k_uptime_get();
continue;
}
encoder_poll();
next += encoder_period_ms();
now = k_uptime_get();
if (next <= now) {
/* 落后了，多半是传感器正在退避。重新对齐，不要补课。 */
next = now;
} else {
k_msleep(MAX(1, (int32_t)(next - now)));
}
}
}
encoder_poll() 的主体，按顺序做：
1、打时间戳，采一次样
2、处理磁场守卫：忽略掉芯片把磁铁判成了太弱，直接读 RAW ANGLE（0x0C）当角度用
3、绕圈计数与方向
4、转速估计
5、计算电角度，然后发布
FOC 从原理到 dq 更新
手感的本质是"转速下转矩的表达"。要让 -300 mA × sin(2π·pos/30°) 这样的定律真的成为手上的力，必须满足两件事：转矩正比于一个电流指令，而且这个比例不随轴的位置变化。
dq 控制解决的就是这个：把定子电流拆成两路，一路沿转子磁极方向（d 轴，只发热不出力），一路垂直于它（q 轴，只出力）。转矩
T = (3/2) · p · λ · iq
在 d 轴对齐、q 轴正交的前提下与机械角无关。于是"手感"退化成 iq 关于位置和速度的函数。
三条变换
项目里它们就地内联在 src/foc.h：
/* Clarke：三相 → 静止两相。系数 2/3 是为了让幅值不变。 */
static inline void clarke(float a, float b, float c, float *alpha, float *beta)
{
*alpha = (2.0f / 3.0f) * (a - 0.5f * b - 0.5f * c);
*beta = (2.0f / 3.0f) * (SQRT3_2 * (b - c));
}
/* Park：静止两相 → 转子两相（st/ct 是电角度 θ 的正余弦）。 */
static inline void park_sc(float alpha, float beta, float st, float ct,
float *d, float *q)
{
*d = alpha * ct + beta * st;
*q = -alpha * st + beta * ct;
}
/* 逆 Park：转子两相 → 静止两相，调制器要的是这个。 */
static inline void inv_park_sc(float d, float q, float st, float ct,
float *alpha, float *beta)
{
*alpha = d * ct - q * st;
*beta = d * st + q * ct;
}
为什么用 st/ct 而不是角度：控制环里用不到 atan2，只需要旋转矩阵，把角度的正余弦先算好传进来少一次三角函数调用。
估算电流：没有电流采样，电流靠模型估算
这套硬件没有电流传感器（没有采样电阻，也没有母线电压测量）。所以闭环只到"我想要的 iq"为止，后面靠电机模型把 iq 换算成电压：
vq = R · iq + ωe · λ /* q 轴：电阻压降 + 反电动势 */
vd = -ωe · Lq · iq /* d 轴：交叉耦合，低速下很小 */
其中 λ 是磁链，由 Kv 推出来：
#define FLUX_WB ((2.0f / 3.0f) * (9.5493f / (float)CONFIG_FOC_MOTOR_KV) / \
(float)CONFIG_FOC_POLE_PAIRS)
Kt = 9.5493 / Kv 是"N·m 每安培"的经典换算（Kv 单位 rpm/V）。Kv 220 时 Kt = 43.4 mN·m/A，也就是 1 A 只有 43 毫牛·米——一根手指轻松给出几百毫牛·米，所以整套力反馈的有用电流区间就是几百毫安。这就是后面所有参数用 mA 而不是 A 的原因。
模型的绝对精度取决于相电阻、Kv/磁链、母线电压、电感，没有电流采样就无法把它们分开。这对手感的绝对强度有影响，对形状没有影响。
调制：min-max 注入实现 SVPWM
modulate_ab 通过 min-max 注入实现 SVPWM，计算出三个占空比：
void modulate_ab(float alpha, float beta)
{
float v[PHASE_COUNT];
float vmax, vmin, mid;
float mag = sqrtf(alpha * alpha + beta * beta);
if (mag > (float)CONFIG_FOC_PHASE_VOLTAGE_LIMIT_MV) {
float scale = (float)CONFIG_FOC_PHASE_VOLTAGE_LIMIT_MV / mag;
alpha *= scale;
beta *= scale;
}
v[PHASE_A] = alpha;
v[PHASE_B] = -0.5f * alpha + SQRT3_2 * beta;
v[PHASE_C] = -0.5f * alpha - SQRT3_2 * beta;
/* 三个相电压的中值。相电流只取决于三者之差，所以给三相同时加一个
* 相同的量电机看不见，可以随便选。取中点让最高与最低关于零对称，
* 桥臂能给出的幅值从 Vbus/2 提高到 Vbus/sqrt(3)：同样的母线多出
* 15.5% 的电压。 */
vmax = v[0];
vmin = v[0];
for (unsigned int i = 1; i < PHASE_COUNT; i++) {
vmax = MAX(vmax, v[i]);
vmin = MIN(vmin, v[i]);
}
mid = (vmax + vmin) * 0.5f;
for (unsigned int i = 0; i < PHASE_COUNT; i++) {
float permille = (float)DUTY_ZERO_VECTOR +
((v[i] - mid) /
(float)CONFIG_FOC_BUS_VOLTAGE_MV) * DUTY_FULL;
if (permille < 0.0f) {
permille = 0.0f;
}
if (permille > (float)DUTY_FULL) {
permille = (float)DUTY_FULL;
}
duty_permille[i] = (uint16_t)permille;
}
}
占空比用千分比（1000 表示常高）而不是纳秒，因为 PWM 计数器频率要运行时才知道。零矢量是把三相都写到 DUTY_ZERO_VECTOR = 500：三个相端同电位，绕组里没有电流。
一次 dq 更新
static void dq_step(float theta_e, float omega_e)
{
float iq_ref = CLAMP((float)drive.iq_ma / 1000.0f, -IQ_MAX_A, IQ_MAX_A);
float vq = drive.r_ohm * iq_ref + omega_e * FLUX_WB;
float vd = -omega_e * drive.l_h * iq_ref;
float alpha, beta;
float st, ct;
sin_cos_deg(theta_e, &st, &ct);
inv_park_sc(vd, vq, st, ct, &alpha, &beta);
/* 矢量是伏，调制器要毫伏。 */
modulate_ab(alpha * 1000.0f, beta * 1000.0f);
}
这里的 theta_e 就是转子的电角度，也就是编码器报出的 enc.elec（按样本年龄外推后的值）。逆 Park 把 vq 放在 q 轴上，得到的 αβ 矢量因此比转子 d 轴超前 90 电角度：d=0 时 alpha = −vq·sinθ、beta = vq·cosθ，矢量角正好是 θ+90°。定子磁场这样才与 d 轴正交，产生转矩。若把电压放在 d 轴上（vd），定子磁场与转子对齐，效果是"吸住不让转"——那是标定用的对齐测试，不是控制。
vq 里的两项分别是电阻压降和反电动势补偿：前者让"要 100 mA"在静止时也成立，后者让同样的 iq 在高速下仍然成立。因为没有电流环，所以这两项一旦不准，实际电流就偏离请求——这就是为什么 KV 和相电阻的标定会直接影响手感的绝对强度。
10 kHz 控制环与角度守卫
控制环的节拍来自 MRT 通道的中断，所以要把计数器子系统打开；它每次停止通道都会打一条 warning，而驱动每停一次就要停一次，所以那条也一起关掉：
# MRT 通道给控制环定时，它的中断决定周期，而不是线程睡眠。
CONFIG_COUNTER=y
# MRT 驱动在每次停止通道时打一条 warning——驱动每停一次就停一次。
# 只留它的错误。1 = ERR。
CONFIG_COUNTER_LOG_LEVEL_ERR=y
控制环不能靠 k_sleep()：内核睡眠按 tick 取整，CONFIG_SYS_CLOCK_TICKS_PER_SEC=10000 下一格是 100 µs，而一次一格的睡眠实测接近 175 µs，把睡眠线程卡在 4 kHz 以下。改用 MRT 通道的中断发信号量：
static const struct device *const tick_dev =
DEVICE_DT_GET(DT_NODELABEL(mrt0_channel0));
K_SEM_DEFINE(tick_sem, 0, 1);
static void tick_cb(const struct device *dev, void *user_data)
{
ARG_UNUSED(dev);
ARG_UNUSED(user_data);
if (k_sem_count_get(&tick_sem) == 0U) {
k_sem_give(&tick_sem);
}
}
static int tick_init(void)
{
struct counter_top_cfg cfg = {
.ticks = counter_get_frequency(tick_dev) /
CONFIG_FOC_MOTOR_RATE_HZ,
.callback = tick_cb,
};
if (!device_is_ready(tick_dev)) {
LOG_ERR("tick timer %s is not ready", tick_dev->name);
return -ENODEV;
}
if (cfg.ticks < 2U) {
LOG_ERR("tick period of %u counts is too short", cfg.ticks);
return -EINVAL;
}
/* 只保存周期和回调，真正启动它的是 drive_set_state() 里的
* counter_start()。 */
return counter_set_top_value(tick_dev, &cfg);
}
SYS_INIT(tick_init, APPLICATION, CONFIG_APPLICATION_INIT_PRIORITY);
信号量容量是 1，所以落后时丢的是补不回来的旧节拍，而不是积压。这一点在调试里很重要：如果环被打断，看到的是丢帧计数，不是随时间增长的延迟。
控制循环本身不长：等一个节拍、取一次快照、检查角度是否还新鲜、算一次定律、写一次桥臂。后面还会在中间再插一段超速守卫，这里先用注释标出它的位置。
static void drive_thread(void *p1, void *p2, void *p3)
{
ARG_UNUSED(p1);
ARG_UNUSED(p2);
ARG_UNUSED(p3);
while (1) {
float theta_e = 0.0f;
float omega_e = 0.0f;
float rpm = 0.0f;
float mech = 0.0f;
int64_t sample_us = 0;
int64_t age = 0;
int64_t age_tick = 0;
bool angle_ok = false;
/* 超时只在这台定时器本身停摆时才起作用：那种情况下这个循环
* 继续循环，而不是阻塞在一个永远不会到来的节拍上。驱动空闲
* 时计数器是停的，所以这也是两次运行之间的节拍来源。 */
k_sem_take(&tick_sem, K_MSEC(500));
/* 先取编码器快照再拿 drive_lock，两把锁不会同时握着。 */
if (encoder_active) {
int64_t last_ok;
k_mutex_lock(&encoder_lock, K_FOREVER);
last_ok = enc.last_ok_us;
if (enc.samples > 0) {
theta_e = enc.elec;
rpm = enc.speed_rpm;
mech = enc.mech;
/* 旋钮的定律用它给自己的速度估计打时间戳，
* 所以要的是这个戳，不是值。 */
sample_us = enc.last_ok_us;
}
k_mutex_unlock(&encoder_lock);
int64_t now_cyc = k_cyc_to_us_floor64(k_cycle_get_64());
int64_t now_tick = k_ticks_to_us_floor64(k_uptime_ticks());
age = (now_cyc - last_ok) / 1000;
age_tick = (now_tick - last_ok) / 1000;
angle_ok = (age <= CONFIG_FOC_DQ_ANGLE_TIMEOUT_MS);
omega_e = rpm / 60.0f * (float)CONFIG_FOC_POLE_PAIRS *
2.0f * FOC_PI;
/* 把手里的样本外推到"现在"，磁场才落在转子现在的位置
* 而不是采样时的位置。不做这一步，磁场平均滞后样本
* 年龄这么多：800 rpm 下是 50 电角度，施加的电压被拆
* 成一小份出力的 q 和一大份只发热的 d。 */
if (drive.extrapolate && angle_ok) {
float step = (float)MIN(age,
EXTRAPOLATE_MAX_MS);
theta_e += omega_e * step / 1000.0f *
180.0f / FOC_PI;
theta_e = fmodf(theta_e, 360.0f);
if (theta_e < 0.0f) {
theta_e += 360.0f;
}
}
}
k_mutex_lock(&drive_lock, K_FOREVER);
if (!drive.running) {
k_mutex_unlock(&drive_lock);
continue;
}
if (!angle_ok) {
/* 角度冻住的闭环会把磁场停在一个不动的方向上：转子
* 被强行按住，电流一直流，而且没有反电动势限制它。
* 停机。 */
drive_fault_stop();
LOG_ERR("knob stopped: no fresh encoder sample for %lld ms "
"(the tick clock says %lld ms)",
(long long)age, (long long)age_tick);
continue;
}
/* 第 8 章在这里接上超速守卫：它要用手感的限速参数。 */
/* 定律先算，下一次更新用它；位置来自刚取到的样本。 */
drive.iq_ma = knob_step(mech, rpm, sample_us);
dq_step(theta_e, omega_e);
pwm_apply_all();
k_mutex_unlock(&drive_lock);
}
}
K_THREAD_DEFINE(drive_tid, CONFIG_FOC_MOTOR_STACK_SIZE, drive_thread, NULL, NULL,
NULL, CONFIG_FOC_MOTOR_PRIORITY, 0, 0);
这里做了角度外推，theta = theta_sample + omega·(now − t_sample)。它只在速度估计准确时才有意义；速度有几个百分点的噪声，在 800 rpm 下折合一个样本周期约 1 电角度，代价可以接受。外推上限 5 ms，超出的样本属于卡住的采样线程，那时角度守卫本来就要停机。
性能：开启 FPU 后一次控制律 8.2 µs，一次完整更新 35.5 µs，其中 18.7 µs 花在写桥臂并等 PWM 重载上。10 kHz 下每 100 µs 用掉 35.5 µs。
五种手感
同一个电机、同一根轴，手感只是这几条曲线里的哪一条在生效：
五条定律
float knob_torque_ma(const struct knob_state *k, float pos_deg, float vel_rpm)
{
float iq = 0.0f;
switch (k->mode) {
case KNOB_FREE:
/* 连阻尼都没有：这个模式就是用来看轴自己的齿槽和轴承摩擦的。 */
break;
case KNOB_DAMPING:
/* 下面那段阻尼项就是全部定律。 */
break;
case KNOB_DETENT: {
/* 每个间隔一个正弦：齿槽处为零，两个齿槽之间四分之一处最大，
* 半程处回零——半程就是"齿槽为什么能被选中"的那道刀锋。
* 力在任何地方都没有台阶，所以传感器的分辨率没有东西可以
* 与之共振。
*
* 先折叠再算相位，因为 float 只有 24 位尾数：转过几圈的轴，
* 位置是几百度的量，先变相位再折叠会把齿槽本身量化掉。
*/
float st, ct;
sin_cos_deg(360.0f * fmodf(pos_deg, k->detent_deg) /
k->detent_deg, &st, &ct);
iq -= k->detent_ma * st;
break;
}
case KNOB_LIMIT:
/* 范围之内这个旋钮是自由的：没有弹簧、没有齿槽，只有阻尼。
* 范围之外，墙就是一根刚度说了算的弹簧。 */
if (pos_deg > k->limit_max) {
iq -= k->wall_ma_per_deg * (pos_deg - k->limit_max);
} else if (pos_deg < k->limit_min) {
iq -= k->wall_ma_per_deg * (pos_deg - k->limit_min);
}
break;
case KNOB_CENTER:
iq = -k->center_ma_per_deg * pos_deg;
/* 只有弹簧是回不到零的：静止时轴停在弹簧转矩与摩擦力相等的
* 地方，以这组参数算是好几度以外。一个朝着误差方向的常数项
* 就是用来消掉这个偏差的。 */
if (k->stiction_ma > 0.0f &&
fabsf(pos_deg) > KNOB_CENTER_BAND_DEG) {
iq += (pos_deg > 0.0f) ? -k->stiction_ma : k->stiction_ma;
}
break;
case KNOB_MODE_COUNT:
break;
}
然后是分阶段钳位
/*
* 除自由模式外，每个模式都叠加阻尼。它让弹簧不振荡，也让齿槽不会
* 被高速冲过。
*
* 三级各自钳位，顺序就是这段代码的全部意义。目标离轴很远的手感
* 会要求一个巨大的转矩——轴停在限位范围外 30 圈时要求 475000 mA
* ——如果先把所有项加起来再钳一次，那一次钳位会顺从模式的符号，
* 阻尼和限速就一起被丢掉了。这是实测过的：项相加时那个限位把轴
* 推到了 1623 rpm，而阻尼和限速都在要求刹车。
*/
if (k->mode != KNOB_FREE) {
iq = CLAMP(iq, -k->iq_max_ma, k->iq_max_ma);
iq = CLAMP(iq - k->damping_ma_per_rpm * vel_rpm, -k->iq_max_ma,
k->iq_max_ma);
/* 限速放在最后，而且超过之后净转矩只能与运动方向相反：
* 随着超出量增大，允许用来驱动的余量掉到零。它是一名调速器，
* 不是一个可以投票的项，所以要求一切的模式斗不过它。 */
float over = fabsf(vel_rpm) - (float)CONFIG_FOC_KNOB_MAX_RPM;
if (over > 0.0f) {
float allow = MAX(k->iq_max_ma -
MIN(over * KNOB_SPEED_BRAKE_MA_PER_RPM,
k->iq_max_ma), 0.0f);
iq = (vel_rpm > 0.0f) ? MIN(iq, allow) : MAX(iq, -allow);
}
}
return CLAMP(iq, -k->iq_max_ma, k->iq_max_ma);
}
速度估计：为什么不用编码器自带的
阻尼项要微分位置，而编码器一步是 0.088 度。1 ms 采样下，单次差分只能分辨 14.7 rpm——那是一个阶梯，不是一个速度。窗口 N 个样本把分辨率除以 N，代价是 N ms 的滞后：
#define KNOB_VEL_HIST 9
BUILD_ASSERT(CONFIG_FOC_KNOB_VEL_WINDOW < KNOB_VEL_HIST,
"the knob's speed window cannot reach past the history it keeps");
static void knob_push(struct knob_state *k, float pos_deg, int64_t us)
{
uint32_t span, newest, oldest;
int64_t dt_us;
/* 历史存的是编码器坐标系的绝对值，因为只有差值会被读出来。 */
k->hist_pos[k->hist_i] = pos_deg;
k->hist_us[k->hist_i] = us;
k->hist_i = (k->hist_i + 1U) % KNOB_VEL_HIST;
if (k->hist_n < KNOB_VEL_HIST) {
k->hist_n++;
}
/* 一个窗口要两个端点。开机后的头几个样本落在这里，于是一个模式
* 起步时速度是零，而不是历史另一端碰巧是什么。 */
span = MIN(k->win, k->hist_n - 1U);
if (span == 0U) {
k->vel_rpm = 0.0f;
return;
}
newest = (k->hist_i + KNOB_VEL_HIST - 1U) % KNOB_VEL_HIST;
oldest = (k->hist_i + KNOB_VEL_HIST - 1U - span) % KNOB_VEL_HIST;
dt_us = k->hist_us[newest] - k->hist_us[oldest];
if (dt_us <= 0) {
return;
}
k->vel_rpm = (k->hist_pos[newest] - k->hist_pos[oldest]) *
60000000.0f / (360.0f * (float)dt_us);
}
默认 FOC_KNOB_VEL_WINDOW=4，分辨率 3.7 rpm，滞后 4 ms。在 2 mA/rpm 的阻尼增益下，3.7 rpm 的噪声是 7 mA 的转矩——比手能感觉到的门槛低得多。窗口再大就换不来什么了。
每个采样周期算一次
int knob_step(float pos_deg, float rpm_enc, int64_t sample_us)
{
struct knob_state *k = &knob;
bool fresh = (sample_us != k->last_us);
float iq;
if (fresh) {
knob_push(k, pos_deg, sample_us);
k->last_us = sample_us;
/* 窗口还没有两个端点之前，编码器自己的滤波估计是唯一的
* 速度。这在模式起步时有用：带着速度进来的轴应当从第一次
* 更新就被阻尼，而不是等窗口需要的四次。 */
if (k->hist_n < 2U) {
k->vel_rpm = rpm_enc;
}
}
k->pos_deg = pos_deg - k->origin;
iq = knob_torque_ma(k, k->pos_deg, k->vel_rpm);
k->iq_ma = (int)(iq + (iq >= 0.0f ? 0.5f : -0.5f));
return k->iq_ma;
}
控制环以 10 kHz 调用它，而位置样本只有 1 kHz，定律是采样值的函数，样本之间保持不变是精确的，不需要跑得更快。
模式的启动与切换
int knob_mode_start(enum knob_mode mode)
{
bool have_sample;
int ret;
if (!encoder_active) {
LOG_ERR("the knob needs the encoder, and the boot scan found none");
return -ENODEV;
}
k_mutex_lock(&encoder_lock, K_FOREVER);
have_sample = (enc.samples > 0);
k_mutex_unlock(&encoder_lock);
if (!have_sample) {
LOG_ERR("the knob needs a sample from the encoder and has none");
return -ENODATA;
}
ret = drive_arm();
if (ret < 0) {
return ret;
}
k_mutex_lock(&drive_lock, K_FOREVER);
knob_start(mode);
/* 上一次运行留下的东西不能进第一次更新。 */
drive.iq_ma = 0;
k_mutex_unlock(&drive_lock);
drive_set_state(DRIVE_KNOB);
if (mode == KNOB_FREE) {
/* 自由模式要求的转矩是零，所以桥臂没有什么可给的。EN 保持
* 拉高时三相停在零矢量上，那是整个绕组同一个平均电位：对
* 一根被手转动的轴来说，这等于把反电动势短接，而短接就是
* 刹车。高阻是唯一让绕组开路的状态，所以自由就是这个意思。 */
ret = gpio_pin_set_dt(&driver_en, 0);
if (ret < 0) {
LOG_ERR("could not drive EN low: %d", ret);
return ret;
}
}
LOG_INF("knob mode %s", knob_mode_name(mode));
return 0;
}
int knob_mode_next(void)
{
enum knob_mode next;
k_mutex_lock(&drive_lock, K_FOREVER);
next = (enum knob_mode)(((unsigned int)knob.mode + 1U) %
(unsigned int)KNOB_MODE_COUNT);
k_mutex_unlock(&drive_lock);
return knob_mode_start(next);
}
knob_start() 把速度历史一起清掉——否则上次运行留下的样本会被当成速度，第一次阻尼项就是一个冲击。
给控制环装上超速守卫
手感能推着自己转，这是它和"一个转矩源"的区别。角度 1 ms 采一次、磁场按它放置，所以一个足够硬的齿槽会正好激励起自己的机械振荡（37 Hz 机械 = 260 Hz 电气，每周期只采到三次）。从控制环的角度看，症状是轴的转速一直待在几百 rpm 以上：那不是手，是模式在推自己。
守卫的判据因此不是"转速高"而是"转速高并且持续"：实测量到一次 120° 的回归在 1204 rpm 上待了几十毫秒，而真正失控的模式会一直待在上面，所以门槛取五分之一秒。
守卫用的是上面表里的两个阈值：FOC_KNOB_ABORT_RPM（1200 rpm，停机线）和 FOC_KNOB_MAX_RPM（600 rpm，刹车线）。然后把 drive_thread 留的那个位置填上：
/* 轴超过限速持续的更新次数。目标离轴很远的手感在回来路上会经过
* 这个速度（实测一次 120 度的回归到过 1204 rpm，持续几十毫秒），
* 而真正振荡的手感会一直待在上面，所以守卫要的是五分之一秒，
* 不是一个瞬间。 */
uint32_t over_speed = 0U;
const uint32_t over_speed_limit = MAX(1U, CONFIG_FOC_MOTOR_RATE_HZ / 5U);
/* 旋钮的限速守卫。手转旋钮是几十 rpm，猛拨几百；一直待在
* 这个速度以上的，是手感在推自己转——从控制环的角度看，
* 那就是一个不稳定的手感。 */
if (fabsf(rpm) > (float)CONFIG_FOC_KNOB_ABORT_RPM) {
if (++over_speed > over_speed_limit) {
drive_fault_stop();
LOG_ERR("knob stopped: %.0f rpm for %u ms, past "
"the %d rpm a hand can turn; a mode that "
"stays here is oscillating, not detecting "
"a hand", (double)rpm,
(unsigned int)(over_speed_limit * 1000U /
CONFIG_FOC_MOTOR_RATE_HZ),
CONFIG_FOC_KNOB_ABORT_RPM);
continue;
}
} else {
over_speed = 0U;
}
USB HID
用板上那颗非调试口的 USB1。MCXN236 的 USB1 带片上高速 PHY，板级 dtsi 已经把它作为 zephyr_udc0 打开，所以应用只需要加一个 HID 节点。overlay 的追加部分：
/ {
hid_dev_0: hid_dev_0 {
compatible = "zephyr,hid-device";
label = "mcxn236 knob";
protocol-code = "none";
in-report-size = <5>;
in-polling-period-us = <1000>;
};
};
in-report-size = <5> 取两个报告里最大的那个：报告 2 是"报告 ID + 按键 + X + Y + 滚轮"共 5 字节。1000 µs 是全速中断端点能跑的最快轮询周期。
一个接口、两个报告，两个都写在描述符里，所以主机不管 FOC_USB_FUNCTION 选什么都能枚举出音量控制和滚轮，这个选项只决定旋钮驱动哪一个。
设备与类注册
USBD_DEVICE_DEFINE(knob_usbd, DEVICE_DT_GET(DT_NODELABEL(zephyr_udc0)),
CONFIG_FOC_USB_VID, CONFIG_FOC_USB_PID);
USBD_DESC_LANG_DEFINE(knob_lang);
USBD_DESC_MANUFACTURER_DEFINE(knob_mfr, CONFIG_FOC_USB_MANUFACTURER);
USBD_DESC_PRODUCT_DEFINE(knob_product, CONFIG_FOC_USB_PRODUCT);
USBD_DESC_CONFIG_DEFINE(fs_cfg_desc, "FS Configuration");
USBD_DESC_CONFIG_DEFINE(hs_cfg_desc, "HS Configuration");
/* 自供电：电机有自己的 12 V 电源，板子没有从主机上取这份电流。 */
USBD_CONFIGURATION_DEFINE(knob_fs_config, USB_SCD_SELF_POWERED, 50,
&fs_cfg_desc);
USBD_CONFIGURATION_DEFINE(knob_hs_config, USB_SCD_SELF_POWERED, 50,
&hs_cfg_desc);
控制器是高速的，所以高速配置先起来，全速配置也一起注册（主机自己选，普通 USB 2.0 集线器只给全速）：
if (USBD_SUPPORTS_HIGH_SPEED &&
usbd_caps_speed(&knob_usbd) == USBD_SPEED_HS) {
ret = usbd_add_configuration(&knob_usbd, USBD_SPEED_HS,
&knob_hs_config);
ret |= usbd_register_all_classes(&knob_usbd, USBD_SPEED_HS, 1,
NULL);
ret |= usbd_device_set_code_triple(&knob_usbd, USBD_SPEED_HS, 0,
0, 0);
if (ret != 0) {
LOG_ERR("Could not set up the high-speed configuration: %d",
ret);
return ret;
}
}
ret = usbd_add_configuration(&knob_usbd, USBD_SPEED_FS, &knob_fs_config);
ret |= usbd_register_all_classes(&knob_usbd, USBD_SPEED_FS, 1, NULL);
ret |= usbd_device_set_code_triple(&knob_usbd, USBD_SPEED_FS, 0, 0, 0);
报告：一个位和一个刻度
音量是 consumer control 的位图，所以"一步"等于"置位，再清零"。这里有一个容易踩的点：新的一步到来时如果位还置着，必须先把上一次清零，否则主机会看到一个从未变化的状态，快速转动就变成了一次长按。
static void report_volume(int steps)
{
uint8_t rep[2] = { REPORT_ID_CONSUMER, 0 };
if (usb.pressed) {
(void)hid_device_submit_report(hid_dev, sizeof(rep), rep);
usb.pressed = false;
}
rep[1] = (steps > 0) ? CONSUMER_VOLUME_UP : CONSUMER_VOLUME_DOWN;
if (hid_device_submit_report(hid_dev, sizeof(rep), rep) == 0) {
usb.pressed = true;
usb.release_at = k_uptime_get() + CONFIG_FOC_USB_HOLD_MS;
}
}
static void report_wheel(int steps)
{
uint8_t rep[WHEEL_REPORT_SIZE] = { REPORT_ID_WHEEL, 0, 0, 0,
(uint8_t)(steps > 0 ? 1 : -1) };
(void)hid_device_submit_report(hid_dev, sizeof(rep), rep);
}
static void report_release(void)
{
uint8_t rep[2] = { REPORT_ID_CONSUMER, 0 };
if (usb.pressed && k_uptime_get() >= usb.release_at) {
if (hid_device_submit_report(hid_dev, sizeof(rep), rep) == 0) {
usb.pressed = false;
}
}
}
FOC_USB_HOLD_MS 默认 20 ms：位要保持足够长让主机看见，又不能长到让快速转动变成连击。
报告线程每醒来一次，它做四件事：
1、在 encoder_lock 里取一次机械角快照
2、顺便用 enc.samples 判断角度是否可用，不可用就把 have_last 清掉，避免下一次把整个绝对位置当成一步报出去）
3、把本次与上次的差值累加进 accum；用 FOC_USB_STEP_MDEG / 1000 度换算成整数步，余数留在 accum 里给下一轮，这样即使步长不是每周期行程的整数倍，长期速率也不会漂移
4、最后按当前功能发出音量位或滚轮刻度，并顺手调用 report_release() 在 FOC_USB_HOLD_MS（20 ms）之后把音量位清零。
static void usb_knob_thread(void *p1, void *p2, void *p3)
{
const float step = (float)CONFIG_FOC_USB_STEP_MDEG / 1000.0f;
float last = 0.0f;
float accum = 0.0f;
bool have_last = false;
int64_t next = k_uptime_get();
while (1) {
bool valid;
float pos;
k_mutex_lock(&encoder_lock, K_FOREVER);
pos = enc.mech;
valid = (enc.samples > 0);
k_mutex_unlock(&encoder_lock);
if (!valid) {
/* 还没有角度就没有行程可量。等有的时候重来，而不是
* 把整个位置当成一步报出去。 */
have_last = false;
} else {
int steps;
if (!have_last) {
last = pos;
accum = 0.0f;
have_last = true;
}
accum += pos - last;
steps = (int)(accum / step);
if (steps != 0) {
/* 余量留给下一次，一个步进发出去。快到无法
* 描述时是合并而不是排队：告诉主机旋钮转得
* 多快，而不是它的每一个度。 */
accum -= (float)steps * step;
steps = CLAMP(steps, -1, 1);
if (usb.ready && usb.function != USB_FUNC_OFF) {
report_step(steps);
}
}
if (usb.ready && usb.function != USB_FUNC_OFF) {
report_release();
}
last = pos;
}
next += CONFIG_FOC_USB_POLL_MS;
if (next <= k_uptime_get()) {
next = k_uptime_get();
} else {
k_msleep(next - k_uptime_get());
}
}
}
K_THREAD_DEFINE(usb_tid, CONFIG_FOC_USB_STACK_SIZE, usb_knob_thread, NULL, NULL,
NULL, CONFIG_FOC_USB_PRIORITY, 0, 0);
完成后，插入电脑时可以正常识别到设备：
两个按键
一个 input 设备同时带两个用户按键，所以靠事件码区分：
static void button_cb(struct input_event *evt, void *user_data)
{
ARG_UNUSED(user_data);
/* 一个设备带两个按键，事件码是区分它们的依据，松开不是动作。 */
if (evt->type != INPUT_EV_KEY || evt->value == 0) {
return;
}
switch (evt->code) {
case INPUT_KEY_WAKEUP: /* SW2，板上第二个标着名字的按键 */
usb_knob_toggle_function();
break;
case INPUT_KEY_0: /* SW3：轮换到下一种手感 */
(void)knob_mode_next();
break;
default:
break;
}
}
/* input 设备是按键节点所属的那个 gpio-keys 控制器，不是某个按键本身：
* 子节点描述的是引脚。 */
INPUT_CALLBACK_DEFINE(DEVICE_DT_GET(DT_PARENT(DT_NODELABEL(user_button_3))),
button_cb, NULL);
SW2 只在前两者（volume / wheel）之间切换；both 和 off 是构建时选的，按一下就离开它们。SW2 同时是板上唤醒与 MCUboot 按键，但本固件不进入低功耗模式，MCUboot 的用法发生在复位时，所以没有冲突。
演示
图片效果不太明显，建议看视频。
USB 音量，限位模式，在 -90 到 +90 度自由旋转，超出范围后阻力上升并且在松手后回到限位范围内：
USB 音量，回中模式：
USB 音量，齿槽模式，每 30 度一个槽位：
USB 音量，阻尼模式：
USB 音量，自由模式：
总结
本项目在 FRDM-MCXN236 上用 Zephyr 实现了一个 USB 力反馈旋钮，固件按 SimpleFOC 的 estimated current 模式驱动电机，通过这个项目理解了基本的 FOC 原理。
FRDM-MCXN236 有 150 MHz 的 M33 带 FPU 可以轻松地跑 10 kHz 的环路，剩下的时间足够留给编码器线程和 USB 线程。Zephyr 的驱动生态非常完善，几乎不需要写底层驱动，通过硬件定时器触发线程唤醒也可以稳定实现 10Khz 的控制频率。
fishlovescoding
EPTmachine
码匠许师傅