前情提要

今天過來繼續擼我的無刷電機小車。驅動無刷電機底層需要實現三大部分：功率驅動，位置反饋以及電流反饋。前面我已經適配了功率驅動（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應用創新大賽的事，再過來詳細測試優化，到時候再給出測試的視頻效果。

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