目錄

項目背景及功能

效果演示

外設使用情況概述

硬件設計

軟件設計

具體過程

開源代碼

結(jié)語

1 項目背景及功能

大多數(shù)玄鐵K230實現(xiàn)的FOC云臺控制方案都是使用玄鐵K230作為上位機，通過串口等通訊協(xié)議向其他單片機發(fā)送控制信號，再由其他單片機驅(qū)動無刷電機。

并且玄鐵K230上的FOC控制算法是在RT-Smart（RT-Thread的分支）上實現(xiàn)，使用硬件定時器更新輸出力矩，比只用micropython實現(xiàn)，實時性會更優(yōu)異。

同時也將FOC驅(qū)動算法封裝成了micropython的庫，在micropython上可以直接調(diào)用，這樣也有了micropython方便編寫代碼與調(diào)試的特性。我們可以使用micropython原先的一些官方的AI庫配合上自定義的FOC庫，實現(xiàn)諸如物體自動跟蹤等功能。

PS:本項目主要代碼和功能都是在RT-Smart（RT-Thread的分支）上完成的，只是封裝到了micropython，并不是只用micropython完成的。

2 效果演示

在官方人臉識別的例程上，加上了電機控制，將識別出的位置作為閉環(huán)輸入。

自動跟蹤的速度很大程度和模型的效率有關，這個人臉識別大概40ms一幀，效果還算差強人意。

我也試過例程給的YOLO模型，大概120ms一幀，跟蹤效果就會比較卡。后面我會去嘗試一下更快的模型，稍后發(fā)出。

CanMV IDE幀緩沖區(qū)延遲還是比較高的，如果把腳本保存到玄鐵K230離線跑的話，跟蹤效果看起來會比幀緩沖區(qū)這里好。

3 外設使用情況概述

將玄鐵K230的6路PWM全都用上作為電機控制

使用兩路IIC用于獲取編碼器值

使用了硬件定時器0定時更新力矩

4 硬件設計

玄鐵K230剛好有6路PWM，正好可以實現(xiàn)驅(qū)動FOC云臺（兩個FOC電機），但是在玄鐵K230這個板子上引出的PWM引腳只有五根，所以我們得先對玄鐵K230做點小改。

本來筆者看到只有5個PWM引腳引出都準備放棄由玄鐵K230驅(qū)動兩個電機的方案了，但是翻閱原理圖發(fā)現(xiàn)非常幸運的是USR按鈕的引腳是GPIO53正好可以作為PWM5的輸出，而且這塊位置也比較好焊接，焊好后也不影響原功能使用。

焊點比較小怕脫焊，可以打點熱熔膠或綠油。

簡單畫了個板方便接線，這塊板上面是MS8313芯片用來驅(qū)動電機，畫的很潦草大神勿噴。

電機用的是常見的2804電機,使用MS8313芯片作為驅(qū)動，編碼器是AS5600。下面這是FOC硬件框圖。

裝好后長這樣，因為寫這篇文章時電路板還沒到，稍后測試完電路板會發(fā)上來，這里用的是杜邦線連接，所以看起來會比較亂。

接線對應引腳：

云臺Y軸電機編碼器IIC1_SCL → GPIO34IIC1_SDA → GPIO35云臺X軸電機編碼器IIC0_SCL → GPIO48IIC0_SDA → GPIO49Y軸電機三路PWMPWM0 → GPIO42PWM2 → GPIO46PWM4 → GPIO52GPIO_OUTPUT → GPIO40 用作Y軸電機的EN引腳X軸電機三路PWMPWM1 → GPIO61PWM3 → GPIO47PWM5 → GPIO53GPIO_OUTPUT → GPIO04 用作X軸電機的EN引腳

5 軟件設計

下面是軟件的總體框圖，micropython其實就是跑在RT-Smart上的一個程序，對于芯片低層的一些驅(qū)動函數(shù)做了抽象，方便了我們開發(fā)。

但是micropython實時性和靈活性相對較低，并且玄鐵K230上的micropython不支持硬件定時器。FOC控制對于實時性要求還是相對較高的，所以我選擇了先用C實現(xiàn)FOC庫，這樣可以調(diào)用硬件定時器，保證了實時性。

6 具體過程

6.1 搭建CanMV K230開發(fā)環(huán)境

玄鐵K230是一個有大小核的芯片，并且有四種開發(fā)環(huán)境，分別是CanMV、K230 RT-Smart Only、SDK Linux、SDKLinux+RT-Smart SDK。

CanMV：大核跑RT-Smart，沒有Linux，上電后自動運行micropython環(huán)境，可連CanMV IDE使用，RT-Smart串口調(diào)試接口是Uart3。

K230 RT-Smart Only：大核跑RT-Smart，沒有Linux，和CanMV的環(huán)境相比少了micropython。

SDK Linux：大核跑Linux，沒有RT-Smart，純 Linux 進行開發(fā)。

SDKLinux+RT-Smart SDK：大核跑RT-Smart，小核跑Linux，可以方便的使用RT-Smart做硬件操作，也有Linux的資源，但是鏡像編譯時長是最久的。

這里因為我們要用到micropython所以要搭建CanMV的開發(fā)環(huán)境。

可以參考下官方的CanMV SDK搭建過程https://www.kendryte.com/k230_canmv/zh/main/zh/userguide/how_to_build.html

這里我建議使用WSL Ubuntu20.04.06 LTS來搭建，編譯調(diào)試都可以在Windows上解決非常方便。

搭建完成后輸入下在SDK根目錄下執(zhí)行make list-def可以看到SDK所有支持的開發(fā)板，我們選擇帶lckfb字樣的。

然后輸入make k230_canmv_lckfb_defconfig就選擇了玄鐵K230作為編譯目標。

緊接著輸入make就可以全局編譯，如果是第一次搭建完環(huán)境一定要全局編譯一次，不然后面局部編譯是用不了的。

如果遇到權(quán)限不足的情況，可以chmod 777 文件夾。輸出Build K230 done就是編譯成功

這個開發(fā)環(huán)境有四個可供參考的庫源碼和例程文件夾

RT-Smart用戶態(tài)操作例程：/canmv_k230/src/rtsmart/mpp/userapps/sample

Hal庫源碼：/canmv_k230/src/rtsmart/libs/rtsmart_hal/drivers

Hal庫例程：/canmv_k230/src/rtsmart/libs/testcases/rtsmart_hal

micropython封裝實現(xiàn)：/canmv_k230/src/canmv/port

如果要在RT-Smart上開發(fā)建議參考官方文檔，和這些源碼。這里我主要是用Hal庫來實現(xiàn)。

PS：這些例程不是針對玄鐵K230這個開發(fā)板的，所以可能直接用沒有效果，就比如用戶態(tài)例程中的sample_pwm，如果我們要使用該例程輸出pwm，必須參考sample_gpio重新綁定fgpio引腳到pwm，不然不會有輸出。

6.2 編寫AS5600編碼器 IIC驅(qū)動

初始化IIC，先綁定引腳到IIC外設，再創(chuàng)建一個IIC對象，后面我們通過這個對象操作IIC總線

#definei2c_clock 4000000 drv_i2c_inst_t* i2c =NULL; if(drv_fpioa_set_pin_func(34, IIC1_SCL) ==-1||drv_fpioa_set_pin_func(35, IIC1_SDA) ==-1) { printf("Failed to set fpioa pin function\n"); return-1; } if(drv_i2c_inst_create(1, i2c_clock,1000,0xff,0xff, &i2c) ==-1) { printf("Failed to create i2c instance\n"); return-1; }

讀取AS5600編碼器值，我們通過指定i2c_msg_t類型結(jié)構(gòu)體里.flags的值就可以讓IIC總線發(fā)送或接收消息。

詳細IIC的操作最好參考下Hal庫的IIC驅(qū)動源碼，官方文檔和例程在這塊給的不是很全，Hal庫例程只給了寫IIC操作沒給讀IIC操作。

#defineAS5600_I2C_ADDR 0x36 #defineAS5600_ANGLE_REG 0x0C uint16_tAS5600_Get_Angle(drv_i2c_inst_t* i2c){ uint8_twrite_buf[1] = {AS5600_ANGLE_REG}; // 要寫入的寄存器地址 uint8_tread_buf[2]; // 讀取數(shù)據(jù)的緩沖區(qū)，最大 256 字節(jié) // 構(gòu)造寫消息，發(fā)送要讀取的寄存器地址 i2c_msg_twrite_msg = { .addr = AS5600_I2C_ADDR, .flags = DRV_I2C_WR, .len =1, .buf = write_buf }; // 構(gòu)造讀消息，讀取寄存器數(shù)據(jù) i2c_msg_tread_msg = { .addr = AS5600_I2C_ADDR, .flags = DRV_I2C_RD, .len =2, .buf = read_buf }; i2c_msg_tmsgs[2] = {write_msg, read_msg}; // 消息數(shù)組 drv_i2c_transfer(i2c, msgs,2); // 發(fā)送 I2C 消息 uint16_traw_angle = (read_buf[0] <

完成了這部分代碼我們就可以讀出編碼器的數(shù)據(jù)了。

可以再將AS5600的讀取值轉(zhuǎn)為弧度。

floatgetAngle_Without_track(drv_i2c_inst_t* i2c){ floatAngle=AS5600_Get_Angle(i2c)*0.08789*PI/180; returnAngle; }

6.3 編寫電機三路PWM的輸出驅(qū)動

和IIC的操作類似，先綁定fgpio，但是PWM的操作是直接用函數(shù)不是通過一個PWM對象。

intret =0; ret |= drv_fpioa_set_pin_func(42, PWM0); ret |= drv_fpioa_set_pin_func(46, PWM0); ret |= drv_fpioa_set_pin_func(52, PWM0); if(ret !=0) { printf("Failed to set pwm fpioa pin function\n"); return-1; } drv_pwm_init(); drv_pwm_set_freq(0, 100000); drv_pwm_set_freq(0, 100000); drv_pwm_set_freq(0, 100000); drv_pwm_set_duty(0,0);//第一個形參是要操作的PWM通道，第二個是占空比 drv_pwm_set_duty(0,0); drv_pwm_set_duty(0,0); drv_pwm_enable(0); drv_pwm_enable(0); drv_pwm_enable(0); return0;

對于一個電機三路PWM占空比的設置我們可以將其封裝成一個函數(shù)。

voidsetPwm(floatUa,floatUb,floatUc,intPWM0_CHANNEL,intPWM1_CHANNEL,intPWM2_CHANNEL){ // 限制占空比從0到1 floatdc_a = _constrain(Ua / voltage_power_supply,0.0f,1.0f); floatdc_b = _constrain(Ub / voltage_power_supply,0.0f,1.0f); floatdc_c = _constrain(Uc / voltage_power_supply,0.0f,1.0f); //寫入PWM到PWM 0 1 2 通道 drv_pwm_set_duty(PWM0_CHANNEL - PWM0, (int)(dc_a*100)); drv_pwm_set_duty(PWM1_CHANNEL - PWM0, (int)(dc_b*100)); drv_pwm_set_duty(PWM2_CHANNEL - PWM0, (int)(dc_c*100)); }

6.4 實現(xiàn)FOC算法

有了輸入和輸出我們就可以實現(xiàn)FOC算法，在這塊我基本都是參考Deng_FOC的設計。

因為我也是要做這個項目才入門的FOC，目前也只是實現(xiàn)了電壓力矩位置閉環(huán)，稍后我會把其他閉環(huán)完成，大佬輕噴。

https://github.com/ToanTech/DengFOC_Lib/tree/main

兩個角度歸一化函數(shù)，用于限制角度

// 歸一化角度到 [0,2PI] float_normalizeAngle(floatangle) { floata=fmod(angle,2*PI); //取余運算可以用于歸一化，列出特殊值例子算便知 returna >=0? a : (a +2*PI); } // 將角度限制到 -180 到 +180 度的函數(shù) float_normalizeAngle_180(floatangle) { while(angle >180.0) { angle -=360.0; } while(angle < -180.0)?? ? ? ?{? ? ? ? ? ?angle +=?360.0;? ? ? ?}? ? ? ?return?angle;? ?}

執(zhí)行克拉克逆變換和帕克逆變換，并設置電機力矩

#define_constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))//將amt限制在[low, high] voidsetTorque(floatUq,floatangle_el,float*Ualpha,float*Ubeta,float*Ua,float*Ub,float*Uc,intPWM0_CHANNEL,intPWM1_CHANNEL,intPWM2_CHANNEL){ Uq=_constrain(Uq,-voltage_power_supply/2,voltage_power_supply/2); // float Ud=0; angle_el = _normalizeAngle(angle_el); // 帕克逆變換 *Ualpha = -Uq*sin(angle_el); *Ubeta = Uq*cos(angle_el); // 克拉克逆變換 *Ua = *Ualpha + voltage_power_supply/2; *Ub = (sqrt(3)*(*Ubeta)-(*Ualpha))/2+ voltage_power_supply/2; *Uc = (-(*Ualpha)-sqrt(3)*(*Ubeta))/2+ voltage_power_supply/2; setPwm(*Ua,*Ub,*Uc, PWM0_CHANNEL, PWM1_CHANNEL, PWM2_CHANNEL); }

獲取編碼器為0時的電角度

voidDFOC_alignSensor(drv_i2c_inst_t* i2c,float*zero_electric_angle,float*Ualpha,float*Ubeta,float*Ua,float*Ub,float*Uc,intPWM0_CHANNEL,intPWM1_CHANNEL,intPWM2_CHANNEL){ setTorque(3, _3PI_2, Ualpha, Ubeta, Ua, Ub, Uc, PWM0_CHANNEL, PWM1_CHANNEL, PWM2_CHANNEL); sleep(1); *zero_electric_angle=_electricalAngle(0.0f, i2c); setTorque(0, _3PI_2, Ualpha, Ubeta, Ua, Ub, Uc, PWM0_CHANNEL, PWM1_CHANNEL, PWM2_CHANNEL); printf("0電角度：%f\n", *zero_electric_angle); }

獲取電機當前的電角度

float_electricalAngle(floatzero_electric_angle,drv_i2c_inst_t* i2c) { return _normalizeAngle((float)(DIR * PP) *getAngle_Without_track(i2c)-zero_electric_angle); }

具體使用先初始化FOC控制器

#definePWM0_PIN_1 42 #definePWM1_PIN_1 46 #definePWM2_PIN_1 52 #definePWM0_CHANNEL_1 PWM0 #definePWM1_CHANNEL_1 PWM2 #definePWM2_CHANNEL_1 PWM4 #definePWM0_PIN_2 61 #definePWM1_PIN_2 47 #definePWM2_PIN_2 53 #definePWM0_CHANNEL_2 PWM1 #definePWM1_CHANNEL_2 PWM3 #definePWM2_CHANNEL_2 PWM5 drv_i2c_inst_t* AS5600_i2c_1 =NULL; drv_i2c_inst_t* AS5600_i2c_2 =NULL; floatvoltage_power_supply;//電源電壓 floatUalpha_1,Ubeta_1=0,Ua_1=0,Ub_1=0,Uc_1=0; floatUalpha_2,Ubeta_2=0,Ua_2=0,Ub_2=0,Uc_2=0;

AS5600_Init(&AS5600_i2c_1, IIC1_SCL,34, IIC1_SDA,35,4000000); AS5600_Init(&AS5600_i2c_2, IIC0_SCL,48, IIC0_SDA,49,4000000); FOC_PWM_Init(PWM0_CHANNEL_1, PWM0_PIN_1, PWM1_CHANNEL_1, PWM1_PIN_1, PWM2_CHANNEL_1, PWM2_PIN_1); FOC_PWM_Init(PWM0_CHANNEL_2, PWM0_PIN_2, PWM1_CHANNEL_2, PWM1_PIN_2, PWM2_CHANNEL_2, PWM2_PIN_2); DFOC_Vbus(12.6); //設定驅(qū)動器供電電壓 DFOC_alignSensor(AS5600_i2c_1,7,-1, &zero_electric_angle_1, &Ualpha_1, &Ubeta_1, &Ua_1, &Ub_1, &Uc_1, PWM0_CHANNEL_1, PWM1_CHANNEL_1, PWM2_CHANNEL_1); DFOC_alignSensor(AS5600_i2c_2,7,-1, &zero_electric_angle_2, &Ualpha_2, &Ubeta_2, &Ua_2, &Ub_2, &Uc_2, PWM0_CHANNEL_2, PWM1_CHANNEL_2, PWM2_CHANNEL_2); PID_Init(&pid1,0.15,0,1.8, motor_target_1); PID_Init(&pid2,0.15,0,1.8, motor_target_2); printf("FOC_Init\n");

更新電機輸出力矩

floatSensor_Angle_1=getAngle_Without_track(AS5600_i2c_1); floatSensor_Angle_2=getAngle_Without_track(AS5600_i2c_2); Motor_Output_1 = PID_Compute(&pid1, Sensor_Angle_1);//筆者實測在位置閉環(huán)中加個D項會快很多 Motor_Output_2 = PID_Compute(&pid2, Sensor_Angle_2); setTorque(Motor_Output_1,_electricalAngle(zero_electric_angle_1, AS5600_i2c_1), &Ualpha_1, &Ubeta_1, &Ua_1, &Ub_1, &Uc_1, PWM0_CHANNEL_1, PWM1_CHANNEL_1, PWM2_CHANNEL_1); setTorque(Motor_Output_2,_electricalAngle(zero_electric_angle_2, AS5600_i2c_2), &Ualpha_2, &Ubeta_2, &Ua_2, &Ub_2, &Uc_2, PWM0_CHANNEL_2, PWM1_CHANNEL_2, PWM2_CHANNEL_2);

因為我們還要封裝到micropython，所以力矩的更新不能放在while(1)里，并且為了實時性，也必須要用到定時器。

所以我們來配置一個定時器。

定時器也是通過一個對象來操作，通過回調(diào)函數(shù)執(zhí)行中斷代碼。

初始化定時器，對定時器設置必須先關停定時器，hard_timer_callback是我們自定義的回調(diào)函數(shù)

voidhard_timer_callback(void* args){ float Sensor_Angle_1=getAngle_Without_track(AS5600_i2c_1); float Sensor_Angle_2=getAngle_Without_track(AS5600_i2c_2); Motor_Output_1 = PID_Compute(&pid1, Sensor_Angle_1);//筆者實測在位置閉環(huán)中加個D項會快很多 Motor_Output_2 = PID_Compute(&pid2, Sensor_Angle_2); setTorque(Motor_Output_1,_electricalAngle(zero_electric_angle_1, AS5600_i2c_1), &Ualpha_1, &Ubeta_1, &Ua_1, &Ub_1, &Uc_1, PWM0_CHANNEL_1, PWM1_CHANNEL_1, PWM2_CHANNEL_1); setTorque(Motor_Output_2,_electricalAngle(zero_electric_angle_2, AS5600_i2c_2), &Ualpha_2, &Ubeta_2, &Ua_2, &Ub_2, &Uc_2, PWM0_CHANNEL_2, PWM1_CHANNEL_2, PWM2_CHANNEL_2); } intTimer_Init(inttimer_num, drv_hard_timer_inst_t** timer){ rt_hwtimer_info_t info; uint32_t freq; if(drv_hard_timer_inst_create(timer_num, timer) == -1) { printf("Failed to create timer instance\n"); return-1; } drv_hard_timer_stop(*timer); drv_hard_timer_set_mode(*timer, HWTIMER_MODE_PERIOD); drv_hard_timer_get_info(*timer, &info); uint32_t valid_freq = (info.minfreq + info.maxfreq) /2; drv_hard_timer_set_freq(*timer, valid_freq); drv_hard_timer_get_freq(*timer, &freq); printf("Timer frequency:%dHz\n", freq); drv_hard_timer_set_period(*timer,1); drv_hard_timer_register_irq(*timer, hard_timer_callback, NULL); drv_hard_timer_start(*timer); return0; }

6.5 測試FOC代碼

到此我們已經(jīng)編寫完了FOC的代碼，我們在封裝到micropython前可以先編譯測試一下。

只要在我們代碼里聲明一下主函數(shù)然后逐個初始化就行了，我們還可以給個形參方便我們測試。

intmain(intargc,char*argv[]){ if(argc

將代碼放在/canmv_k230/src/rtsmart/libs/testcases/rtsmart_hal目錄下，在該目錄下直接執(zhí)行make就可以編譯代碼。

如果輸出[SUCCESS] Built all RTSmart HAL testcases就說明代碼沒有錯誤編譯成功，輸出文件夾在

canmv_k230/output/k230_canmv_lckfb_defconfig/rtsmart/libs/elf

將對應文件拷到內(nèi)存卡，用TTL轉(zhuǎn)USB模塊連接K230的串口3，使用終端調(diào)試工具調(diào)試。

如果使用官方的CanMV鏡像，初始化完成會默認執(zhí)行micropython，輸入Ctrl+C 退出程序，然后回車，進入到放置elf的位置，執(zhí)行elf。

6.6 封裝到micropython

測試完成可以用后我們就可以移植到micropython，先在/canmv_k230/src/canmv/port新建一個我們庫的文件夾。

在創(chuàng)建一個.c文件，然后可以直接把我們的代碼復制過去。

封裝前要先引用一下兩個頭文件#include"py/obj.h"和#include"py/runtime.h"

編寫初始化函數(shù)，micropython的函數(shù)都要以mp_obj_t為返回值，如果沒有返回則return mp_const_none;

STATICmp_obj_tDFOC_Init(void){ AS5600_Init(&AS5600_i2c_1,IIC1_SCL,34,IIC1_SDA,35,4000000); AS5600_Init(&AS5600_i2c_2,IIC0_SCL,48,IIC0_SDA,49,4000000); ... ... Timer_Init(0, &timer); mp_printf(&mp_plat_print,"FOC Module Initialized\n"); returnmp_const_none; } STATICMP_DEFINE_CONST_FUN_OBJ_0(mp_DFOC_Init_obj, DFOC_Init);

編寫控制函數(shù)，這些都大同小異

STATICmp_obj_tDFOC_Set_Motor_Angle(mp_obj_tangle_obj_1,mp_obj_tangle_obj_2){ pid1.setpoint =mp_obj_get_float(angle_obj_1); pid2.setpoint =mp_obj_get_float(angle_obj_2); returnmp_const_none; } STATICMP_DEFINE_CONST_FUN_OBJ_2(mp_DFOC_Set_Motor_Angle_obj, DFOC_Set_Motor_Angle);

編寫完函數(shù)后要用MP_DEFINE_CONST_FUN_OBJ_*注冊一下該函數(shù)在micropython中的函數(shù)對象。

注冊對象要根據(jù)函數(shù)的形參使用相應的宏，

類似的宏有

MP_DEFINE_CONST_FUN_OBJ_0(obj_name, fun_name) MP_DEFINE_CONST_FUN_OBJ_1(obj_name, fun_name) MP_DEFINE_CONST_FUN_OBJ_2(obj_name, fun_name) MP_DEFINE_CONST_FUN_OBJ_3(obj_name, fun_name) MP_DEFINE_CONST_FUN_OBJ_VAR(obj_name, n_args_min, fun_name) MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(obj_name, n_args_min, n_args_max, fun_name) MP_DEFINE_CONST_FUN_OBJ_KW(obj_name, n_args_min, fun_name)

下面是大于四個形參的寫法

STATICmp_obj_tDFOC_Set_PID(size_tn_args,constmp_obj_t*args){ mp_obj_tKp_obj = args[0]; mp_obj_tKi_obj = args[1]; mp_obj_tKd_obj = args[2]; mp_obj_tnum = args[3]; if(mp_obj_get_int(num) ==1) { pid1.Kp =mp_obj_get_float(Kp_obj); pid1.Ki =mp_obj_get_float(Ki_obj); pid1.Kd =mp_obj_get_float(Kd_obj); } elseif(mp_obj_get_int(num) ==2) { pid2.Kp =mp_obj_get_float(Kp_obj); pid2.Ki =mp_obj_get_float(Ki_obj); pid2.Kd =mp_obj_get_float(Kd_obj); } returnmp_const_none; } STATICMP_DEFINE_CONST_FUN_OBJ_VAR(mp_DFOC_Set_PID_obj,4, DFOC_Set_PID);

編寫完函數(shù)后，我們要把剛剛注冊的對象放到mp_rom_map_elem_t類型的數(shù)組中，這個數(shù)組用于存儲MicroPython模塊該庫的全局符號表。就是存了這個模塊的名字，和所有函數(shù)名。

STATIC constmp_rom_map_elem_t dfoc_module_globals_table[] = { {MP_ROM_QSTR(MP_QSTR___name__),MP_ROM_QSTR(MP_QSTR_dfoc) },//這個dfoc就是我們未來在寫micropython要調(diào)用的庫名 {MP_ROM_QSTR(MP_QSTR_DFOC_Init),MP_ROM_PTR(&mp_DFOC_Init_obj) },//DFOC_Init就是在micropython要使用的初始化函數(shù)名 {MP_ROM_QSTR(MP_QSTR_DFOC_Set_Motor_Angle),MP_ROM_PTR(&mp_DFOC_Set_Motor_Angle_obj) }, {MP_ROM_QSTR(MP_QSTR_DFOC_Set_Motor_Angle_1),MP_ROM_PTR(&mp_DFOC_Set_Motor_Angle_1_obj) }, {MP_ROM_QSTR(MP_QSTR_DFOC_Set_Motor_Angle_2),MP_ROM_PTR(&mp_DFOC_Set_Motor_Angle_2_obj) }, {MP_ROM_QSTR(MP_QSTR_DFOC_AS5600_GetAngle),MP_ROM_PTR(&mp_DFOC_AS5600_GetAngle_obj) }, {MP_ROM_QSTR(MP_QSTR_DFOC_Set_PID),MP_ROM_PTR(&mp_DFOC_Set_PID_obj) }, };

最后將這個表注冊到micropython中，注冊類型是module。

這一段寫好基本不用改，后面增刪函數(shù)只用修改上面的表。

STATICMP_DEFINE_CONST_DICT(dfoc_globals_table, dfoc_module_globals_table); constmp_obj_module_tdfoc_module = { .base = { &mp_type_module }, .globals = (mp_obj_dict_t*)&dfoc_globals_table, }; MP_REGISTER_MODULE(MP_QSTR_dfoc, dfoc_module);

6.7 編譯micropython

在/canmv_k230/src/canmv/port的Makefile文件內(nèi)添加一行，將我們新增的文件夾添加進編譯，然后在上一級/canmv目錄make一下。

在確保代碼沒問題的情況下，如果編譯不了，清空一下編譯緩存，再全局編譯一下就可以了。

如上輸出代表編譯成功。

會在canmv_k230/output/k230_canmv_lckfb_defconfig/canmv輸出一個micropython文件，把它替換掉SD卡內(nèi)原先的micropython文件，可以使用DiskGenius這款軟件。

6.8 測試micropython

將玄鐵K230連接到CanMV IDE，試下我們剛剛封裝的代碼，順便測試一下PID的值

importtime importdfoc importmath dfoc.DFOC_Init() defDFOC_Set_Motor_1(angle1): #40 ~ 130限位，防止把線扯斷了 if(angle1 >130): angle1=130 elif(angle1 150): angle2=150 elif(angle2

6.9 位置環(huán)效果

6.10 配合官方AI庫實現(xiàn)物體自動跟蹤

配合官方人臉檢測例程，寫一個物體自動跟蹤，只需要加個PID，建議加上積分限幅。

fromlibs.PipeLineimportPipeLinefromlibs.AIBaseimportAIBasefromlibs.AI2DimportAi2dfromlibs.Utilsimport*importos,sys,ujson,gc,mathfrommedia.mediaimport*importnncase_runtimeasnnimportulab.numpyasnpimportimageimportaidemoimportdfocdefDFOC_Set_Motor_1(angle1): #40 ~ 130 if(angle1 >130): angle1 =130 elif(angle1 150): angle2 =150 elif(angle2 max_limit: self.integral = max_limit elifself.integral < min_limit:? ? ? ? ? ? self.integral = min_limit? ? ? ? # 計算微分項? ? ? ? derivative = error -?self.last_error? ? ? ? # 計算PID輸出? ? ? ? output =?self.kp * error +?self.ki *?self.integral +?self.kd * derivative? ? ? ? # 更新上一次的誤差? ? ? ? self.last_error = error? ? ? ? return?outputpid1 = PID(kp=0.015, ki=0.001, kd=0.006, setpoint=0, integral_limit=(-10,?10))pid2 = PID(kp=0.015, ki=0.001, kd=0.006, setpoint=0, integral_limit=(-10,?10))motor_x =?90motor_y =?90def?move(x, y):? ? global?motor_x? ? global?motor_y? ? temp1 = pid1.update(x)? ? temp2 = pid2.update(y)? ? motor_x = motor_x - temp1? ? motor_y = motor_y - temp2? ? DFOC_Set_Motor_1(motor_y)? ? DFOC_Set_Motor_2(motor_x)# 自定義人臉檢測類，繼承自AIBase基類class?FaceDetectionApp(AIBase):? ? def?__init__(self, kmodel_path, model_input_size, anchors, confidence_threshold=0.5, nms_threshold=0.2, rgb888p_size=[224,224], display_size=[1920,1080], debug_mode=0):? ? ? ? super().__init__(kmodel_path, model_input_size, rgb888p_size, debug_mode) ?# 調(diào)用基類的構(gòu)造函數(shù)? ? ? ? self.kmodel_path = kmodel_path ?# 模型文件路徑? ? ? ? self.model_input_size = model_input_size ?# 模型輸入分辨率? ? ? ? self.confidence_threshold = confidence_threshold ?# 置信度閾值? ? ? ? self.nms_threshold = nms_threshold ?# NMS（非極大值抑制）閾值? ? ? ? self.anchors = anchors ?# 錨點數(shù)據(jù)，用于目標檢測? ? ? ? self.rgb888p_size = [ALIGN_UP(rgb888p_size[0],?16), rgb888p_size[1]] ?# sensor給到AI的圖像分辨率，并對寬度進行16的對齊? ? ? ? self.display_size = [ALIGN_UP(display_size[0],?16), display_size[1]] ?# 顯示分辨率，并對寬度進行16的對齊? ? ? ? self.debug_mode = debug_mode ?# 是否開啟調(diào)試模式? ? ? ? self.ai2d = Ai2d(debug_mode) ?# 實例化Ai2d，用于實現(xiàn)模型預處理? ? ? ? self.ai2d.set_ai2d_dtype(nn.ai2d_format.NCHW_FMT, nn.ai2d_format.NCHW_FMT, np.uint8, np.uint8) ?# 設置Ai2d的輸入輸出格式和類型? ? # 配置預處理操作，這里使用了pad和resize，Ai2d支持crop/shift/pad/resize/affine，具體代碼請打開/sdcard/app/libs/AI2D.py查看? ? def?config_preprocess(self, input_image_size=None):? ? ? ? with?ScopedTiming("set preprocess config",?self.debug_mode >0): # 計時器，如果debug_mode大于0則開啟 ai2d_input_size = input_image_sizeifinput_image_sizeelseself.rgb888p_size # 初始化ai2d預處理配置，默認為sensor給到AI的尺寸，可以通過設置input_image_size自行修改輸入尺寸 top, bottom, left, right,_ =letterbox_pad_param(self.rgb888p_size,self.model_input_size) self.ai2d.pad([0,0,0,0, top, bottom, left, right],0, [104,117,123]) # 填充邊緣 self.ai2d.resize(nn.interp_method.tf_bilinear, nn.interp_mode.half_pixel) # 縮放圖像 self.ai2d.build([1,3,ai2d_input_size[1],ai2d_input_size[0]],[1,3,self.model_input_size[1],self.model_input_size[0]]) # 構(gòu)建預處理流程 # 自定義當前任務的后處理，results是模型輸出array列表，這里使用了aidemo庫的face_det_post_process接口 defpostprocess(self, results): withScopedTiming("postprocess",self.debug_mode >0): post_ret = aidemo.face_det_post_process(self.confidence_threshold,self.nms_threshold,self.model_input_size[1],self.anchors,self.rgb888p_size, results) iflen(post_ret) ==0: returnpost_ret else: returnpost_ret[0] # 繪制檢測結(jié)果到畫面上 defdraw_result(self, pl, dets): withScopedTiming("display_draw",self.debug_mode >0): ifdets: pl.osd_img.clear() # 清除OSD圖像 fordetindets: # 將檢測框的坐標轉(zhuǎn)換為顯示分辨率下的坐標 x, y, w, h =map(lambdax:int(round(x,0)), det[:4]) xm = x + w/2-1280/2 ym = y + h/2-720/2 x = x *self.display_size[0] //self.rgb888p_size[0] y = y *self.display_size[1] //self.rgb888p_size[1] w = w *self.display_size[0] //self.rgb888p_size[0] h = h *self.display_size[1] //self.rgb888p_size[1] pl.osd_img.draw_rectangle(x, y, w, h, color=(255,255,0,255), thickness=2) # 繪制矩形框 pl.osd_img.draw_cross(int(1280/2*self.display_size[0] //self.rgb888p_size[0]),int(720/2*self.display_size[1] //self.rgb888p_size[1]), color=(255,255,0,255), size=10, thickness=3) move(xm, ym) else: pl.osd_img.clear()if__name__ =="__main__": dfoc.DFOC_Init() dfoc.DFOC_Set_PID(0.15,0,1.8,1) dfoc.DFOC_Set_PID(0.15,0,1.8,2) DFOC_Set_Motor_1(90) DFOC_Set_Motor_2(90) time.sleep(2) # 添加顯示模式，默認hdmi，可選hdmi/lcd/lt9611/st7701/hx8399/nt35516,其中hdmi默認置為lt9611，分辨率1920*1080；lcd默認置為st7701，分辨率800*480 display_mode="hdmi" # k230保持不變，k230d可調(diào)整為[640,360] rgb888p_size = [1280,720] # 設置模型路徑和其他參數(shù) kmodel_path ="/sdcard/examples/kmodel/face_detection_320.kmodel" # 其它參數(shù) confidence_threshold =0.5 nms_threshold =0.2 anchor_len =4200 det_dim =4 anchors_path ="/sdcard/examples/utils/prior_data_320.bin" anchors = np.fromfile(anchors_path, dtype=np.float) anchors = anchors.reshape((anchor_len, det_dim)) # 初始化PipeLine，用于圖像處理流程 pl = PipeLine(rgb888p_size=rgb888p_size, display_mode=display_mode) pl.create() # 創(chuàng)建PipeLine實例 display_size=pl.get_display_size() # 初始化自定義人臉檢測實例 face_det = FaceDetectionApp(kmodel_path, model_input_size=[320,320], anchors=anchors, confidence_threshold=confidence_threshold, nms_threshold=nms_threshold, rgb888p_size=rgb888p_size, display_size=display_size, debug_mode=0) face_det.config_preprocess() # 配置預處理 whileTrue: withScopedTiming("total",1): img = pl.get_frame() # 獲取當前幀數(shù)據(jù) res = face_det.run(img) # 推理當前幀 face_det.draw_result(pl, res) # 繪制結(jié)果 pl.show_image() # 顯示結(jié)果 gc.collect() # 垃圾回收 face_det.deinit() # 反初始化 pl.destroy() # 銷毀PipeLine實例

快速定位效果

7 開源代碼

https://github.com/Death6sentence/FOC_Lib_for_K230micropython

8 結(jié)語

很高興能參加本次的RT-Thread嵌入式大賽，由于最近剛好有多個項目開發(fā)比較倉促，作品做的比較簡陋，目前效果覺得大體上令人滿意就發(fā)出來了。但是在開發(fā)過程中也是越發(fā)覺得玄鐵K230這個板子比較“騷氣”，感覺雙核架構(gòu)還可以研究出很多有意思玩法，基于Linux的一些網(wǎng)絡協(xié)議棧，應該能讓玄鐵K230更方便的做一些物聯(lián)網(wǎng)開發(fā)。目前玄鐵K230的CanMV官方鏡像是只有大核跑RT-Smart，筆者也是比較希望官方能給個像以前一樣，小核也能跑Linux的CanMV鏡像，這樣可以方便開發(fā)在micropython上的一些網(wǎng)絡協(xié)議，這樣就可以讓玄鐵K230訪問一些網(wǎng)絡API。

這個FOC庫（包括硬件設計）筆者正在逐步完善中，如果有更多的人一起豐富玄鐵K230的生態(tài)，未來應該會有很多有意思的庫。