題目：運(yùn)動目標(biāo)控制與追蹤系統(tǒng) 本項(xiàng)目基于 K230 平臺實(shí)現(xiàn)運(yùn)動目標(biāo)位置復(fù)位、屏幕邊框巡航、膠帶正方形巡航、數(shù)字 8 循跡演示；并預(yù)留自動追蹤紅色光斑的擴(kuò)展接口。

目錄

項(xiàng)目概述

題目要求—功能對照

硬件清單

接線與引腳映射

軟件與運(yùn)行環(huán)境

標(biāo)定與坐標(biāo)映射（關(guān)鍵）

代碼結(jié)構(gòu)與主要函數(shù)

使用方法

擴(kuò)展：自動追蹤紅色光斑（對接題目(5)）

時間與性能估算

常見問題

安全與注意事項(xiàng)

項(xiàng)目源碼

項(xiàng)目概述

目標(biāo)：在 1 m 處的白色屏幕（有效面積 ≥ 0.6×0.6 m2）上，用綠色激光光斑模擬“運(yùn)動目標(biāo)”，并按鍵觸發(fā)不同的軌跡控制；同時給出像素→電機(jī)角度的標(biāo)定方法。

平臺：K230（攝像頭、FPIOA、GPIO、UART）。

核心思路：

攝像頭采圖與顯示；

像素坐標(biāo)與雙電機(jī)角度的雙線性插值標(biāo)定（pixel_to_angle）；

通過串口（UART1）下發(fā)“使能 / 清零 / 限速位置模式”控制幀驅(qū)動兩個電機(jī)。

使用手冊：

K230：https://wiki.01studio.cc/docs/canmv_k230/

張大頭步進(jìn)電機(jī)：https://www.yuque.com/zhangdatouzhikong/gzng3d

電機(jī)控制工具：https://pan.baidu.com/share/init?surl=rpwA7QtdAglsVxj_y77mmQ&pwd=x7dy

題目要求—功能對照

說明：

題目“原點(diǎn)為正方形左上角”，代碼中已按此設(shè)定（Reset_posi() 將兩個電機(jī)角度設(shè)為 (0,0)）。

(3) 膠帶巡航與 (2) 邊框巡航本質(zhì)相同，只需將軌跡點(diǎn)標(biāo)定到膠帶中心線上（§6）。

硬件清單

K230 開發(fā)板（帶攝像頭接口）

攝像頭（RGB565，工作分辨率 320×240）

3D打印外殼 + 2 臺步進(jìn)電機(jī)（串口協(xié)議控制）

綠色激光筆（光斑直徑 ≤ 1 cm）

紅色激光筆（用于擴(kuò)展項(xiàng)自動追蹤）

4個按鍵（上拉輸入）

接線與引腳映射

UART1：TXD=IO3，RXD=IO4（FPIOA.set_function）

按鍵（上拉輸入）：

SW1→IO35（使能/失能 切換）

SW2→IO34（原點(diǎn)復(fù)位）

SW3→IO33（邊線/膠帶巡航）

SW4→IO32（數(shù)字 8 軌跡）

電機(jī)地址：電機(jī)1：0x01、電機(jī)2： 0x02

軟件與運(yùn)行環(huán)境

固件 / SDK：K230 對應(yīng) Python 運(yùn)行環(huán)境與 media.* 模塊（攝像頭、媒體）

主要依賴：time, os, sys, math, media.sensor, media.display, media.media, machine(UART/FPIOA/Pin)

串口參數(shù)：UART1，115200 bps

標(biāo)定與坐標(biāo)映射（關(guān)鍵）

代碼通過 四點(diǎn)標(biāo)定 + 雙線性插值 將 像素 (x,y) 映射到 電機(jī)角度 (ax, ay)：

# 獲取電機(jī)角度defpixel_to_angle(x, y): # 像素的四角點(diǎn) px_tl= (71,30) # 左上 px_tr= (248,29) # 右上 px_br= (246,206) # 右下 px_bl= (72,205) # 左下 # 對應(yīng)的電機(jī)角度 ang_tl= (0.0,0.0) # 左上 ang_tr= (26.0,0.0) # 右上 ang_br= (26.2,25.8) # 右下 ang_bl= (-0.6,25.6) # 左下 # 把目標(biāo)像素(x,y)映射到[0,1]×[0,1]的歸一化坐標(biāo)(u,v) u= (x - px_tl[0]) / (px_tr[0] - px_tl[0]) v= (px_tl[1] - y) / (px_tl[1] - px_bl[1]) u= max(0, min(1, u)) v= max(0, min(1, v)) # 對x軸角/y軸角分別做雙線性插值 ax= (1-u)*(1-v)*ang_tl[0] + u*(1-v)*ang_tr[0] + u*v*ang_br[0] + (1-u)*v*ang_bl[0] ay= (1-u)*(1-v)*ang_tl[1] + u*(1-v)*ang_tr[1] + u*v*ang_br[1] + (1-u)*v*ang_bl[1] return(round(ax,1), round(ay,1))

(注：這個部分需要自行通過攝像頭識別四個角點(diǎn)的像素坐標(biāo)以及控制電機(jī)轉(zhuǎn)動到四個角點(diǎn)并記錄下角度，方便后續(xù)做像素→電機(jī)角度的映射)

u,v 為像素在四邊形內(nèi)的歸一化坐標(biāo)；

通過雙線性插值得到 (ax, ay)，并四舍五入到 0.1°；

下發(fā)控制前再放大 10 倍（協(xié)議以 0.1° 為單位）：bx=int(ax*10)、by=int(ay*10)。

（注：放大10倍是因?yàn)槊罾锩嫖恢媒嵌龋悍秶鸀?0000000-FFFFFFFF，即0-4294967295，保留一位小數(shù)，值為1表示0.1°，值為10表示1°，以此類推）

如何滿足 (2)/(3) 的“距邊線≤2 cm”：

在標(biāo)定時，將四角像素點(diǎn)精確對齊到屏幕上 0.5 m 正方形 的四個頂點(diǎn)；

巡航點(diǎn)列（x1..x4, y1..y4 或自定義數(shù)組）沿膠帶中軸布置；

調(diào)整 speed 與采樣密度，確?；《?直線段逼近膠帶路徑。

代碼結(jié)構(gòu)與主要函數(shù)

串口協(xié)議：

Set_zero(addr)：將當(dāng)前位置角度清零；

# 將當(dāng)前位置角度清零defSet_zero(addr): # addr=電機(jī)地址 cmd =bytearray(4) # 定義長度為4的指令幀 cmd[0] = addr # 地址（電機(jī)ID） cmd[1] = 0x0A # 功能碼 cmd[2] = 0x6D # 輔助碼 cmd[3] = 0x6B # 校驗(yàn)碼 uart.write(cmd) # 通過串口發(fā)送這條指令

Enable_elect(addr, state)：電機(jī)使能控制；

# 電機(jī)使能控制defEnable_elect(addr, state): # addr=電機(jī)地址,state=使能狀態(tài) cmd= bytearray(6) # 定義長度為6的指令幀 cmd[0] = addr # 地址（電機(jī)ID） cmd[1] = 0xF3 # 功能碼 cmd[2] = 0xAB # 輔助碼 cmd[3] = state # 使能狀態(tài):00/01分別表示不使能/使能 cmd[4] = 0x00 # 同步標(biāo)志 cmd[5] = 0x6B # 校驗(yàn)碼 uart.write(cmd) # 通過串口發(fā)送這條指令

Posi_ctrl(addr, vel, ang)：直通限速位置模式控制。

# 直通限速位置模式控制defPosi_ctrl(addr, vel, ang): # addr=電機(jī)地址, vel=速度, ang=角度 ifang >50: # 角度限制 pass cmd= bytearray(12) # 定義長度為12的指令幀 cmd[0] = addr # 地址（電機(jī)ID） cmd[1] = 0xFB # 功能碼 cmd[2] = 0x01 # 方向 cmd[3] = (vel >>8) &0xFF # 速度(高8位字節(jié)) cmd[4] = vel &0xFF # 速度(低8位字節(jié)) cmd[5] = (ang >>24) &0xFF # 位置角度(高8位字節(jié)bit24 - bit31) cmd[6] = (ang >>16) &0xFF # 位置角度(bit16 - bit23) cmd[7] = (ang >>8) &0xFF # 位置角度(bit8 - bit15) cmd[8] = ang &0xFF # 位置角度(低8位字節(jié)bit0 - bit7) cmd[9] = 0x01 # 運(yùn)動模式 cmd[10] =0x00 # 同步標(biāo)志 cmd[11] =0x6B # 校驗(yàn)碼 uart.write(cmd) # 通過串口發(fā)送這條指令

系統(tǒng)參數(shù)：

addr_1/addr_2、speed、time_1/ time_2、預(yù)設(shè)角點(diǎn) (x0..x4, y0..y4)

(x1..x4, y1..y4)這四個坐標(biāo)是通過實(shí)踐得出的，為了不完全脫離膠帶稍微做了一些調(diào)整。

# 設(shè)置運(yùn)動目標(biāo)控制系統(tǒng)所需參數(shù)addr_1=0x01 # 電機(jī)1地址addr_2=0x02 # 電機(jī)2地址time_1=0.03 # 30mstime_2=2 # 2sspeed =30 # 速度state =1 # 電機(jī)狀態(tài)x0=0 ; y0 = 0 # 原點(diǎn)x1=2 ; y1 = 3 # 左上x2=257; y2 = 3 # 右上x3=260; y3 = 256 # 右下x4=0 ; y4 = 255 # 左下

系統(tǒng)功能：

Clean_elect()：雙電機(jī)清零；

# 將電機(jī)xy值清零def Clean_elect(): Set_zero(addr_1) time.sleep(time_1) Set_zero(addr_2)

Enabled_elect(state)：雙電機(jī)使能；

# 不使能/使能電機(jī)def Enabled_elect(state): Enable_elect(addr_1,state) time.sleep(time_1) Enable_elect(addr_2,state)

Reset_posi()：回到原點(diǎn)（左上角）；

# 啟動運(yùn)動目標(biāo)控制系統(tǒng)def Reset_posi(): Posi_ctrl(addr_1, speed, x0) time.sleep(time_1) Posi_ctrl(addr_2, speed, y0)

Start_system()：按序到達(dá) 5 個角點(diǎn)，實(shí)現(xiàn)邊框/膠帶巡航；

# 啟動運(yùn)動目標(biāo)控制系統(tǒng)def Start_system(): Posi_ctrl(addr_1, speed, x1) time.sleep(time_1) Posi_ctrl(addr_2, speed, y1) time.sleep(time_2) Posi_ctrl(addr_1, speed, x2) time.sleep(time_1) Posi_ctrl(addr_2, speed, y2) time.sleep(time_2) Posi_ctrl(addr_1, speed, x3) time.sleep(time_1) Posi_ctrl(addr_2, speed, y3) time.sleep(time_2) Posi_ctrl(addr_1, speed, x4) time.sleep(time_1) Posi_ctrl(addr_2, speed, y4) time.sleep(time_2) Posi_ctrl(addr_1, speed, x1) time.sleep(time_1) Posi_ctrl(addr_2, speed, y1)

（注：這部分的代碼還未優(yōu)化，比較冗余，這樣寫意義不大?？梢孕薷某色@取四個角點(diǎn)的像素點(diǎn)，通過pixel_to_angle(x, y)這個函數(shù)得到電機(jī)轉(zhuǎn)動的角度進(jìn)而控制電機(jī)運(yùn)動）

pixel_to_angle()：像素→角度（雙線性插值）；（詳見 §6 標(biāo)定）

get_motor_angles_from_pixel()：單點(diǎn)下發(fā)；

# 傳入像素坐標(biāo)并返回電機(jī)角度defget_motor_angles_from_pixel(x, y): ax, ay = pixel_to_angle(x, y) bx =int(ax *10) # 電機(jī)控制需要乘以10 by =int(ay *10) # 電機(jī)控制需要乘以10 print(f"輸入像素坐標(biāo): ({x},{y}) -> 電機(jī)角度: ({ax},{ay}) -> 換算角度: ({bx},{by})") Posi_ctrl(addr_1,100, bx) time.sleep(0.01) Posi_ctrl(addr_2,100, by) time.sleep(0.01)

Reco_eight()：攝像頭找圓（find_circles），采樣 N=48 點(diǎn)/圓并縮放至 96% 半徑；

# 識別數(shù)字8def Reco_eight(): # ----- 拍一張圖 ----- for _ inrange(5): sensor.snapshot() img = sensor.snapshot() # 用于標(biāo)記每個圓的采樣點(diǎn)編號 point_number =1 scale_factor =0.96 # 檢測圓形 for c in img.find_circles(threshold=6000, x_margin=10, y_margin=10, r_margin=10, r_min=35, r_max=45, r_step=4): # 畫出圓 r = c.r() img.draw_circle(c.x(), c.y(), r, color=(255,0,0), thickness=2) print("找到圓: 圓心=(", c.x(),",", c.y(),") 半徑=", c.r()) # ----- 在圓周上采樣48個點(diǎn) ----- N =48 for i inrange(N): theta =2* math.pi * i / N - math.pi /2 px =int(c.x() + r * math.cos(theta)) py =int(c.y() + r * math.sin(theta)) # 計(jì)算新的縮小后的點(diǎn)坐標(biāo) new_px =int(c.x() + (px - c.x()) * scale_factor) new_py =int(c.y() + (py - c.y()) * scale_factor) trajectory_points.append((new_px, new_py)) point_number +=1 # 增加點(diǎn)編號 # 顯示結(jié)果 Display.show_image(img, x=round((lcd_width - sensor.width()) /2), y=round((lcd_height - sensor.height()) /2))

Eight_track(points, t)：按“左圓上半圈→右圓上半圈→右圓下半圈→左圓下半圈”的順/逆時針順序形成 8 字軌跡。

# 數(shù)字8循跡def Eight_track(trajectory_points,times): # 順時針走第一個圓的前半圈 for(x, y)intrajectory_points[:24]: get_motor_angles_from_pixel(x, y) time.sleep(times) # 延時times秒 # 順時針走第二個圓的前半圈 for(x, y)intrajectory_points[48:72]: get_motor_angles_from_pixel(x, y) time.sleep(times) # 延時times秒 # 逆時針走第二個圓的后半圈 for(x, y)intrajectory_points[72:96]: get_motor_angles_from_pixel(x, y) time.sleep(times) # 延時times秒 # 逆時針走第一個圓的后半圈 for(x, y)intrajectory_points[24:48]: get_motor_angles_from_pixel(x, y) time.sleep(times) # 延時times秒 x,y = trajectory_points[1] get_motor_angles_from_pixel(x, y)

使用方法

上電與初始化：

上電前我們需要手動把電機(jī)置于原點(diǎn)；

代碼開頭完成 UART、按鍵、攝像頭初始化；

默認(rèn) Enabled_elect(1) 使能電機(jī)并執(zhí)行 Clean_elect()，記錄當(dāng)前位置為原點(diǎn)。

按鍵操作：

SW1：電機(jī)使能/失能切換；

SW2：回到原點(diǎn)（左上角）；

SW3：沿預(yù)設(shè)邊框/膠帶點(diǎn)位順時針巡航；

SW4：拍照識別兩個圓，生成 8 字軌跡并沿軌跡運(yùn)行（Eight_track(trajectory_points, 0.1)）。

擴(kuò)展：自動追蹤紅色光斑（對接題目(5)）

當(dāng)前代碼暫未實(shí)現(xiàn)自動追蹤紅光斑，后續(xù)可以按照如下開發(fā)路徑進(jìn)行完善：

檢測紅色光斑：在攝像頭圖像做 HSV/閾值分割，尋找最大連通域質(zhì)心 (xr, yr)；

像素→角度：調(diào)用 pixel_to_angle(xr, yr) 得到期望角度 (ax, ay)；

閉環(huán)控制：周期性下發(fā) Posi_ctrl(vel, ang)，或加入 PID（對誤差 e=目標(biāo)角-當(dāng)前角）；

收斂判定與提示：誤差持續(xù) < 閾值（例如 0.2°）且時間 < 2 s，觸發(fā)蜂鳴器/LED 連續(xù)提示；

抗抖與安全：對質(zhì)心做時間濾波，限制角速度與加速度。

時間與性能估算

邊框巡航：Start_system() 在四邊與對角點(diǎn)之間各等待 time_2 = 2 s，總時長約 8~10 s（遠(yuǎn)小于 30 s 限制）。如需更“貼邊”，可在側(cè)邊插入更多中間點(diǎn)。

數(shù)字 8：每個圓 N=48 點(diǎn)，Eight_track(..., 0.1) 單圈約 48×2×0.1 = 9.6 s，可按評分需求調(diào)節(jié) N 與 times。

常見問題

光斑不在邊線上 / 偏差大：檢查 §6 標(biāo)定四點(diǎn)是否準(zhǔn)確落在 0.5 m 正方形頂點(diǎn)；必要時重新測量角度對照表（ang_*）。

運(yùn)動不連貫 / 抖動：適當(dāng)降低 speed，增加過渡點(diǎn)；在串口下發(fā)間隙留足 time.sleep。

找不到圓 / 8 字不閉合：調(diào)節(jié) find_circles 的 threshold / r_min / r_max；提高屏幕對比、降低環(huán)境光。

電機(jī)無響應(yīng)：確認(rèn) Enable_elect(..., 1) 已使能；串口連線正確；地址 0x01/0x02 與實(shí)物一致。

安全與注意事項(xiàng)

激光直視有危險，請確保對人眼安全；

設(shè)備擺放與圖示一致（屏幕 1 m 處），防止反射；

運(yùn)行前先執(zhí)行 Clean_elect()，避免機(jī)械撞限位。

項(xiàng)目源碼

GitHub倉庫地址：https://github.com/ruanqiuqiu/RSOC_K230_project

RT-Thread Github 開源倉庫，歡迎撒個星(Star)支持，更期待你的代碼貢獻(xiàn)：https://github.com/RT-Thread/rt-thread