無(wú)刷電機(jī)小車開發(fā)記錄—移植SimpleFOC流程

前情提要

今天過來繼續(xù)擼我的無(wú)刷電機(jī)小車。驅(qū)動(dòng)無(wú)刷電機(jī)底層需要實(shí)現(xiàn)三大部分：功率驅(qū)動(dòng)，位置反饋以及電流反饋。前面我已經(jīng)適配了功率驅(qū)動(dòng)（6線互補(bǔ)PWM），和位置反饋（PWM接口）的底層驅(qū)動(dòng)代碼。

而電流反饋我電路使用的是線內(nèi)探測(cè)方案，輸出模擬電壓值。驅(qū)動(dòng)程序可暫時(shí)使用RTT的ADC底層驅(qū)動(dòng)。

相關(guān)硬件電路

功率驅(qū)動(dòng)電路和位置反饋的磁編碼器芯片，前面文章給出過簡(jiǎn)介。這里我給出最后一塊電路檢測(cè)相關(guān)的電路：

這里我只檢測(cè)了A，B兩相的相電流，第三項(xiàng)電流可由電流和為零（基爾霍夫第一定律）計(jì)算得到。

其中R45和R49為線內(nèi)采樣電阻，壓差經(jīng)過LT199G1運(yùn)放放大后輸出。LT199G1的放大倍數(shù)是50，由于后面MCU的ADC只能檢測(cè)0~3.3V的正電壓信號(hào)，所以這里加了個(gè)1.5V偏置。

可檢測(cè)最大電流I=1.5V/50/0.01R=3A。對(duì)于我的這個(gè)小電機(jī)來說足夠了，甚至后面可能還需要根據(jù)測(cè)試結(jié)果適當(dāng)增大采樣電阻阻值。

移植SimpleFOC

萬(wàn)事俱備只欠東風(fēng)，到了驅(qū)動(dòng)無(wú)刷電機(jī)最關(guān)鍵的時(shí)刻了，就是移植FOC算法。目前網(wǎng)絡(luò)上可以看到的FOC算法很多，雖然我也沒有過多的接觸，但感覺核心算法基本一致，無(wú)非就是外圍做了一些優(yōu)化，變形。所以我這里選擇了比較簡(jiǎn)單的SimpleFOC進(jìn)行移植。后面可以根據(jù)自己的測(cè)試自行優(yōu)化。

FOC的原理我這里暫時(shí)不提及，網(wǎng)絡(luò)上可以找到包括稚暉君在內(nèi)的很多大佬的科普文章，也肯定比我講解的好，可自行查閱。如果有需要，我這里后面再另外補(bǔ)充一篇算法講解篇。這里只簡(jiǎn)單提及移植過程。

下面給出SimpleFOC的官網(wǎng)鏈接，更多詳情可參考官網(wǎng)教程。如果完全按照官網(wǎng)提供的電路搭建平臺(tái)，甚至可以不用管任何算法相關(guān)的內(nèi)容，直接驅(qū)動(dòng)無(wú)刷電機(jī)。如果完全以實(shí)現(xiàn)功能為目的，建議采用此途徑。

找到Arduino-FOC倉(cāng)庫(kù)，克隆或者直接點(diǎn)擊“Download ZIP”按鈕下載源代碼。

目錄結(jié)構(gòu)

解壓源代碼，可先簡(jiǎn)單了解一下目錄結(jié)構(gòu)，根目錄下主要有兩個(gè)文件夾，一個(gè)examples，內(nèi)部是基于Arduino的例程。另外一個(gè)是最主要的src源代碼目錄。

例程文件后面用到的時(shí)候再說，這里先看src下的目錄文件：

沒錯(cuò)，這里可以看到，SimpleFOC是基于C++的。基于C++面向?qū)ο蟮奶匦?，可以把整個(gè)結(jié)構(gòu)封裝的更好，也可以使用一些C++的高級(jí)語(yǔ)法。

其中“common”目錄下除了公用的PID和低通濾波相關(guān)代碼外，還包括了驅(qū)動(dòng)，電流傳感器，位置傳感器的抽象類，包含了各傳感器的公共屬性。而外面的”drivers”,”current_sense”,”sensors”目錄下封裝了面向各實(shí)際方案的不同子類。

common目錄內(nèi)容：

common/base_classes目錄內(nèi)容：

比如src/drivers目錄下就包含了3線PWM的BLDC電機(jī)驅(qū)動(dòng)和6線PWM的BLDC電機(jī)驅(qū)動(dòng)，以及幾個(gè)步進(jìn)電機(jī)的驅(qū)動(dòng)類，他們都繼承與BLDCDriver類，然后各自實(shí)現(xiàn)各自獨(dú)有的屬性功能。

而子類目錄下的hardware_specific文件夾內(nèi)，包含的是面向不同硬件平臺(tái)的驅(qū)動(dòng)接口，也是直接適配新的MCU硬件平臺(tái)的時(shí)候需要適配的代碼。

為什么要說直接適配呢？是因?yàn)槿绻皇亲鲭姍C(jī)驅(qū)動(dòng)板，完全可以使用SimpleFOC的軟件框架，把MCU等其它的相關(guān)驅(qū)動(dòng)代碼通過接口封裝進(jìn)來。而我這里顯然不屬于這種直接適配。我這里是想把SimpleFOC移植到RTThread的框架下，后面還想借助RTThread做其它功能。

SimpleFOC源碼加入RTT目錄

最后這個(gè)SimpleFOC想以一個(gè)功能包的形式嵌入到RTThread系統(tǒng)內(nèi)，可以像RTThread已有的功能包一樣，通過UI或者menuconfig進(jìn)行配置。所以，在./packages目錄下創(chuàng)建SimpleFOC目錄，然后把SimpleFOC源碼包內(nèi)的src文件夾拷貝進(jìn)來：

添加SConscript文件

上面的操作只是在文件系統(tǒng)內(nèi)加入了代碼包，要想讓RTT編譯加入的代碼，還需要添加并修改SConscript構(gòu)建文件。./packages/SimpleFOC目錄下的SConscript文件比較簡(jiǎn)單，直接把./packages目錄下的拷貝一份即可：

拷貝到./packages/SimpleFOC目錄下的SConscript文件內(nèi)容如下，主要是起到承接作用，讓scons構(gòu)建工具去子目錄內(nèi)繼續(xù)查找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目錄內(nèi)的SConscript文件稍復(fù)雜。我這里使用的這級(jí)SConscript文件管理整個(gè)源碼包的構(gòu)建。

最主要的是SOURCES的源碼添加和CPPPATH的頭文件目錄的定義。語(yǔ)法說明請(qǐng)參見python以及RTT官網(wǎng)相關(guān)教程文檔。

...
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文件內(nèi)。如下是我在”Board extended module Drivers”菜單下加入的SimpleFOC的相關(guān)配置：

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

把相關(guān)的.cpp文件修改為.c文件

這里只是先修改文件類型，讓構(gòu)建文件能找到相關(guān)文件，具體內(nèi)容后面在修改。比如./packages/SimpleFOC/src/common目錄內(nèi)的相關(guān)文件：

使用UI工具配置RTT

完成了以上修改既可以配置RTThread系統(tǒng)，把需要的代碼真正添加進(jìn)來了。

我這里添加進(jìn)來的所有文件列表如下：

用C代碼實(shí)現(xiàn)原有C++代碼的功能

這里是比較耗費(fèi)時(shí)間的，要用C語(yǔ)言的語(yǔ)法，實(shí)現(xiàn)類似C++的繼承以及函數(shù)重載的功能。大體思路就是繼承使用結(jié)構(gòu)體包含的方式實(shí)現(xiàn)，函數(shù)重載就是用函數(shù)指針的方式鏈接子類（只是類的概念）的實(shí)現(xiàn)函數(shù)到父類內(nèi)。具體的可參加我開源的源代碼。

功能測(cè)試

開環(huán)控制

源碼包的/examples/motion_control目錄下有一些控制例程，我這里暫時(shí)測(cè)試了開環(huán)控制和速度閉環(huán)控制，開環(huán)控制不用關(guān)聯(lián)位置傳感器和電流傳感器，只實(shí)現(xiàn)驅(qū)動(dòng)器相關(guān)的代碼即可：

測(cè)試代碼如下，如果上面的流程沒有出錯(cuò)，這里即可看到電機(jī)可以正常轉(zhuǎn)起來了，只不過是開環(huán)的旋轉(zhuǎn)效果不是很好，盡量不要測(cè)試太久，當(dāng)心電機(jī)發(fā)熱嚴(yán)重。

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);
...
}

閉環(huán)控制

上面的開環(huán)測(cè)試可以正常旋轉(zhuǎn)就說明電路沒有問題，就可以測(cè)試閉環(huán)控制效果了，代碼如下：

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的參數(shù)，通過motorL_V命令設(shè)置轉(zhuǎn)速。

結(jié)束語(yǔ)

這里的具體測(cè)試效果，我就暫時(shí)先不附加視頻展示了，等后面忙完RISC-V應(yīng)用創(chuàng)新大賽的事，再過來詳細(xì)測(cè)試優(yōu)化，到時(shí)候再給出測(cè)試的視頻效果。

由于還沒有完全測(cè)試完畢，所以之前的硬件電路圖紙和代碼也一直沒開源出來，到目前為止，至少測(cè)試了無(wú)刷電機(jī)部分的電路沒什么大問題。所以后面我會(huì)先把目前階段的相關(guān)文件開源出來，地址會(huì)加到首篇文章內(nèi)。后面再測(cè)試優(yōu)化再進(jìn)行更新。