【easy_mc】一个简单的电机FOC控制系统

flowchart RL
    subgraph A[HW Impl]
    a1[PWM Module]
    a2[ADC-High Speed Module]
    a3[ADC-Low Speed Module]
    a4[Encoder Module]
    a5[Debug Module]
    end
    subgraph B[FOC]
    b1[SVPWM]
    b2[FOC Control]
    b3[High Task]
    b4[Low Task]
    b3-->b2
    b2-->b1
    end
    
    B --> |HW Control
    easy_mc_hw_start
    easy_mc_hw_stop
    easy_mc_hw_init
    easy_mc_hw_deinit|A
    
    B --> |PWM Control
    easy_mc_hw_set_u_v_w_count|A
    
    B --> |Debug
    easy_mc_hw_vofa_debug_out
    easy_mc_hw_debug_io_control|A
    
    A --> |ABZ Encoder
    Angle|B
    A --> |High Task - Isr
    easy_mc_isr_task_high_frequency
    Ia,Ib,Ic|B
    A --> |Low Task - Isr
    easy_mc_isr_task_low_frequency
    Vbus,Ibus|B

HW Impl

在不同硬件实现时，需要实现如下接口。

HW Control

主要用于控制所有硬件模块初始化和开关。包括：

easy_mc_hw_start，启动PWM输出。

easy_mc_hw_stop，关闭PWM输出。

easy_mc_hw_init，完成硬件模块时钟使能和初始化。

easy_mc_hw_deinit，关闭硬件模块时钟。

PWM Control

用于控制PWM的输出占空比。包括：

easy_mc_hw_set_u_v_w_count，控制UVW的输出占空比。

Debug

用于满足调试需要，包括VOFA时序输出和IO Debug信号输出。包括：

easy_mc_hw_vofa_debug_out，需要用DMA实现，FOC会在每次Low Task中Trigger一次，在中断中，波特率要求1ms内发送完毕。

easy_mc_hw_debug_io_control，控制io翻转，用于调试时序信号。

ABZ Encoder

暂时只实现了ABZ传感器，用于角度采集和z信号输入。包括：

easy_mc_hw_encoder_get_count，FOC算法调用，用于获取当前Encoder的计数值。

easy_mc_hw_encoder_set_zero，FOC算法调用，用于重置Encoder的计数值。

easy_mc_isr_set_zero，硬件触发调用FOC算法，用于通知FOC算法来了Z信号。

High Task

在每次Ia、Ib和Ic采样结束后触发，之后FOC根据当前的电流和角度信息，控制PWM输出，进而达到电机控制的目的。包括：

easy_mc_isr_task_high_frequency，FOC核心业务，后续展开。

Low Task

在每1ms需要触发1次，之后FOC会计算速度、调试输出、Vbus和Ibus采样这些行为。包括：

easy_mc_isr_task_low_frequency，FOC核心业务，后续展开。

easy_mc_hw_1ms_task，硬件需要实现，用于处理Vbus和Ibus采样等行为。

FOC-Low Task

1ms触发1次，主要完成以下任务，做一些耗时但是不紧急的业务。

需要注意，考虑到用户一般操作是在轮训之类场景做，为避免中断异步的问题，需要将用户操作接口和实际控制分离，用户页面不允许直接控制FOC，而是要通过中间接口，所有用户控制操作都需要在easy_mc_user_handle_command完成。

graph TD
A[FOC用户控制处理
easy_mc_user_handle_command]
B[Vbus,Ibus采样
easy_mc_hw_1ms_task]
C[Vofa输出
easy_mc_vofa_polling_send_data]
A-->B
B-->C

FOC-High Task

真正进行FOC控制的地方，按照PWM的周期运行。

graph TD
A[Encoder更新，包括speed,position计算
easy_mc_abz_encoder_update]
B[FOC用户控制处理
easy_mc_user_handle_command]
C[FOC状态机
easy_mc_high_freq_task_loop]
A-->B
B-->C

FOC状态机

FOC有以下状态，

MOTOR_STATE_INIT：初始化状态，刚上电在这个状态，什么都不做。

MOTOR_STATE_ADC_INIT：从初始化状态切换到工作状态时，必须走这个状态，进行ADC偏置计算。

MOTOR_STATE_ELECTRIC_ZERO_INIT：进行电角度定位。

MOTOR_STATE_MECHANICAL_ZERO_INIT：进行机械角度定位，将Z点作为position的0点。

MOTOR_STATE_ENABLE：电机运行状态，可以接受用户命令。

MOTOR_STATE_DISABLE：电机停止状态，可以接受用户命令。

typedef enum
{
    MOTOR_STATE_INIT = 0,
    MOTOR_STATE_ADC_INIT = 1,
    MOTOR_STATE_ELECTRIC_ZERO_INIT = 2,
    MOTOR_STATE_MECHANICAL_ZERO_INIT = 3,
    MOTOR_STATE_ENABLE = 4,
    MOTOR_STATE_DISABLE = 5,
} easy_mc_state_t;

状态机切换如下图所示。

部分对位置不敏感的项目，可以跳过MOTOR_STATE_MECHANICAL_ZERO_INIT状态，MOTOR_STATE_ENABLE和MOTOR_STATE_DISABLE可以切换。

MOTOR_STATE_ADC_INIT

在关闭电机PWM输出的情况下，计算ADC偏置，然后读取EASY_MC_ADC_OFFSET_CALC_MAX_CNT次而后取平均值。

MOTOR_STATE_ELECTRIC_ZERO_INIT

对于FOC控制而言，电角度非常关键，因为其是基于角度闭环控制的，如果电角度实际相位偏差，当控制Iq电流时，其实一部分能量作用到Id上，导致能量损失。尤其当电角度相位偏差为90°时，会导致只控制Iq并不会输出任何电流。

本项目基于FOC原理，在刚上电时，会用Uq开环控制，Uq配置为0，将Ud陆续增加，一段时间后，这时电机位置一定是在其电角度相位0点的位置，然后对encoder编码器进行归零操作，这时候采样角度和电角度就都为0了。

而后陆续降低Ud为0，以避免快速切换状态导致的电机抖动。

EASY_MC_CONFIG_ZERO_ALIGN_MAX_VOLTAGE，参数用于配置归零操作时的最大电压值。部分电机齿槽效益比较大，需要较大的电压来拉住电机。

EASY_MC_CONFIG_ENCODER_ZERO_ALIGN_TIMEOUT_MS，参数用于配置归零操作时的工作时间。根据需要调整。

MOTOR_STATE_MECHANICAL_ZERO_INIT

大多数业务场景下，需要一个准确的位置信息，通常是通过Z信号来定位的。

EASY_MC_CONFIG_ENCODER_ZERO_ALIGN_SPEED_RPM，配置机械角度定位时的运行速度，这时是用速度闭环控制。

EASY_MC_CONFIG_ENCODER_ZERO_ALIGN_SPEED_TIMEOUT_MS，配置机械角度定位时的超时时间。

FOC控制

最终控制电机的处理都在这里。网上有很多PID控制的图片了，这里就不在重新写了，这里重点描述控制流程。

graph TD
subgraph A[角度计算]
    a1[OPEN_INC]
    a2[ENCODER_ABZ]
    a3[FIX]
    end
B[电流计算]

subgraph C[FOC控制]
    subgraph CA[UQ]
        ca1[Park逆变化]
        ca2[SVPWM计算]
        ca3[PWM输出]
        end
    subgraph CB[IQ]
        cb1[Clark变化]
        cb2[Park变化]
        cb3[Id/Iq经PID输出Ud/Uq]
        end
    subgraph CC[SPEED]
        cc1[Id=0]
        cc2[Iq=速度差的PID输出]
        end
    subgraph CD[POSITION]
        cd1[Id=0]
        cd2[Iq=位置差的PID输出]
        end
    subgraph CE[IQ_WITH_POSITION_LIMIT]
        ce1[Positon溢出时，调整Iq=0]
        end
    end

A-->B
B-->C


CC-->|Id/Iq|CB
CD-->|Id/Iq|CB
CE-->|Id/Iq|CB
CB-->|Ud/Uq|CA

角度模式

本项目考虑各种场景，对角度来源定义了3种模式，分别是：

MOTOR_THETA_MODE_OPEN_INC：角度开环控制，按照motor_control.theta_inc角度提升，一般不怎么用，控制效果不怎么好。

MOTOR_THETA_MODE_ENCODER_ABZ：用ABZ编码器作为角度输入，开始运行后基本都会工作在这个模式下。

MOTOR_THETA_MODE_FIX：固定角度模式，在MOTOR_STATE_ELECTRIC_ZERO_INIT是就是用这个模式。

typedef enum
{
    MOTOR_THETA_MODE_OPEN_INC = 0,
    MOTOR_THETA_MODE_ENCODER_ABZ = 1,
    MOTOR_THETA_MODE_FIX = 2,
} easy_mc_theta_mode_t;

控制模式

就是FOC的那些控制模式，然后加入了一些特殊的控制模式。

MOTOR_CONTROL_MODE_UQ：Uq的控制，不去计算电压电流，直接根据角度控制PWM输出，在MOTOR_STATE_ELECTRIC_ZERO_INIT用的就是这个模式，也可以用于验证电角度是否归零，配置Ud为有效值，手动旋转电机，看电机是否会动，不动基本证明电角度设置正确。

MOTOR_CONTROL_MODE_IQ：电流闭环控制，也就是力矩控制模式，很多资料讲了，就是根据采样的Id和Iq于Id_ref和Iq_ref进行对比，经过PID调节后，转换为Ud，Uq。再进行MOTOR_CONTROL_MODE_UQ控制。

MOTOR_CONTROL_MODE_SPEED：速度闭环控制，根据采样的Speed于Speed_ref的差值，经过PID调节后，转换为Iq_ref，而后进行MOTOR_CONTROL_MODE_IQ控制。

MOTOR_CONTROL_MODE_POSITION：位置闭环控制，根据采样的Position于Position_ref的差值，经过PID调节后，转换为Iq_ref，而后进行MOTOR_CONTROL_MODE_IQ控制。

MOTOR_CONTROL_MODE_IQ_WITH_POSITION_LIMIT：在电流闭环控制的基础上，就Position限制，在特定对行程敏感的业务场景下，快速关闭电机输出。本质还是MOTOR_CONTROL_MODE_IQ控制。

typedef enum
{
    MOTOR_CONTROL_MODE_UQ = 0,
    MOTOR_CONTROL_MODE_IQ = 1,
    MOTOR_CONTROL_MODE_SPEED = 2,
    MOTOR_CONTROL_MODE_POSITION = 3,
    MOTOR_CONTROL_MODE_IQ_WITH_POSITION_LIMIT = 4,
} easy_mc_control_mode_t;

配置参数

默认参数在easy_mc_config_default.h中定义了默认值。

用户需要根据自己的板子实现app_easy_mc_config.h配置文件，里面根据具体板子情况，调整配置参数。

Porting说明

直接参考历程去实现即可。

euler_stm32g431：是欧拉电子的个人空间-欧拉电子个人主页-哔哩哔哩视频的板子+一个工业电机米格伺服电机110ST-M04030 05025 SJT-M040D凯恩帝华大广数包邮-淘宝网。

boot_stm32g474re：是另外一款电机。

待实现功能

抗锯齿算法

Odriver里面的算法简单直接，但是进行校准需要6min（Odriver文档写的），尝试实现了一下，在低转速下效果还可以，但是没太多实际意义，后续还是考虑要用别的更好的办法来做。

防齿 — ODrive 文档 0.6.11 文档

解析odrive抗齿槽算法讲解-CSDN博客

ODrive隐藏功能——转矩脉动抑制（抗齿槽算法）_哔哩哔哩_bilibili

下面是本地尝试实现后的低转速的效果，看起来还可以。但是实际用Iq控制时，体验不是那么丝滑。

实现注意事项

PWM载波频率选择

一个比较好的文章是这个：电机控制中载波频率大小的确定 - 知乎

低速电机建议用>20kHz，以减少电磁噪声。

电流环PI参数选择

可以参考（十） FOC的PI参数调节_哔哩哔哩_bilibili，告别经验调参–电流环PI参数自动整定 - 知乎，lab05a:FAST电流环pid参数自整定方法的探讨 - C2000™︎ 微控制器论坛 - C2000 微控制器 - E2E™ 设计支持，ST MCSDK FOC电流环PI参数整定 - 大大通(简体站)以及永磁同步电机（PMSM）磁场定向控制（FOC）电流环PI调节器参数整定_foc电流环pi参数整定过程-CSDN博客

有很多文章在讲，实际在使用的时候，遇到一个问题，就是这个参数太高了，这里需要乘以一个系数，也就是配置电流除以输出电压，不然电机会比较暴躁。（告别经验调参–电流环PI参数自动整定 - 知乎）

由于本项目是并联形PID，所以参数值计算公式如下：

#define EASY_MC_CURRENT_PI_BANDWIDTH (EASY_MC_CONFIG_PWM_FREQUENCY / 4)
#define EASY_MC_CURRENT_PI_COEX (EASY_MC_CONFIG_CURRENT_MAX / EASY_MC_CONFIG_VBUS_VOLTAGE)

#define EASY_MC_CONFIG_PID_CURRENT_KP (EASY_MC_CONFIG_LS * EASY_MC_CURRENT_PI_BANDWIDTH * EASY_MC_CURRENT_PI_COEX)
#define EASY_MC_CONFIG_PID_CURRENT_KI (EASY_MC_CONFIG_RS * EASY_MC_CURRENT_PI_BANDWIDTH * EASY_MC_CURRENT_PI_COEX)

注意，也不是一定要用这个公式计算，实际还是得调，EASY_MC_CURRENT_PI_COEX有时候配置直接是1/ EASY_MC_CONFIG_VBUS_VOLTAGE，电机更安静一些，具体工程上按需调整。

ADC采样注意

ADC采样时刻注意事项

FOC 电流采样方案对比（单电阻/双电阻/三电阻） - 知乎，写的很清楚了，根据具体的采样方案调整。

ADC采样滤波器注意事项

【电机控制】BLDC有感FOC控制——ADC电流采样_bldc电流采样-CSDN博客，提到一个工程问题，ADC电流不对劲，是工程中滤波器出问题了。可以用这个网站计算滤波器频率，RC滤波器截止频率在线计算器，

ADC采样周期注意事项

主要是各种节省成本方案的考量，可以看看这个文章。【电机控制】PMSM无感FOC控制（五）相电流检测及重构 — 单电阻采样-CSDN博客

ADC采样工程波形分析

看到各种ADC波形后的处理，FOC的ADC采样电流不平滑_foc电流采样波形-CSDN博客

ADC采样时钟配置

STM32G474配置

ADC的时钟不是越高越好，要看spec，要求最快速度为4Msps，那Msps单位是什么呢，继续往下看。

a313a5e6-7694-4cc8-b02f-2a38225a5998

Msps的单位就是 fADC / (sampling time [cycles] + resolution [bits] + 0.5)，fADC最快是60MHz，sampling time [cycles]最小是2.5，resolution [bits]用12bit的话，那就是60 / (2.5 + 12 + 0.5) = 4Msps。

需要注意，fADC的取值范围还要根据具体电压来。

a5772817-42c2-4329-bf80-9248297a3289

测试环境搭建

为了进一步分析FOC行为，基于STM32G4芯片搭建测试环境，工程用ARM的GCC编译。

串口日志输出，波特率115200，打印一些状态信息。

VOFA日志输出，波特率2000000，用DMA TX，用于打印高频业务。

IO翻转调试，提供两个IO口用于分析时序问题。

三电阻采样电路，有三路ADC采集A/B/C的电流。

母线电阻采样电路，用于采集母线电流，不用于电机控制，主要用于电路分析。

母线电压采样电路，用于采集母线电压，不用于电机控制，主要用于电路分析。

测试例程

SVPWM测试

最主要的测试就是SVPWM测试了，分为了几个子测试，通过串口日志看状态、VOFA看波形以及IO翻转看时序。

其中foc_svpwm_test测试用串口输出了1个周期的Tcm输出。foc_svpwm_wave_test通过VOFA循环输出变化波形，看有没异常点。

void foc_test(void)
{
    foc_svpwm_test();

    foc_svpwm_wave_test();
}

void app_main(void)
{
    foc_test();
}

VOFA分析

通过foc_svpwm_wave_test一直工作，看VOFA波形，确保u_alpha和u_beta的峰峰值为1/根号3。马鞍波的峰值为0.5。

IO时序分析

通过foc_svpwm_wave_test一直工作，看IO0和IO1的翻转，可以看到一次SVPWM用时1.2us，SIN/COS以及PACK逆变换用时2us。

PWM测试

主要测试PWM输出是否正常，并且测试ADC触发是否在3个MOS的Low侧中间打开，避免ADC采样时间点异常。

初始化完硬件后，将UVW的PWM输出调整一下占空比。确保有波形输出，电机需断开。同时注册easy_mc_isr_task_high_frequency_user_handle回调，配置EASY_MC_CONFIG_DEBUG_CUSTOM_IRQ_HANDLE宏，使用用户自定义high接口（避免foc整个业务运行）。

void easy_mc_isr_task_high_frequency_user_handle(void)
{
    easy_mc_hw_debug_io_control(0, 1);
    
    easy_mc_hw_debug_io_control(0, 0);
}

void app_main(void)
{
    uint32_t test_unit = EASY_MC_PWM_PERIOD_CYCLES / 10;
    easy_mc_hw_init();

    easy_mc_hw_start();

    easy_mc_hw_set_u_v_w_count(test_unit, test_unit * 2, test_unit * 3);
}