目錄

項(xiàng)目背景及功能

效果演示

外設(shè)使用情況概述

硬件設(shè)計(jì)

軟件設(shè)計(jì)

具體過(guò)程

開源代碼

結(jié)語(yǔ)

1 項(xiàng)目背景及功能

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

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

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

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

2 效果演示

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

自動(dòng)跟蹤的速度很大程度和模型的效率有關(guān)，這個(gè)人臉識(shí)別大概40ms一幀，效果還算差強(qiáng)人意。

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

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

3 外設(shè)使用情況概述

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

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

使用了硬件定時(shí)器0定時(shí)更新力矩

4 硬件設(shè)計(jì)

玄鐵K230剛好有6路PWM，正好可以實(shí)現(xiàn)驅(qū)動(dòng)FOC云臺(tái)（兩個(gè)FOC電機(jī)），但是在玄鐵K230這個(gè)板子上引出的PWM引腳只有五根，所以我們得先對(duì)玄鐵K230做點(diǎn)小改。

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

焊點(diǎn)比較小怕脫焊，可以打點(diǎn)熱熔膠或綠油。

簡(jiǎn)單畫了個(gè)板方便接線，這塊板上面是MS8313芯片用來(lái)驅(qū)動(dòng)電機(jī)，畫的很潦草大神勿噴。

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

裝好后長(zhǎng)這樣，因?yàn)閷戇@篇文章時(shí)電路板還沒(méi)到，稍后測(cè)試完電路板會(huì)發(fā)上來(lái)，這里用的是杜邦線連接，所以看起來(lái)會(huì)比較亂。

接線對(duì)應(yīng)引腳：

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

5 軟件設(shè)計(jì)

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

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

6 具體過(guò)程

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

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

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

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

SDK Linux：大核跑Linux，沒(méi)有RT-Smart，純 Linux 進(jìn)行開發(fā)。

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

這里因?yàn)槲覀円玫絤icropython所以要搭建CanMV的開發(fā)環(huán)境。

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

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

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

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

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

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

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

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

Hal庫(kù)源碼：/canmv_k230/src/rtsmart/libs/rtsmart_hal/drivers

Hal庫(kù)例程：/canmv_k230/src/rtsmart/libs/testcases/rtsmart_hal

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

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

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

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

初始化IIC，先綁定引腳到IIC外設(shè)，再創(chuàng)建一個(gè)IIC對(duì)象，后面我們通過(guò)這個(gè)對(duì)象操作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編碼器值，我們通過(guò)指定i2c_msg_t類型結(jié)構(gòu)體里.flags的值就可以讓IIC總線發(fā)送或接收消息。

詳細(xì)IIC的操作最好參考下Hal庫(kù)的IIC驅(qū)動(dòng)源碼，官方文檔和例程在這塊給的不是很全，Hal庫(kù)例程只給了寫IIC操作沒(méi)給讀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 編寫電機(jī)三路PWM的輸出驅(qū)動(dòng)

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

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);//第一個(gè)形參是要操作的PWM通道，第二個(gè)是占空比 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;

對(duì)于一個(gè)電機(jī)三路PWM占空比的設(shè)置我們可以將其封裝成一個(gè)函數(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 實(shí)現(xiàn)FOC算法

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

因?yàn)槲乙彩且鲞@個(gè)項(xiàng)目才入門的FOC，目前也只是實(shí)現(xiàn)了電壓力矩位置閉環(huán)，稍后我會(huì)把其他閉環(huán)完成，大佬輕噴。

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

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

// 歸一化角度到 [0,2PI] float_normalizeAngle(floatangle) { floata=fmod(angle,2*PI); //取余運(yùn)算可以用于歸一化，列出特殊值例子算便知 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í)行克拉克逆變換和帕克逆變換，并設(shè)置電機(jī)力矩

#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時(shí)的電角度

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

獲取電機(jī)當(dāng)前的電角度

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); //設(shè)定驅(qū)動(dòng)器供電電壓 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");

更新電機(jī)輸出力矩

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);//筆者實(shí)測(cè)在位置閉環(huán)中加個(gè)D項(xiàng)會(huì)快很多 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);

因?yàn)槲覀冞€要封裝到micropython，所以力矩的更新不能放在while(1)里，并且為了實(shí)時(shí)性，也必須要用到定時(shí)器。

所以我們來(lái)配置一個(gè)定時(shí)器。

定時(shí)器也是通過(guò)一個(gè)對(duì)象來(lái)操作，通過(guò)回調(diào)函數(shù)執(zhí)行中斷代碼。

初始化定時(shí)器，對(duì)定時(shí)器設(shè)置必須先關(guān)停定時(shí)器，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);//筆者實(shí)測(cè)在位置閉環(huán)中加個(gè)D項(xiàng)會(huì)快很多 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 測(cè)試FOC代碼

到此我們已經(jīng)編寫完了FOC的代碼，我們?cè)诜庋b到micropython前可以先編譯測(cè)試一下。

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

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

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

如果輸出[SUCCESS] Built all RTSmart HAL testcases就說(shuō)明代碼沒(méi)有錯(cuò)誤編譯成功，輸出文件夾在

canmv_k230/output/k230_canmv_lckfb_defconfig/rtsmart/libs/elf

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

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

6.6 封裝到micropython

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

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

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

編寫初始化函數(shù)，micropython的函數(shù)都要以mp_obj_t為返回值，如果沒(méi)有返回則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_*注冊(cè)一下該函數(shù)在micropython中的函數(shù)對(duì)象。

注冊(cè)對(duì)象要根據(jù)函數(shù)的形參使用相應(yīng)的宏，

類似的宏有

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)

下面是大于四個(gè)形參的寫法

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ù)后，我們要把剛剛注冊(cè)的對(duì)象放到mp_rom_map_elem_t類型的數(shù)組中，這個(gè)數(shù)組用于存儲(chǔ)MicroPython模塊該庫(kù)的全局符號(hào)表。就是存了這個(gè)模塊的名字，和所有函數(shù)名。

STATIC constmp_rom_map_elem_t dfoc_module_globals_table[] = { {MP_ROM_QSTR(MP_QSTR___name__),MP_ROM_QSTR(MP_QSTR_dfoc) },//這個(gè)dfoc就是我們未來(lái)在寫micropython要調(diào)用的庫(kù)名 {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) }, };

最后將這個(gè)表注冊(cè)到micropython中，注冊(cè)類型是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)添加一行，將我們新增的文件夾添加進(jìn)編譯，然后在上一級(jí)/canmv目錄make一下。

在確保代碼沒(méi)問(wèn)題的情況下，如果編譯不了，清空一下編譯緩存，再全局編譯一下就可以了。

如上輸出代表編譯成功。

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

6.8 測(cè)試micropython

將玄鐵K230連接到CanMV IDE，試下我們剛剛封裝的代碼，順便測(cè)試一下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庫(kù)實(shí)現(xiàn)物體自動(dòng)跟蹤

配合官方人臉檢測(cè)例程，寫一個(gè)物體自動(dòng)跟蹤，只需要加個(gè)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? ? ? ? # 計(jì)算微分項(xiàng)? ? ? ? derivative = error -?self.last_error? ? ? ? # 計(jì)算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)# 自定義人臉檢測(cè)類，繼承自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 ?# 錨點(diǎn)數(shù)據(jù)，用于目標(biāo)檢測(cè)? ? ? ? self.rgb888p_size = [ALIGN_UP(rgb888p_size[0],?16), rgb888p_size[1]] ?# sensor給到AI的圖像分辨率，并對(duì)寬度進(jìn)行16的對(duì)齊? ? ? ? self.display_size = [ALIGN_UP(display_size[0],?16), display_size[1]] ?# 顯示分辨率，并對(duì)寬度進(jìn)行16的對(duì)齊? ? ? ? self.debug_mode = debug_mode ?# 是否開啟調(diào)試模式? ? ? ? self.ai2d = Ai2d(debug_mode) ?# 實(shí)例化Ai2d，用于實(shí)現(xiàn)模型預(yù)處理? ? ? ? self.ai2d.set_ai2d_dtype(nn.ai2d_format.NCHW_FMT, nn.ai2d_format.NCHW_FMT, np.uint8, np.uint8) ?# 設(shè)置Ai2d的輸入輸出格式和類型? ? # 配置預(yù)處理操作，這里使用了pad和resize，Ai2d支持crop/shift/pad/resize/affine，具體代碼請(qǐng)打開/sdcard/app/libs/AI2D.py查看? ? def?config_preprocess(self, input_image_size=None):? ? ? ? with?ScopedTiming("set preprocess config",?self.debug_mode >0): # 計(jì)時(shí)器，如果debug_mode大于0則開啟 ai2d_input_size = input_image_sizeifinput_image_sizeelseself.rgb888p_size # 初始化ai2d預(yù)處理配置，默認(rèn)為sensor給到AI的尺寸，可以通過(guò)設(shè)置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)建預(yù)處理流程 # 自定義當(dāng)前任務(wù)的后處理，results是模型輸出array列表，這里使用了aidemo庫(kù)的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] # 繪制檢測(cè)結(jié)果到畫面上 defdraw_result(self, pl, dets): withScopedTiming("display_draw",self.debug_mode >0): ifdets: pl.osd_img.clear() # 清除OSD圖像 fordetindets: # 將檢測(cè)框的坐標(biāo)轉(zhuǎn)換為顯示分辨率下的坐標(biāo) 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) # 添加顯示模式，默認(rèn)hdmi，可選hdmi/lcd/lt9611/st7701/hx8399/nt35516,其中hdmi默認(rèn)置為lt9611，分辨率1920*1080；lcd默認(rèn)置為st7701，分辨率800*480 display_mode="hdmi" # k230保持不變，k230d可調(diào)整為[640,360] rgb888p_size = [1280,720] # 設(shè)置模型路徑和其他參數(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實(shí)例 display_size=pl.get_display_size() # 初始化自定義人臉檢測(cè)實(shí)例 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() # 配置預(yù)處理 whileTrue: withScopedTiming("total",1): img = pl.get_frame() # 獲取當(dāng)前幀數(shù)據(jù) res = face_det.run(img) # 推理當(dāng)前幀 face_det.draw_result(pl, res) # 繪制結(jié)果 pl.show_image() # 顯示結(jié)果 gc.collect() # 垃圾回收 face_det.deinit() # 反初始化 pl.destroy() # 銷毀PipeLine實(shí)例

快速定位效果

7 開源代碼

https://github.com/Death6sentence/FOC_Lib_for_K230micropython

8 結(jié)語(yǔ)

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

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