RT-Thread-无刷电机小车开发记录05——移植SimpleFOCRT-Thread问答社区

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

发布于 2023-09-17 15:57:28 浏览：1775 订阅该版

[tocm]

# 前情提要
  今天过来继续撸我的无刷电机小车。驱动无刷电机底层需要实现三大部分：功率驱动，位置反馈以及电流反馈。前面我已经适配了功率驱动（6线互补PWM），和位置反馈（PWM接口）的底层驱动代码（相关内容可查看前几篇文章）。而电流反馈我电路使用的是线内探测方案，输出模拟电压值。驱动程序可暂时使用RTT的ADC底层驱动。因为我报名了RISC-V应用创新大赛，要花一些精力去玩去学习图像识别相关的东西了，所以无刷电机小车这一系列文章要暂停两个月更新。今天要做的就是在暂停更新之前先让电机转起来，各方面的适配优化等后面再慢慢玩。
# 相关硬件电路
  功率驱动电路和位置反馈的磁编码器芯片，前面文章给出过简介。这里我给出最后一块电路检测相关的电路：
![screenshot_image.png](https://oss-club.rt-thread.org/uploads/20230917/d3421ee235107a5bdea07bf901500819.png.webp)
  这里我只检测了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的官网链接，更多详情可参考官网教程。如果完全按照官网提供的电路搭建平台，甚至可以不用管任何算法相关的内容，直接驱动无刷电机。如果完全以实现功能为目的，建议采用此途径。
[SimpleFOC官网：https://www.simplefoc.com/](https://www.simplefoc.com/ "SimpleFOC官网：https://www.simplefoc.com/")
  下面给出github的开源链接，有多个仓库，还包括了一个中文文档的仓库，有兴趣的可以看看。我也没看中文文档翻译的好不好，我是直接看的源代码。其实结合代码内的注释来阅读代码已经很直观了。
[SimpleFOC的github开源链接：https://github.com/simplefoc](https://github.com/simplefoc "SimpleFOC的github开源链接：https://github.com/simplefoc")
  找到Arduino-FOC仓库，克隆或者直接点击“Download ZIP”按钮下载源代码。
![screenshot_image.png](https://oss-club.rt-thread.org/uploads/20230917/0c8ecf0be60a10903fb225819c026bf7.png.webp)
## 目录结构
  解压源代码，可先简单了解一下目录结构，根目录下主要有两个文件夹，一个examples，内部是基于Arduino的例程。另外一个是最主要的src源代码目录。
![screenshot_image.png](https://oss-club.rt-thread.org/uploads/20230917/8630c8951550d85b65d0728222643338.png)
  例程文件后面用到的时候再说，这里先看src下的目录文件：
![screenshot_image.png](https://oss-club.rt-thread.org/uploads/20230917/63aabe664d6ee3641b30be018029ef12.png)
  没错，这里可以看到，SimpleFOC是基于C++的。基于C++面向对象的特性，可以把整个结构封装的更好，也可以使用一些C++的高级语法。其中“common”目录下除了公用的PID和低通滤波相关代码外，还包括了驱动，电流传感器，位置传感器的抽象类，包含了各传感器的公共属性。而外面的"drivers","current_sense","sensors"目录下封装了面向各实际方案的不同子类。
  **common**目录内容：
![screenshot_image.png](https://oss-club.rt-thread.org/uploads/20230917/83daa265579fc6806ac50dcf25b0861b.png)
  **common/base_classes**目录内容：
![screenshot_image.png](https://oss-club.rt-thread.org/uploads/20230917/aa8dc7ed82284a7b527f41d5028c14a9.png)
  比如**src/drivers**目录下就包含了3线PWM的BLDC电机驱动和6线PWM的BLDC电机驱动，以及几个步进电机的驱动类，他们都继承与BLDCDriver类，然后各自实现各自独有的属性功能。
![screenshot_image.png](https://oss-club.rt-thread.org/uploads/20230917/a74a1ccd01bc5306a5409cba16780c92.png)
  而子类目录下的**hardware_specific**文件夹内，包含的是面向不同硬件平台的驱动接口，也是直接适配新的MCU硬件平台的时候需要适配的代码。为什么要说**直接适配**呢？是因为如果只是做电机驱动板，完全可以使用SimpleFOC的软件框架，把MCU等其它的相关驱动代码通过接口封装进来。而我这里显然不属于这种**直接适配**。我这里是想把SimpleFOC移植到RTThread的框架下，后面还想借助RTThread做其它功能。
## SimpleFOC源码加入RTT目录
  最后这个SimpleFOC想以一个功能包的形式嵌入到RTThread系统内，可以像RTThread已有的功能包一样，通过UI或者menuconfig进行配置。所以，在**./packages**目录下创建**SimpleFOC**目录，然后把SimpleFOC源码包内的**src**文件夹拷贝进来：
![screenshot_image.png](https://oss-club.rt-thread.org/uploads/20230917/1a8884b38e0bb9fad4e3cf8a02423386.png)
## 添加SConscript文件
  上面的操作只是在文件系统内加入了代码包，要想让RTT编译加入的代码，还需要添加并修改**SConscript**构建文件。**./packages/SimpleFOC**目录下的**SConscript**文件比较简单，直接把**./packages**目录下的拷贝一份即可：
![screenshot_image.png](https://oss-club.rt-thread.org/uploads/20230917/9a7bb087b006fa6954a77915b9dc055d.png)
  拷贝到**./packages/SimpleFOC**目录下的**SConscript**文件内容如下，主要是起到承接作用，让**scons**构建工具去子目录内继续查找**SConscript**文件。
```python
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官网相关教程文档。

```python
...
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的相关配置：
```python
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**目录内的相关文件：
![screenshot_image.png](https://oss-club.rt-thread.org/uploads/20230917/640a25123741724505c66cefa7641aea.png)
## 使用UI工具配置RTT
  完成了以上修改既可以配置RTThread系统，把需要的代码真正添加进来了。
![screenshot_image.png](https://oss-club.rt-thread.org/uploads/20230917/523c54600a310a53f4cb9fb28410557e.png)
  我这里添加进来的所有文件列表如下：
![screenshot_image.png](https://oss-club.rt-thread.org/uploads/20230917/f5d0e6f3d53b6b33c1a541efa951d0e7.png.webp)
## 用C代码实现原有C++代码的功能
  这里是比较耗费时间的，要用C语言的语法，实现类似C++的继承以及函数重载的功能。大体思路就是继承使用结构体包含的方式实现，函数重载就是用函数指针的方式链接子类（只是类的概念）的实现函数到父类内。具体的可参加我开源的源代码。
# 功能测试
## 开环控制
  源码包的**/examples/motion_control**目录下有一些控制例程，我这里暂时测试了开环控制和速度闭环控制，开环控制不用关联位置传感器和电流传感器，只实现驱动器相关的代码即可：
![screenshot_image.png](https://oss-club.rt-thread.org/uploads/20230917/2365466ec92236ec1cf5bed8a979eb02.png)
  测试代码如下，如果上面的流程没有出错，这里即可看到电机可以正常转起来了，只不过是开环的旋转效果不是很好，尽量不要测试太久，当心电机发热严重。
```c
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);
...
}

```
## 闭环控制
  上面的开环测试可以正常旋转就说明电路没有问题，就可以测试闭环控制效果了，代码如下：
```c
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%.3f\n", p, i);
        rt_kprintf(buf);
    }
    else {
        rt_kprintf("Usage: \n");
        rt_kprintf("motorL_V_PI <p_val> <i_val> - set the left motor velocity PI value\n");
        rt_kprintf("eg:motorL_V_PI 0.2 5 is set left motor velocity P to 0.2, I to 5\n");
        result = -RT_ERROR;
    }
    return RT_EOK;
}
MSH_CMD_EXPORT(motorL_V_PI, motorL_V_PI <p_val> <i_val>);

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:%.3f\n", velocity);
        rt_kprintf(buf);
    }
    else {
        rt_kprintf("Usage: \n");
        rt_kprintf("motorL_V <velocity>  - set the left motor velocity\n");
        rt_kprintf("eg:motorL_V 6 is set left motor velocity to 6 rad/s\n");
        result = -RT_ERROR;
    }
    return RT_EOK;
}
MSH_CMD_EXPORT(motorL_V, motorL_V <velocity>);
```
  可以通过**motorL_V_PI**命令修改PID的参数，通过**motorL_V**命令设置转速。
# 结束语
  这里的具体测试效果，我就暂时先不附加视频展示了，等后面忙完RISC-V应用创新大赛的事，再过来详细测试优化，到时候再给出测试的视频效果。由于还没有完全测试完毕，所以之前的硬件电路图纸和代码也一直没开源出来，到目前为止，至少测试了无刷电机部分的电路没什么大问题。所以后面我会先把目前阶段的相关文件开源出来，地址会加到首篇文章内。后面再测试优化再进行更新。
# 相关链接
[本系列首篇文章链接： https://club.rt-thread.org/ask/article/5c0c4ba7eb4ab1ad.html](https://club.rt-thread.org/ask/article/5c0c4ba7eb4ab1ad.html "本系列首篇文章链接： https://club.rt-thread.org/ask/article/5c0c4ba7eb4ab1ad.html")