目录
master
分支1
标签0
+ 疑修
Web IDE
EDY
docs: 添加子模块说明并移除许可
29天前42次提交
README.md

STORA_Drive — PMSM FOC 双编码器伺服驱动器

基于 STM32G474 的永磁同步电机 (PMSM) 磁场定向控制 (FOC) 伺服驱动器，支持 位置 / 速度 / 转矩 三环级联控制，通过 CAN 总线 与上位机通信。

硬件规格

项目 参数
MCU STM32G474RBTx (Cortex-M4F, FPUv2, 170 MHz)
栅极驱动 DRV8301 (SPI 配置, 3 路半桥)
电机端编码器 MT6701 (I2C3, 0x06) — 14 bit 磁编码器
关节端编码器 MT6701 (I2C2, 0x06) — 双编码器架构
通信 FDCAN1 (CAN-FD 兼容)
调试串口 UART4 (DMA)
母线电压 24 V DC
减速比 8:1

电机参数 (GIM6010-8)

参数 值 单位
极对数 14 —
相电阻 0.44 Ω
相电感 0.23 mH
额定电流 10.5 A
堵转电流 23.4 A
额定转速 (关节端) 120 RPM
最大转速 (关节端) 420 RPM
额定转矩 5.0 Nm
堵转转矩 11.0 Nm
转矩常数 0.47 Nm/A

控制架构

┌─────────────┐     ┌─────────────┐     ┌──────────────┐
│  位置环      │ ──→ │  速度环      │ ──→ │  电流环 (ISR)  │ ──→ SVPWM → TIM1
│  ~3.33 kHz   │     │  ~3.33 kHz   │     │  20 kHz        │
│  (FreeRTOS)  │     │  (FreeRTOS)  │     │  (ADC 注入中断) │
└─────────────┘     └─────────────┘     └──────────────┘
       ↑                   ↑                    ↑
  位置 PID            速度 PID            d/q 轴 PI
  梯形轨迹            速度斜坡         零极点对消整定
  • 电流环: 20 kHz ISR (与 PWM 同频) — Clarke/Park 变换 → I_d/I_q PI → 逆 Park → SVPWM
  • 速度环: FreeRTOS 任务 (~3.33 kHz) — 编码器速度估计 → PID → I_q_ref
  • 位置环: FreeRTOS 任务 (~3.33 kHz) — 编码器位置 → PID + 梯形轨迹 → vel_ref

控制模式

模式 CAN 值 说明
禁用 (DISABLE) 0 PWM 关闭, 电机自由旋转
位置 (POSITION) 1 位置闭环, 含梯形轨迹规划
速度 (VELOCITY) 2 速度闭环, 可配速度斜坡
转矩 (TORQUE) 3 电流 (转矩) 闭环
开环 (OPEN_LOOP) — 固定电压强拖 (调试用)

输入模式 (setpoint 生成方式)

模式 说明
PASSTHROUGH 直通 — 外部设定值直接生效
TRAP_TRAJ 梯形轨迹 — 位置平滑加减速
VEL_RAMP 速度斜坡 — 速度/转矩以固定斜率逼近

项目结构

Motor_Driver/
├── Core/                        # CubeMX 生成 (HAL 初始化)
│   ├── Inc/                     # main.h, FreeRTOSConfig.h, stm32g4xx_it.h ...
│   └── Src/                     # main.c, app_freertos.c, tim.c, adc.c ...
├── Drivers/
│   ├── Config/                  # 编译期配置
│   │   ├── hardware_config.h    # 启动标定、PWM/ADC 时序
│   │   └── motor_params.h       # 电机参数 (R/L/极对数/限幅)
│   ├── Hardware/                # 硬件驱动 (唯一可调 HAL 的层)
│   │   ├── drv8301.c/h          # DRV8301 栅极驱动 (SPI)
│   │   ├── mt6701.c/h           # MT6701 磁编码器 (I2C)
│   │   ├── pwm.c/h              # PWM 启动/停止/ADC 同步
│   │   ├── encoder.c/h          # 编码器抽象层 (双编码器绑定)
│   │   ├── flash_storage.c/h    # Flash 参数持久化 (CRC 校验)
│   │   └── i2c_recovery.c/h     # I2C 总线锁死恢复
│   ├── FOC/                     # 纯数学算法 (零 HAL 依赖)
│   │   ├── foc.c/h              # Clarke/Park/逆 Park/SVPWM
│   │   ├── pid.c/h              # PID 控制器 (微分滤波 + 抗饱和)
│   │   ├── trap_traj.c/h        # 梯形轨迹规划
│   │   └── gear_solver.c/h      # 跨齿轮比多圈位置跟踪
│   ├── Axis/                    # 轴应用层 (控制核心)
│   │   ├── axis.c/h             # 顶层聚合 (STORA_Drive) + 初始化/模式切换
│   │   ├── motor.c/h            # 电机参数初始化
│   │   ├── current_loop.c/h     # 电流环 ISR (20 kHz)
│   │   ├── velocity_loop.c/h    # 速度环任务
│   │   ├── position_loop.c/h    # 位置环任务
│   │   ├── input_dispatch.c/h   # 输入调度 (PASSTHROUGH/TRAP_TRAJ/VEL_RAMP)
│   │   ├── axis_monitor.c/h     # 系统监控 (温度/电压/错误/看门狗)
│   │   ├── motor_protection.c/h # 电机保护 (过载/堵转检测)
│   │   └── utils.c/h            # 低通滤波器 (FILTER_FO)
│   └── Communication/           # CAN 协议
│       ├── can_comm.c/h         # FDCAN 自定义二进制协议
│       └── ota_trigger.c/h      # OTA 升级触发 (ENTER_BL → 写控制块 → 复位)
├── Bootloader/                  # 独立 Bootloader 工程 (OTA 升级)
│   ├── Inc/bootloader_config.h  # Flash 布局 / 协议 / 控制块共享定义
│   └── Src/                     # bootloader_main.c / flash.c / can.c
└── MDK-ARM/                     # Keil uVision 工程
    ├── Motor_Driver.uvprojx     # APP 工程 (重定位 0x08004000)
    └── Bootloader.uvprojx       # Bootloader 工程 (0x08000000)

层级调用规则: Axis → Communication / FOC / Hardware / Config → HAL (仅 Hardware 层可调 HAL)

CAN 协议

帧格式

基于 FDCAN1 的自定义二进制协议, 消息长度 3–8 字节不等 (取决于命令类型):

字节 含义
0 命令类型
1 寄存器地址 (单字节)
2–5 数据 (大端, int32) — 仅写/读响应的数据域

命令类型:

  • 0x00 — 网络管理 (心跳/同步)
  • 0x01 — 写 (需返回确认, DLC≥6)
  • 0x03 — 读 (需返回数据)
  • 0x05 — 快写 (无返回, 立即生效, DLC≥6)
  • 0x06–0x0A — Bootloader 升级命令 (ENTER/ERASE/DATA/CRC/RESET), 详见 docs/CAN_PROTOCOL.md §14

寄存器表

坐标约定: CAN 收发使用减速器末端 (关节端) 坐标; REDUCTION_RATIO (×8) 在内部自动转换

实时数据 (只读 R)

地址 名称 单位 说明
0x05 I_q 当前电流 mA q 轴实际电流
0x06 当前速度 脉冲/s 关节端速度
0x07 当前位置 脉冲 关节端位置
0x0D I_d 当前电流 mA d 轴实际电流
0x0E V_d 输出电压 mV d 轴电压参考值
0x12 V_q 输出电压 mV q 轴电压参考值
0x15 告警指示 位掩码 错误/警告标志
0x1B 母线电压 0.1V 电源电压
0x1D 当前温度 1°C PCB 板温度
0x62 d 轴电流 mA 同 0x0D

电机参数 (只读 R)

地址 名称 单位 说明
0x90 电机极对数 — ROTATOR_POLAR_PAIR
0x91 相电阻 mΩ 标定测量值
0x92 相电感 μH 标定测量值
0x93 电流控制带宽 rad/s ω_c = R/L

控制命令 (读写 R/W)

地址 名称 单位 说明
0x08 目标电流 mA I_q 设定值
0x09 目标速度 脉冲/s 关节端速度设定
0x0A 目标位置 脉冲 关节端位置设定
0x0F 工作模式 枚举 1=位置-速度, 2=位置-时间, 3=速度, 4=电流, 5=同步位置
0x10 使能/失能 0/1 1=进入闭环
0x11 停止运行 — 写 0x01 触发 (速度归零)
0x15 清除告警 0x00 写 0x00 清除所有错误
0x54 控制模式 枚举 0=禁用, 1=位置, 2=速度, 3=转矩
0x55 设置零点 — 捕获当前位置为零点 (标定)
0x56 保存零点 — 零点持久化到 Flash
0x57 输入模式 枚举 0=PASSTHROUGH, 1=TRAP_TRAJ, 2=VEL_RAMP
0x58 速度斜坡速率 0.1 (rad/s)/s VEL_RAMP 模式用
0x59 转矩斜坡速率 0.01 A/s VEL_RAMP 模式用

限幅参数 (读写 R/W)

地址 名称 单位 说明
0x30 最大电流 mA 电机电流限制
0x31 最大速度 脉冲/s 关节端速度限制
0x50 目标位置 脉冲 关节端位置设定 (带限幅)
0x51 速度限制 脉冲/s 关联位置环积分限幅
0x52 最大转矩 — 转矩限制
0x53 速度限制 脉冲/s 同 0x51

PID 增益 (读写 R/W)

地址 名称 说明
0x23 电流环 Kp d/q 轴共用
0x24 电流环 Ki d/q 轴共用
0x25 速度环 Kp —
0x26 速度环 Ki 联动更新 integral_limit
0x27 位置环 Kp —
0x28 位置环 Ki 联动更新 integral_limit
0x80 电流环 Kd —
0x82 速度环 Kd —
0x83 位置环 Kd —

系统配置 (读写 R/W)

地址 名称 说明
0x4B CAN 波特率 修改需谨慎
0x4C 设备 CAN ID 需 SN 后 3 字节认证
0x4D 保存到 Flash 写 0x006579F3 持久化 PID 参数
0x4E 修改主机 ID 需 SN 后 3 字节认证

启动序列

上电后按 hardware_config.h 中的启动标志依次执行任务链:

1. 电机参数标定 (R/L 测量, 可选)
2. 编码器电气零位标定 (编码器偏移)
3. 编码器方向检测
4. 开环强拖 (可选)
5. 进入闭环控制

标定结果 (编码器偏移、电机参数、PID 增益) 保存在片上 Flash (带 CRC32 校验), 下次上电自动加载。

保护功能

保护 触发条件 动作
过温保护 PCB 温度 > 85°C 错误停机
过压/欠压 母线 < 8V 或 > 30V 错误停机
过载检测 (3 级) 电流超阈值 × 时间窗口 逐级告警/停机
堵转检测 低速 + 持续高电流 错误停机
看门狗 未按时喂狗 紧急停机
I2C 锁死恢复 I2C 总线异常 自动复位总线

构建与烧录

环境

  • IDE: Keil MDK-ARM v5 (uVision)
  • 编译器: ARMCC V5.06 update 5 (build 528)
  • 工程文件: MDK-ARM/Motor_Driver.uvprojx (APP) + MDK-ARM/Bootloader.uvprojx (Bootloader)

Flash 布局

0x08000000 ─ 0x08003FFF  (16 KB,  Page 0–7)   Bootloader
0x08004000 ─ 0x0801EFFF  (108 KB, Page 8–61)  APP (应用固件)
0x0801F000 ─ 0x0801F7FF  (2 KB,   Page 62)    Bootloader 控制块
0x0801F800 ─ 0x0801FFFF  (2 KB,   Page 63)    参数存储区
  1. 编译 Bootloader 工程 MDK-ARM/Bootloader.uvprojx → 烧录到 0x08000000
  2. 编译 APP 工程 MDK-ARM/Motor_Driver.uvprojx → 烧录到 0x08004000
  3. 上电后 Bootloader 校验通过自动跳转 APP

之后即可通过上位机 Motor_Studio 走 CAN OTA 远程升级，无需再动 ST-Link。

步骤 (编译)

  1. 在 Keil IDE 中分别打开两个工程
  2. Build → Rebuild (F7)
  3. 确保 0 error, 0 warning
  4. Flash → Download (F8) 通过 ST-Link 烧录

内存占用

资源 大小
SRAM 128 KB (0x20000000–0x2001FFFF)
Bootloader 16 KB (0x08000000–0x08003FFF)
APP 108 KB (0x08004000–0x0801EFFF)
控制块 + 参数 4 KB (0x0801F000–0x0801FFFF)
FreeRTOS 堆 13 KB (heap_4)
任务栈 (每个) 256 words (1 KB)

RTOS 任务

任务 周期 优先级
速度控制 0.3 ms (3.33 kHz) High
位置控制 0.3 ms (3.33 kHz) AboveNormal
CAN 通信 5 ms (200 Hz) Normal
系统监控 100 ms (10 Hz) BelowNormal

PWM / 控制环时序

参数 值 说明
PWM 频率 20 kHz TIM1, 中央对齐模式 3, ARR=8499
死区时间 ~177 ns TIM_1_8_DEADTIME_CLOCKS=30 @ 170 MHz
电流环 20 kHz ADC 注入中断, CURR_LOOP_PERIOD_US≈50 μs
速度/位置环 ~3.33 kHz POS_AND_SPD_LOOP_PERIOD_US=300 μs
调制方式 SVPWM 幅值不变型 Clarke/Park 变换

编码规范摘要

参考 CLAUDE.md 获取完整规范。关键约定:

  • 命名: ModuleName_FunctionName() (公开), snake_case() (私有)
  • 类型: PascalCase_t (结构体), PascalCase (枚举)
  • 层级: Hardware 是唯一可调 HAL 的层; FOC 是纯数学层
  • Core/ 目录: 仅含 CubeMX 生成代码 + 胶水代码, 不放手写逻辑
  • Git: <type>: <中文描述>, 编译验证后立即提交

子模块说明

本仓库是总工程 Motor_Project 的一个 git submodule（目录 Motor_Driver/）。总工程只记录指向本仓库某个 commit 的指针，修改后需分两步提交，否则总工程不会指向新版本：

# 第一步：本仓库内提交并推送
git checkout master            # 避免在 detached HEAD 上提交
git add .
git commit -m "feat: xxx"
git push origin master

# 第二步：回总工程更新指针并提交
cd ..
git add Motor_Driver           # 更新总工程记录的子模块 commit
git commit -m "chore: 更新 Motor_Driver 子模块"
git push origin master

⚠️ 只提交本仓库而不更新总工程指针，他人克隆总工程得到的仍是旧版本代码。

关于
README.md
118.0 MB
邀请码
    Gitlink（确实开源）
  • 加入我们
  • 官网邮箱：gitlink@ccf.org.cn
    • QQ群
  • QQ群
    • 公众号
  • 公众号

版权所有：中国计算机学会技术支持：开源发展技术委员会
京ICP备13000930号-9 京公网安备 11010802047560号