无刷电机小车开发记录—移植SimpleFOC流程

前情提要

今天过来继续撸我的无刷电机小车。驱动无刷电机底层需要实现三大部分：功率驱动，位置反馈以及电流反馈。前面我已经适配了功率驱动（6线互补PWM），和位置反馈（PWM接口）的底层驱动代码。

而电流反馈我电路使用的是线内探测方案，输出模拟电压值。驱动程序可暂时使用RTT的ADC底层驱动。

相关硬件电路

功率驱动电路和位置反馈的磁编码器芯片，前面文章给出过简介。这里我给出最后一块电路检测相关的电路：

这里我只检测了A，B两相的相电流，第三项电流可由电流和为零（基尔霍夫第一定律）计算得到。

其中R45和R49为线内采样电阻，压差经过LT199G1运放放大后输出。LT199G1的放大倍数是50，由于后面MCU的ADC只能检测0~3.3V的正电压信号，所以这里加了个1.5V偏置。

可检测最大电流I=1.5V/50/0.01R=3A。对于我的这个小电机来说足够了，甚至后面可能还需要根据测试结果适当增大采样电阻阻值。

移植SimpleFOC

万事俱备只欠东风，到了驱动无刷电机最关键的时刻了，就是移植FOC算法。目前网络上可以看到的FOC算法很多，虽然我也没有过多的接触，但感觉核心算法基本一致，无非就是外围做了一些优化，变形。所以我这里选择了比较简单的SimpleFOC进行移植。后面可以根据自己的测试自行优化。

FOC的原理我这里暂时不提及，网络上可以找到包括稚晖君在内的很多大佬的科普文章，也肯定比我讲解的好，可自行查阅。如果有需要，我这里后面再另外补充一篇算法讲解篇。这里只简单提及移植过程。

下面给出SimpleFOC的官网链接，更多详情可参考官网教程。如果完全按照官网提供的电路搭建平台，甚至可以不用管任何算法相关的内容，直接驱动无刷电机。如果完全以实现功能为目的，建议采用此途径。

找到Arduino-FOC仓库，克隆或者直接点击“Download ZIP”按钮下载源代码。

目录结构

解压源代码，可先简单了解一下目录结构，根目录下主要有两个文件夹，一个examples，内部是基于Arduino的例程。另外一个是最主要的src源代码目录。

例程文件后面用到的时候再说，这里先看src下的目录文件：

没错，这里可以看到，SimpleFOC是基于C++的。基于C++面向对象的特性，可以把整个结构封装的更好，也可以使用一些C++的高级语法。

其中“common”目录下除了公用的PID和低通滤波相关代码外，还包括了驱动，电流传感器，位置传感器的抽象类，包含了各传感器的公共属性。而外面的”drivers”,”current_sense”,”sensors”目录下封装了面向各实际方案的不同子类。

common目录内容：

common/base_classes目录内容：

比如src/drivers目录下就包含了3线PWM的BLDC电机驱动和6线PWM的BLDC电机驱动，以及几个步进电机的驱动类，他们都继承与BLDCDriver类，然后各自实现各自独有的属性功能。

而子类目录下的hardware_specific文件夹内，包含的是面向不同硬件平台的驱动接口，也是直接适配新的MCU硬件平台的时候需要适配的代码。

为什么要说直接适配呢？是因为如果只是做电机驱动板，完全可以使用SimpleFOC的软件框架，把MCU等其它的相关驱动代码通过接口封装进来。而我这里显然不属于这种直接适配。我这里是想把SimpleFOC移植到RTThread的框架下，后面还想借助RTThread做其它功能。

SimpleFOC源码加入RTT目录

最后这个SimpleFOC想以一个功能包的形式嵌入到RTThread系统内，可以像RTThread已有的功能包一样，通过UI或者menuconfig进行配置。所以，在./packages目录下创建SimpleFOC目录，然后把SimpleFOC源码包内的src文件夹拷贝进来：

添加SConscript文件

上面的操作只是在文件系统内加入了代码包，要想让RTT编译加入的代码，还需要添加并修改SConscript构建文件。./packages/SimpleFOC目录下的SConscript文件比较简单，直接把./packages目录下的拷贝一份即可：

拷贝到./packages/SimpleFOC目录下的SConscript文件内容如下，主要是起到承接作用，让scons构建工具去子目录内继续查找SConscript文件。

import os
from building import *
objs = []
cwd = GetCurrentDir()
list = os.listdir(cwd)
for item in list:
if os.path.isfile(os.path.join(cwd, item, 'SConscript')):
objs = objs + SConscript(os.path.join(item, 'SConscript'))
Return('objs')

/packages/SimpleFOC/src目录内的SConscript文件稍复杂。我这里使用的这级SConscript文件管理整个源码包的构建。

最主要的是SOURCES的源码添加和CPPPATH的头文件目录的定义。语法说明请参见python以及RTT官网相关教程文档。

...
SOURCES = ["BLDCMotor.c","common/foc_utils.c","common/base_classes/FOCMotor.c"]
SOURCES += ["common/base_classes/CurrentSense.c"]
SOURCES += ["common/base_classes/Sensor.c"]
SOURCES += ["common/lowpass_filter.c"]
SOURCES += ["common/pid.c"]
SOURCES += ["common/time_utils.c"]
SOURCES += ["sensors/MagneticSensorPWM.c"]
if GetDepend(['RT_USING_SIMPLEFOC_DRV_6PWM']):
SOURCES += ['drivers/BLDCDriver6PWM.c']
if GetDepend(['RT_USING_SIMPLEFOC_CURRENT_SENSE_INLINE']):
SOURCES += ['current_sense/InlineCurrentSense.c']
...
...
CPPPATH = [CWD, os.path.join(GetCurrentDir(), 'inc'), CWD+'/common', CWD+'/common/base_classes', CWD+'/sensors', CWD+'/drivers']
CPPPATH += [CWD+'/current_sense']
...

修改Kconfig文件

功能包的添加在/board/Kconfig文件内。如下是我在”Board extended module Drivers”菜单下加入的SimpleFOC的相关配置：

menu "Board extended module Drivers"
menuconfig PKG_USING_SIMPLEFOC
bool "Enable SIMPLE_FOC module"
default n
select RT_USING_SIMPLEFOC
if PKG_USING_SIMPLEFOC
config SIMPLE_FOC_VOLTAGE_POWER_SUPPLY
int "voltage power supply"
default 12
choice
prompt "Select driver stype"
default RT_USING_SIMPLEFOC_DRV_6PWM
config RT_USING_SIMPLEFOC_DRV_6PWM
bool "USING 6PWM MODE DRIVER"
config RT_USING_SIMPLEFOC_DRV_3PWM
bool "USING 3PWM MODE DRIVER"
endchoice
choice
prompt "Select current sense stype"
default RT_USING_SIMPLEFOC_CURRENT_SENSE_INLINE
config RT_USING_SIMPLEFOC_CURRENT_SENSE_INLINE
bool "USING INLINE MODE CURRENT SENSE"
config RT_USING_SIMPLEFOC_CURRENT_SENSE_LOWSIDE
bool "USING LOWSIDE MODE CURRENT SENSE"
endchoice
endif
endmenu

把相关的.cpp文件修改为.c文件

这里只是先修改文件类型，让构建文件能找到相关文件，具体内容后面在修改。比如./packages/SimpleFOC/src/common目录内的相关文件：

使用UI工具配置RTT

完成了以上修改既可以配置RTThread系统，把需要的代码真正添加进来了。

我这里添加进来的所有文件列表如下：

用C代码实现原有C++代码的功能

这里是比较耗费时间的，要用C语言的语法，实现类似C++的继承以及函数重载的功能。大体思路就是继承使用结构体包含的方式实现，函数重载就是用函数指针的方式链接子类（只是类的概念）的实现函数到父类内。具体的可参加我开源的源代码。

功能测试

开环控制

源码包的/examples/motion_control目录下有一些控制例程，我这里暂时测试了开环控制和速度闭环控制，开环控制不用关联位置传感器和电流传感器，只实现驱动器相关的代码即可：

测试代码如下，如果上面的流程没有出错，这里即可看到电机可以正常转起来了，只不过是开环的旋转效果不是很好，尽量不要测试太久，当心电机发热严重。

BLDCMotor Motor_left;
BLDCDriver6PWM Motor_left_driver;
float target_velocity = 6.28;
void motor_test_timeout(void *parameter)
{
Motor_left.foc_motor.ops.move(&(Motor_left.foc_motor), (float )(parameter));
}
int main(void)
{
rt_timer_t motor_tm;
...
BLDCMotor_set_default(&Motor_left, 7, 8, 100, NOT_SET);
BLDCDriver6PWM_set_default(&Motor_left_driver, RT_USING_PWM1_NAME, RT_USING_PWM2_NAME, RT_USING_PWM3_NAME, RT_USING_PWM1_CH, RT_USING_PWM2_CH, RT_USING_PWM3_CH, 10000);
Motor_left_driver.bldc_driver.voltage_power_supply = 8;
Motor_left_driver.bldc_driver.voltage_limit = 8;
Motor_left_driver.bldc_driver.ops.init(&(Motor_left_driver.bldc_driver));
Motor_left.linkDriver(&Motor_left, &(Motor_left_driver.bldc_driver));
Motor_left.foc_motor.voltage_limit = 4;
Motor_left.foc_motor.velocity_limit = 50;
Motor_left.foc_motor.controller = velocity_openloop;
Motor_left.foc_motor.ops.init(&(Motor_left.foc_motor));
motor_tm = rt_timer_create("motor_test_tm", motor_test_timeout, &target_velocity, 10, RT_TIMER_FLAG_SOFT_TIMER | RT_TIMER_FLAG_PERIODIC);
if(motor_tm != RT_NULL)
rt_timer_start(motor_tm);
...
}

闭环控制

上面的开环测试可以正常旋转就说明电路没有问题，就可以测试闭环控制效果了，代码如下：

BLDCMotor Motor_left;
BLDCDriver6PWM Motor_left_driver;
MagneticSensorPWM sensor_pwm;
InlineCurrentSense current_sense;
float target_position = 0;
float target_velocity = 6.28;
void MagneticSensorPWM_callback()
{
Motor_left.foc_motor.ops.loopFOC(&(Motor_left.foc_motor));
Motor_left.foc_motor.ops.move(&(Motor_left.foc_motor), target_velocity);
}
int main(void)
{
...
BLDCMotor_set_default(&Motor_left, 7, 8, 100, NOT_SET);
BLDCDriver6PWM_set_default(&Motor_left_driver, RT_USING_PWM1_NAME, RT_USING_PWM2_NAME, RT_USING_PWM3_NAME, RT_USING_PWM1_CH, RT_USING_PWM2_CH, RT_USING_PWM3_CH, 10000);
MagneticSensorPWM_set_default(&sensor_pwm, "MotorL_sensor","pwm_inputcap1");
InlineCurrentSense_set_default_by_gain(¤t_sense, 0.01f, 50, "adc0", 10, 11, NOT_SET);
current_sense.current_sense.ops.init(&(current_sense.current_sense));
sensor_pwm.sensor.ops.init(&(sensor_pwm.sensor));
FOCMotor_linkSensor(&(Motor_left.foc_motor), &(sensor_pwm.sensor));
FOCMotor_linkCurrentSense(&(Motor_left.foc_motor), &(current_sense.current_sense));
Motor_left_driver.bldc_driver.voltage_power_supply = 8;
Motor_left_driver.bldc_driver.voltage_limit = 8;
Motor_left_driver.bldc_driver.ops.init(&(Motor_left_driver.bldc_driver));
Motor_left.linkDriver(&Motor_left, &(Motor_left_driver.bldc_driver));
CurrentSense_linkDriver(&(current_sense.current_sense), &(Motor_left_driver.bldc_driver));
Motor_left.foc_motor.voltage_limit = 4;
Motor_left.foc_motor.controller = velocity;
Motor_left.foc_motor.PID_velocity.output_ramp = 1000;
Motor_left.foc_motor.PID_velocity.P = 0.04;
Motor_left.foc_motor.PID_velocity.I = 0.4;
Motor_left.foc_motor.PID_velocity.D = 0;
Motor_left.foc_motor.LPF_velocity.Tf = 0.01f;
Motor_left.foc_motor.ops.init(&(Motor_left.foc_motor));
Motor_left.foc_motor.ops.initFOC(&(Motor_left.foc_motor), NOT_SET, CW);
MagneticSensorPWM_enableInterrupt(&sensor_pwm, &MagneticSensorPWM_callback);
...
}
static int motorL_V_PI(int argc, char **argv)
{
rt_err_t result = RT_EOK;
float p, i;
char buf[64];
if(argc >= 3)
{
p = atof(argv[1]);
i = atof(argv[2]);
Motor_left.foc_motor.PID_velocity.P = p;
Motor_left.foc_motor.PID_velocity.I = i;
sprintf(buf, "set P:%.3f,I%.3fn", p, i);
rt_kprintf(buf);
}
else {
rt_kprintf("Usage: n");
rt_kprintf("motorL_V_PI - set the left motor velocity PI valuen");
rt_kprintf("eg:motorL_V_PI 0.2 5 is set left motor velocity P to 0.2, I to 5n");
result = -RT_ERROR;
}
return RT_EOK;
}
MSH_CMD_EXPORT(motorL_V_PI, motorL_V_PI );
static int motorL_V(int argc, char **argv)
{
rt_err_t result = RT_EOK;
float velocity;
char buf[64];
if(argc >= 2)
{
velocity = atof(argv[1]);
target_velocity = velocity;
sprintf(buf, "set velocity:%.3fn", velocity);
rt_kprintf(buf);
}
else {
rt_kprintf("Usage: n");
rt_kprintf("motorL_V - set the left motor velocityn");
rt_kprintf("eg:motorL_V 6 is set left motor velocity to 6 rad/sn");
result = -RT_ERROR;
}
return RT_EOK;
}
MSH_CMD_EXPORT(motorL_V, motorL_V );

可以通过motorL_V_PI命令修改PID的参数，通过motorL_V命令设置转速。

结束语

这里的具体测试效果，我就暂时先不附加视频展示了，等后面忙完RISC-V应用创新大赛的事，再过来详细测试优化，到时候再给出测试的视频效果。

由于还没有完全测试完毕，所以之前的硬件电路图纸和代码也一直没开源出来，到目前为止，至少测试了无刷电机部分的电路没什么大问题。所以后面我会先把目前阶段的相关文件开源出来，地址会加到首篇文章内。后面再测试优化再进行更新。