R30 手指关节 FOC 电机驱动器

基于 STM32G431 + DRV8316 的微型手指关节伺服驱动器，采用磁场定向控制 (FOC) 实现高精度位置/速度/转矩三环控制。

硬件平台

组件	型号	备注
MCU	STM32G431CB (Cortex-M4, 170MHz)	内置 CORDIC、OPAMP
电机驱动	DRV8316	三相栅极驱动器, SPI 配置
电机端编码器	MT6701 (I2C2, 0x06)	磁编码器, 14-bit 分辨率
关节端编码器	KTH5701 (I2C1, 0x06)	双编码器模式下使用
供电	12V DC	母线电压

目录结构

R30_MotorDriver/
├── Core/                          # HAL 外设初始化 (CubeMX 生成)
│   ├── Inc/                       # main.h, tim.h, adc.h, i2c.h, gpio.h ...
│   └── Src/                       # main.c, app_freertos.c, stm32g4xx_it.c ...
├── Drivers/
│   ├── Config/                    # 硬件与电机配置
│   │   ├── config.h               # 编码器数量、启动模式、ADC 通道映射
│   │   └── Pmsm_Ctrl.h            # 多电机型号参数表 (MOTOR_TYPE 切换)
│   ├── MotorControl/              # FOC 核心控制模块 ★
│   │   ├── foc.c/h                # FOC 核心: Clarke/Park/SVPWM/电流环 PI
│   │   ├── task_manager.c/h       # RTOS 任务调度: 电流 ISR + 速度/位置任务
│   │   ├── finger_joint_app.c/h   # 应用层: 启动序列、任务链、错误处理
│   │   ├── system_monitor.c/h     # 系统监控: 温度/电压/电流/堵转/看门狗
│   │   ├── encoder.c/h            # 编码器抽象层
│   │   ├── mt6701.c/h             # MT6701 驱动
│   │   ├── pid.c/h                # PID 控制器
│   │   ├── trap_traj.c/h          # 梯形/S曲线轨迹规划
│   │   ├── S7RTT.c/h              # 7段S曲线实时轨迹规划器
│   │   ├── flash.c/h              # 参数 Flash 持久化
│   │   ├── packet_handler.c/h     # 数据包收发
│   │   ├── servo_protocol.c/h     # 伺服通信协议解析
│   │   ├── axis.c/h               # 轴状态错误管理
│   │   ├── i2c_recovery.c/h       # I2C 总线故障恢复
│   │   ├── motor_protection.c/h   # 电机保护 (堵转/过流)
│   │   ├── utils.c/h              # 工具函数
│   │   ├── pll_filter.c/h         # PLL 速度滤波器
│   │   └── dob.c/h                # 扰动观测器
│   ├── FieldOrientedControl/      # 编码器旧代码 (Encoder.c/h)
│   ├── BSP/KTH5701/               # KTH5701 磁编码器驱动
│   ├── FirmwareDriver/            # (预留)
│   └── STM32G4xx_HAL_Driver/      # STM32 HAL 库
├── MDK-ARM/                       # Keil MDK 工程文件
│   ├── software.uvprojx           # 工程文件
│   └── startup_stm32g431xx.s      # 启动文件
├── Middlewares/                   # FreeRTOS + CMSIS
└── software.ioc                   # CubeMX 配置文件

控制架构

三环级联控制

目标位置 ─→ [位置环 PID] ─→ [速度环 PID] ─→ [电流环 FOC] ─→ SVPWM ─→ 电机
   ↑                                    ↑              ↑
   │ 1kHz (RTOS 任务)                   │ 1kHz         │ 20kHz (ADC ISR)
   │                                    │              │
   └── KTH5701 关节编码器 ──────────────┘              │
        (双编码器时)                     MT6701 ───────┘
                                         电机编码器

FreeRTOS 任务分布

任务	频率	优先级	职责
ADC 注入中断 (ISR)	20kHz	最高	FOC 电流环: 采样→Clarke→Park→PI→SVPWM
速度控制任务	1kHz	High	速度环 PID + 轨迹规划, 输出 I_q_ref 到电流环
位置控制任务	1kHz	AboveNormal	位置环 PID, 输出速度指令到速度环
系统监控任务	100Hz	BelowNormal	温度/电压/电流监控, I2C 恢复, DRV8316 故障检测
defaultTask	-	Normal	启动初始化 + 主循环 (喂狗/故障处理/系统复位)

ISR 状态分发 (ODrive 风格)

ADC 注入中断 (20kHz) 根据 motor_foc.state 分发控制逻辑：

状态	控制函数	说明
LOCKIN_SPIN	FOC_OpenLoopVoltageControl	开环电压强拖, θ_e 由速度积分
CLOSED_LOOP_CONTROL	FOC_CurrentLoopControl	闭环 PI 电流控制
MOTOR_CALIBRATION + calib_mode=0	FOC_OpenLoopVoltageControl	固定电压转子对齐
MOTOR_CALIBRATION + calib_mode=1	FOC_CurrentLoopControl	电阻测量 (PI 电流闭环)
MOTOR_CALIBRATION + calib_mode=2	FOC_OpenLoopVoltageControl	电感测量 (交替方波)

状态机与启动流程

参考 ODrive 的 AXIS_STATE_* 状态机, 使用任务链模式串行执行启动标定:

上电 → STARTUP_SEQUENCE
         │
         ├─ ① MOTOR_CALIBRATION      (R/L 测量, 仅当 !pre_calibrated)
         ├─ ② ENCODER_ANGLE_CALIB     (电气零位标定)
         ├─ ③ ENCODER_DIR_FIND        (编码器方向检测)
         ├─ ④ HOMING                  (KTH5701 关节回零, 仅双编码器)
         ├─ ⑤ ENCODER_OFFSET_CALIBRATION (关节位置同步, 每次上电)
         │
         └─ ⑥ 运行模式 (三选一)
              ├─ LOCKIN_SPIN           (开环强拖, 调试用)
              ├─ DEBUG_CLOSED_LOOP     (三环调试模式)
              └─ CLOSED_LOOP_CONTROL   (正常闭环运行) ★

• 任一步骤失败 → AXIS_STATE_IDLE + Motor_Disable()
• calib_mode 控制 ISR 在执行哪个标定子模式 (0/1/2)
• R/L 测量完成后自动计算电流环带宽 (FOC_AutoTuneBandwidth): ω_c = R/L, 受 Nyquist 约束 (≤ 2π·f_pwm/10)
• 标定参数 (R/L/带宽/PI增益/current_limit) 通过 save_para() 持久化到 Flash, 下次上电跳过标定

电机型号配置

通过 Pmsm_Ctrl.h 中的 MOTOR_TYPE 宏切换整机参数:

MOTOR_TYPE	电机	R (Ω)	L (mH)	额定电流 (A)	减速比
0	短电机	1.35	0.134	0.70	119.16
1	长电机	2.33	0.227	0.65	406.25
20	3.0mm	1.97	0.341	1.30	130.0
21	4.4mm	2.61	0.529	0.62	569.24
22 (当前)	7.4mm	1.51	0.211	1.59	569.24

关键宏：

• MOTOR_TYPE 22 — 当前激活的电机型号
• SELF_DEBUG 0 — 设为 1 时跳过 Flash 读取, 使用代码中硬编码的 PID 参数
• ENCODER_COUNT 2 — 1=仅 MT6701, 2=双编码器 (MT6701 + KTH5701)

配置启动行为 (config.h):

STARTUP_MOTOR_CALIBRATION           1   // R/L 自动测量
STARTUP_ENCODER_OFFSET_CALIBRATION  1   // 编码器标定
STARTUP_HOMING                      1   // 关节回零 (双编码器)
STARTUP_CLOSED_LOOP_CONTROL         1   // 启动后进入闭环

通信协议

• 物理层: USART, 1Mbps 波特率
• 数据包格式: 自定义二进制协议 (packet_handler)
• 通信模式: 支持多设备 ID 寻址 (FLASH 存储 DEV_ID)
• 伺服协议: 位置/速度/转矩指令 + 参数读写 (servo_protocol)

关键参数

参数	值	说明
PWM 频率	20kHz	TIM1/TIM8 中心对齐
电流环频率	20kHz	ADC 注入中断, 上溢触发
位置/速度环频率	1kHz	FreeRTOS 周期任务
极对数	6	ROTATOR_POLAR_PAIR
母线电压	12V	SUPPLY_VOLTAGE
开环速度	600 rpm (62.8 rad/s)	OPEN_LOOP_SPEED
电流环带宽	自动	τ_e = L/R 极点频率, Flash 持久化
IWDG 超时	FreeRTOS Idle Hook 喂狗	vApplicationIdleHook

电流保护

两级保护机制确保电机安全运行：

保护层	机制	响应速度	说明
硬钳位	FOC_Current_Limit() 直接 clamp I_d/I_q	即时 (每 ISR)	电流绝对值不超过 motor_foc.motor.current_limit
I²t 热保护	EMA 滤波 (τ=50s) 累积发热量	缓慢衰减	等效发热电流超过额定值时降额输出, 堵转时防止绕组过热

// 硬钳位: Id/Iq 绝对值不能超过 current_limit
Id = clamp(Id, -ilim, ilim);
Iq = clamp(Iq, -ilim, ilim);
// I²t 热保护: 等效发热电流超过 nominal_current 时降额
if (thermal_current_effective > thermal_state.nominal_current) { ... }

编译与调试

1. IDE: Keil MDK-ARM, 打开 MDK-ARM/software.uvprojx
2. 配置工具: CubeMX, 打开 software.ioc 修改外设配置
3. 调试接口: SWD (ST-LINK)
4. 串口输出: USART, 1Mbps, UART_PRINTF 宏用于 printf 调试
5. 看门狗: IWDG 使能, Idle Hook 中喂狗; 启动失败或死锁时自动复位

代码风格参考

• FOC 核心代码对齐 ODrive 命名与结构
• 状态命名: AXIS_STATE_* (ODrive 风格)
• 结构体命名: MotorParams, ControlTarget, MotorFeedback (ODrive 风格)
• 任务链模式: TaskChain_LoadCalibration → TaskChain_Run → TaskChain_ExecuteStep