體育場(chǎng)室內(nèi)清掃機(jī)器人控制系統(tǒng)設(shè)計(jì)

體育場(chǎng)室內(nèi)清掃機(jī)器人控制系統(tǒng)設(shè)計(jì)目錄 保定理工學(xué)院本科畢業(yè)設(shè)計(jì)隨著科技的不斷進(jìn)步，機(jī)器人產(chǎn)業(yè)得到了迅速發(fā)展，越來越多的機(jī)器人被應(yīng)用在軍事、工業(yè)、服務(wù)和醫(yī)療領(lǐng)域，發(fā)揮著舉足輕重的作用[1]。近些年整個(gè)社會(huì)的老年人口越來越多，呈現(xiàn)的老齡化問題非常明顯，伴隨著人們的收入水平不斷地增加，越來越多的人投入到工作中而沒有時(shí)間進(jìn)行家務(wù)勞動(dòng)，所以很多的服務(wù)機(jī)器人開始出現(xiàn)在我們的生活中。清掃機(jī)器人作為家庭環(huán)境服務(wù)的一種，可以自主的清掃垃圾等臟物，而且清掃時(shí)能夠避開障礙物。清掃地面的工作看起來簡(jiǎn)單，但是整個(gè)過程相當(dāng)?shù)暮馁M(fèi)時(shí)間，清掃機(jī)器人具有工作快捷、效率好的優(yōu)點(diǎn)，可以給人們帶來干凈的環(huán)境，徹底解放人們的雙手，也可以減少人力勞動(dòng)成本，具有很廣泛的發(fā)展前景。目前設(shè)計(jì)的機(jī)器人主要是對(duì)于家庭地板的清掃，在工作空間存在較大的清掃局限性，對(duì)于體育場(chǎng)設(shè)計(jì)的清掃機(jī)器人相對(duì)較少。因此設(shè)計(jì)和研發(fā)室內(nèi)體育場(chǎng)清掃機(jī)器人有著廣闊的市場(chǎng)前景和應(yīng)用價(jià)值。2021年呂樹雨，商俊輝，史晨琛等研究人員在《看臺(tái)座椅區(qū)域垃圾清掃機(jī)器人》中提出設(shè)計(jì)專門的適合運(yùn)動(dòng)場(chǎng)或者是操場(chǎng)看臺(tái)的進(jìn)行垃圾清掃的機(jī)器人。由于看臺(tái)范圍整體比較的狹窄，所以對(duì)設(shè)計(jì)的機(jī)器人清掃時(shí)需要考慮狹窄區(qū)域的垃圾處理。"一種新型看臺(tái)座椅垃圾區(qū)域清掃機(jī)器人"能夠?qū)崿F(xiàn)階梯攀爬,狹窄座椅區(qū)域清掃和垃圾的集中收集等功能[2]。這種看臺(tái)清掃機(jī)器人使用起來很方便，可以實(shí)現(xiàn)短時(shí)間內(nèi)體育場(chǎng)的清潔另外操作方便，體現(xiàn)了真正的智能化。2022年王自凱，劉立群，李愛民，易多軒，徐小韋在《基于單片機(jī)控制的清掃機(jī)器人的設(shè)計(jì)》中提出設(shè)計(jì)的STC89C52單片機(jī)的智能清潔機(jī)器人，這款機(jī)器人以單片機(jī)控制為基礎(chǔ)，另外通過使用紅外避障模塊可以更好的躲避障礙物，有效的行駛提升清掃的效率。2022年，溫齊，尹禹超，吳龍?jiān)凇吨е^緣子清掃機(jī)器人的設(shè)計(jì)與分析》中針對(duì)傳統(tǒng)爬桿機(jī)器人在支柱絕緣子上適應(yīng)性差、執(zhí)行清掃作業(yè)困難等問題,論文研究了一種專門針對(duì)支柱絕緣子清掃工作的特種機(jī)器人,該機(jī)器人具有同時(shí)在支柱絕緣子上軸向運(yùn)動(dòng)和旋轉(zhuǎn)運(yùn)動(dòng)的性能。主要闡述該機(jī)器人各種部件的工作原理及特點(diǎn),并進(jìn)行力學(xué)性能校驗(yàn)。該機(jī)器人采用機(jī)械臂彎曲、夾持機(jī)構(gòu)交替爬行實(shí)現(xiàn)支柱絕緣子上行走,采用夾持機(jī)構(gòu)上的旋轉(zhuǎn)電機(jī)進(jìn)行旋轉(zhuǎn)運(yùn)動(dòng),采用可伸縮式毛刷清掃支柱絕緣子,實(shí)現(xiàn)支柱絕緣子清掃功能。2023年周亮軍、陳爾佳、王琪、王飛等研究人員在《一種滾刷盤高壓噴水混合式光伏電池板清掃機(jī)器人的研究》中研究一種滾刷盤-高壓噴水混合式光伏電池板清掃機(jī)器人，該設(shè)計(jì)采用以單電動(dòng)機(jī)為核心的一級(jí)傳動(dòng)結(jié)構(gòu)帶動(dòng)二級(jí)傳動(dòng)結(jié)構(gòu)的模式，減少電動(dòng)機(jī)的使用，從而大大提高經(jīng)濟(jì)效益[3]。獨(dú)特的行走輪輔助輪垂直安裝方式，解決了傳統(tǒng)清潔機(jī)器人在光伏組件上穩(wěn)定性差的問題。2022年

DoHuy;

VuLeAnh;

YiLim;

HoongJoelChanCheng;

TranMinh;

VanDucPhan;

VuMinhBui;

WeegerOliver;

MohanRajeshElara等人在《Heatconductioncombinedgrid-basedoptimizationmethodforreconfigurablepavementsweepingrobotpathplanning》中提出一個(gè)可重構(gòu)的路面清掃機(jī)器人Panthera,可以通過壓縮和擴(kuò)展來重構(gòu)寬度。這種自治系統(tǒng)克服了路面清洗行業(yè)面臨的挑戰(zhàn),如獲取緊湊型和狹窄型路面[4]。寬度的可重構(gòu)性使得機(jī)器人能夠在擴(kuò)展?fàn)顟B(tài)下最大化清洗面積,在壓縮狀態(tài)下通過緊空間進(jìn)行導(dǎo)航,而固定形狀的機(jī)器人無法通過。盡管有其優(yōu)點(diǎn),但大多數(shù)路徑規(guī)劃(PP)算法都是針對(duì)固定形狀機(jī)器人開發(fā)的,對(duì)于能夠在寬度上進(jìn)行擴(kuò)展和壓縮的可重構(gòu)機(jī)器人,PP方面的工作很少或沒有。為了應(yīng)對(duì)這一挑戰(zhàn),通過繪制傳熱學(xué)分析與PP的相似性,提出了一種新的路面掃描可重構(gòu)機(jī)器人PP方法。熱量從熱源傳遞到熱沉,只能通過導(dǎo)電材料傳遞[5]。同樣,PP使得機(jī)器人在進(jìn)入可行區(qū)域的同時(shí),能夠從起始運(yùn)動(dòng)到目標(biāo)。提出的路徑規(guī)劃器采用了一種基于熱梯度上升的組合網(wǎng)格優(yōu)化算法,以平滑度、距離和機(jī)器人足跡最優(yōu)的路徑。路徑規(guī)劃器在四個(gè)虛擬環(huán)境中進(jìn)行測(cè)試,并在真實(shí)環(huán)境中進(jìn)行驗(yàn)證。2023年，KimKyungmin;

SeoChangkook;

KimHwaSoo;

SeoTaeWon等人在《Verticalerrorcontrolforthefa?ade-cleaningrobotequippedonagondola》中提出隨著許多高層建筑的興建,對(duì)其立面的清潔和管理需求自然增加[6]。為了應(yīng)對(duì)這種需求,人們開發(fā)了各種各樣的立面清潔機(jī)器人來保護(hù)工人免受危險(xiǎn)。為了使外立面清潔機(jī)器人在建筑物上可靠運(yùn)行,不僅需要克服窗框等障礙物,還需要對(duì)建筑物外墻進(jìn)行有效清潔。在現(xiàn)有的立面清潔機(jī)器人中,吊籃式立面清潔機(jī)器人具有越障性好、結(jié)構(gòu)簡(jiǎn)單、清潔能力強(qiáng)、安裝方便等優(yōu)點(diǎn)[7]。然而,人們經(jīng)常觀察到,由于安裝在吊籃上的清洗裝置的運(yùn)動(dòng)受到限制,以及風(fēng)引起的吊籃擺動(dòng)等干擾,障礙物周圍的區(qū)域仍然沒有被清理。在這項(xiàng)研究中,提出了一種z軸機(jī)械手(ZAM),用于安裝在吊船上的立面清潔機(jī)器人,以盡量減少障礙物周圍的不清潔區(qū)域。為此,激光傳感器和光傳感器巧妙地結(jié)合起來,以檢測(cè)障礙物。此外,采用控制方法來確保所提出的機(jī)械臂在風(fēng)等擾動(dòng)下具有良好的清潔性能[8]。本研究將完成體育場(chǎng)室內(nèi)清掃機(jī)器人整體的構(gòu)造，該整體通過STM32為基礎(chǔ)、配合使用超聲波傳感器、驅(qū)動(dòng)電機(jī)、攝像頭模塊、無線模塊組成。利用攝像頭單元采集數(shù)據(jù)，傳感器會(huì)生成信號(hào)發(fā)送給單片機(jī)，手機(jī)單元可以通過藍(lán)牙通信的方式連接至單片機(jī)，手機(jī)可以顯示相關(guān)單片機(jī)的信息，而且手機(jī)還能進(jìn)行參數(shù)控制進(jìn)而控制整個(gè)機(jī)器人的運(yùn)動(dòng)。所設(shè)計(jì)的系統(tǒng)具體完成如下幾項(xiàng)功能：1.可以通過相關(guān)的按鈕實(shí)現(xiàn)清掃機(jī)器人的前后左右不同方向的移動(dòng)，而且移動(dòng)過程中也可以進(jìn)行清掃，另外也可以進(jìn)行機(jī)器人的速度控制；2.運(yùn)行過程中能夠有效避障，實(shí)現(xiàn)自我保護(hù)；3.整個(gè)硬件系統(tǒng)的設(shè)計(jì)體積小，重量較輕，便于操縱控制；4.可實(shí)現(xiàn)清掃過程中的長(zhǎng)距離控制；5.顯示機(jī)器人的相應(yīng)狀態(tài)，同時(shí)可以完成遠(yuǎn)距離的無線控制。論文的章節(jié)安排主要有以下幾個(gè)內(nèi)容，具體分為6個(gè)板塊：（1）第1章，緒論，講述清掃機(jī)器人設(shè)計(jì)的必要性以及閱讀相關(guān)的一些文章來描述清掃機(jī)器人的現(xiàn)在研究和將來趨勢(shì)，同時(shí)講述本研究的目的性及功能。（2）第2章，講述整個(gè)機(jī)器人的設(shè)計(jì)，然后進(jìn)行不同應(yīng)用單元的型號(hào)篩選對(duì)比。（3）第3章，系統(tǒng)硬件部分的設(shè)計(jì)，講述針對(duì)總的功能實(shí)現(xiàn)，不同的硬件單元的電路。（4）第4章，系統(tǒng)軟件設(shè)計(jì)，該部分重點(diǎn)講述針對(duì)具體功能代碼編寫的流程圖，以及整個(gè)程序設(shè)計(jì)的總框架。（5）第5章，針對(duì)完成的機(jī)器人作品進(jìn)行電路部分和功能部分的實(shí)驗(yàn)，具體分析相關(guān)的試驗(yàn)過程。（6）第6章，結(jié)論，講述現(xiàn)有的清潔機(jī)器人的研究，并且針對(duì)未來的技術(shù)發(fā)展提出機(jī)器人的進(jìn)一步發(fā)展。本研究將完成體育場(chǎng)室內(nèi)清掃機(jī)器人整體的研究，該整體通過STM32為基礎(chǔ)、配合使用超聲波傳感器、驅(qū)動(dòng)電機(jī)、攝像頭模塊、無線模塊組成。利用攝像頭單元采集數(shù)據(jù)，傳感器會(huì)生成信號(hào)發(fā)送給單片機(jī)，手機(jī)單元可以通過藍(lán)牙通信的方式連接至單片機(jī)，手機(jī)可以顯示相關(guān)單片機(jī)的信息，而且手機(jī)還能進(jìn)行參數(shù)控制進(jìn)而控制整個(gè)機(jī)器人的運(yùn)動(dòng)。方案一：80C51單片機(jī)是MCS-51系列單片機(jī)中的一款型單片機(jī)，即51系列[9]。它設(shè)計(jì)應(yīng)用到CMOS工藝本身運(yùn)行速度快，但是消耗功率很低。但是51單片機(jī)的地址空間僅僅有64kB，外部設(shè)備接口僅有三個(gè)定時(shí)器和一個(gè)串口，相應(yīng)的能夠連的其它的外設(shè)量較少，所以對(duì)全部系統(tǒng)設(shè)計(jì)就會(huì)受到比較大的限制。方案二：STM32是32位的單片機(jī)，STM32系列主要是依賴主流的Cortex內(nèi)核，具有極高的功能性；具有強(qiáng)大的軟件支持和豐富的軟件包；芯片型號(hào)種類多，覆蓋面廣。跟51系列單片機(jī)相比，優(yōu)點(diǎn)，速度更快（時(shí)鐘可以到72M），RAM和ROM的空間更大而且外設(shè)更多。通過兩個(gè)比較，根據(jù)清潔機(jī)器人的功能，本次使用第二個(gè)方案。清掃機(jī)器人這個(gè)選用STM32F407系列芯片。它是ST（意法半導(dǎo)體）研究的ARMCortex?-M4為內(nèi)核的高性能微控制器，其采用了90納米的NVM工藝和ART（自適應(yīng)實(shí)時(shí)存儲(chǔ)器加速器，STM32F407通信接口多達(dá)15個(gè)，定時(shí)器多達(dá)17個(gè)，可以利用支持CompactFlash、SRAM、PSRAM、NOR和NAND存儲(chǔ)器的靈活靜態(tài)存儲(chǔ)器控制器輕松擴(kuò)展存儲(chǔ)容量[10]。圖2.1STM32最小系統(tǒng)圖圖3.1電源模塊實(shí)物圖片圖3.2電源模塊電路圖圖3.3攝像頭模塊實(shí)物圖圖3.4攝像頭模塊電路圖圖3.5BT04引腳圖圖3.6蜂鳴器警報(bào)電路圖3.7SG90舵機(jī)圖片圖3.8超聲波模塊原理圖圖3.9繼電器原理圖圖4.1攝像頭模塊程序流程圖圖4.2藍(lán)牙模塊流程圖圖4.3調(diào)速系統(tǒng)流程圖圖4.4顯示子程序流程圖圖4.5超聲波模塊流程圖圖4.6繼電器模塊流程圖圖5.1實(shí)物展示圖5.2配對(duì)頁(yè)面圖5.3配對(duì)頁(yè)面圖5.4循跡模式圖5.5機(jī)器人追隨小球運(yùn)動(dòng)1.可以通過相關(guān)的按鈕實(shí)現(xiàn)清掃機(jī)器人的前后左右不同方向的移動(dòng)，而且移動(dòng)過程中也可以進(jìn)行清掃，另外也可以進(jìn)行機(jī)器人的速度控制；2.運(yùn)行過程中能夠有效避障，實(shí)現(xiàn)自我保護(hù)；3.整個(gè)硬件系統(tǒng)的設(shè)計(jì)體積小，重量較輕，便于操縱控制；4.可實(shí)現(xiàn)清掃過程中的長(zhǎng)距離控制；5.顯示機(jī)器人的相應(yīng)狀態(tài)，同時(shí)可以完成遠(yuǎn)距離的無線控制。[1]王自凱,劉立群,李愛民等.基于單片機(jī)控制的清掃機(jī)器人的設(shè)計(jì)[J].物聯(lián)網(wǎng)技術(shù),2022,12(08).

[2]史秀男.基于單片機(jī)設(shè)計(jì)的清掃機(jī)器人[J].通訊世界.2015(16).[3]姚興田,馬永林,徐丹等.基于單片機(jī)的樓道清潔機(jī)器人控制系統(tǒng)設(shè)計(jì)[J].制造業(yè)自動(dòng)化.2014(04).

[4]王祥,羅亞,丁紅發(fā).清掃機(jī)器人避障系統(tǒng)區(qū)塊鏈私鑰分片存儲(chǔ)方法[J].機(jī)械設(shè)計(jì)與制造,2022(11).

[5]王輝,羅慶,王鵬等.樓梯清掃策略及技術(shù)應(yīng)用研究[J].甘菊科技,2021(18).

[6]潘瞳,孫文磊,李紅.光伏組件智能清掃機(jī)器人設(shè)計(jì)[J].太陽(yáng)能學(xué)報(bào).2021,42（07）.

[7]賀承宇.清掃機(jī)器人的路徑規(guī)劃與避障算法研究[D].哈爾濱理工大學(xué),2021.

[8]劉斌,趙大荃,鄧偉俊.水面漂浮物自動(dòng)清掃機(jī)器人[J],電工技術(shù).2021（02）.

[9]HolovatyyAndriy.DevelopmentofIoTWeatherMonitoringSystemBasedonArduinoandESP8266Wi-FiModule[J].IOPConferenceSeries:MaterialsScienceandEngineering,2021,10.[10]BishtRavindraSingh,PathakPushparajMani,PanigrahiSorajKumar.High-altitudewall-hangingcleaningrobot[J].MechanismandMachineTheory,2022,168.[11]NguyenChi-Cong,NguyenVan-Tinh,TaoNgoc-Linh,NguyenThanh-Trung,NguyenNgoc-Kien,BuiNgoc-Tam.DesignandFabricationofSiloCleaningRobotUsingVacuumPrinciple[J].JournalofPhysics:ConferenceSeries,2021,19.[12]時(shí)光升,董慧芳,楊佳賓等.一種高層建筑樓梯清掃機(jī)器人的結(jié)構(gòu)設(shè)計(jì)[J].科技風(fēng),2022(36).[13]于入洋,王江濤,何人可等.\o"基于用戶行為的掃地機(jī)器人APP體驗(yàn)優(yōu)化方法研究"基于用戶行為的掃地機(jī)器人APP體驗(yàn)優(yōu)化方法研究[J].\o"包裝工程"包裝工程,\o"2022(02)"2022(02).[14]王星宇;孫京誥.\o"自清潔掃拖烘掃地機(jī)器人的設(shè)計(jì)與實(shí)現(xiàn)"自清潔掃拖烘掃地機(jī)器人的設(shè)計(jì)與實(shí)現(xiàn)[J].\o"現(xiàn)代信息科技"現(xiàn)代信息科技,\o"2021(17)"2021(17).[15]馬可,何人可,馬超民.\o"基于語(yǔ)音交互的家用智能掃地機(jī)器人體驗(yàn)設(shè)計(jì)研究"基于語(yǔ)音交互的家用智能掃地機(jī)器人體驗(yàn)設(shè)計(jì)研究[J].\o"包裝工程"包裝工程,\o"2020(18)"2020(18).[16]曹凱凌;譚玉婷.\o"基于STM32單片機(jī)的寢室掃地機(jī)器人"基于STM32單片機(jī)的寢室掃地機(jī)器人[J].\o"電子制作"電子制作,\o"2018(09)"2018(09).[17]劉隆盛;劉小虎.\o"基于3D打印的小型家用掃地機(jī)器人設(shè)計(jì)"基于3D打印的小型家用掃地機(jī)器人設(shè)計(jì)[J].\o"新余學(xué)院學(xué)報(bào)"新余學(xué)院學(xué)報(bào),\o"2019(03)"2019(03).[18]盧一文.\o"基于單片機(jī)的掃地機(jī)器人的設(shè)計(jì)與實(shí)現(xiàn)"基于單片機(jī)的掃地機(jī)器人的設(shè)計(jì)與實(shí)現(xiàn)[J].\o"數(shù)字技術(shù)與應(yīng)用"數(shù)字技術(shù)與應(yīng)用,\o"2019(02)"2019(02).[19]Gonzalez,David;Perez,Joshue;Milanes,Vicente;Nashashibi,Fawzi.\o"AReviewofMotionPlanningTechniquesforAutomatedVehicles"AReviewofMotionPlanningTechniquesforAutomatedVehicles[J].IEEEtransactionsonintelligenttransportationsystems,2016(4).[20]AndreaCensi;AntonioFranchi;LucaMarchionni;GiuseppeOriolo.\o"SimultaneousCalibrationofOdometryandSensorParametersforMobileRobots."SimultaneousCalibrationofOdometryandSensorParametersforMobileRobots.[J].IEEETrans.Robotics,2013(2).調(diào)速代碼：classPID:_kp=_ki=_kd=_integrator=_imax=0_last_error=_last_derivative=_last_t=0_RC=1/(2*pi*20)def__init__(self,p=0,i=0,d=0,imax=0):self._kp=float(p)self._ki=float(i)self._kd=float(d)self._imax=abs(imax)self._last_derivative=float('nan')defget_pid(self,error,scaler):tnow=millis()dt=tnow-self._last_toutput=0ifself._last_t==0ordt>1000:dt=0self.reset_I()self._last_t=tnowdelta_time=float(dt)/float(1000)output+=error*self._kpifabs(self._kd)>0anddt>0:ifisnan(self._last_derivative):derivative=0self._last_derivative=0else:derivative=(error-self._last_error)/delta_timederivative=self._last_derivative+\((delta_time/(self._RC+delta_time))*\(derivative-self._last_derivative))self._last_error=errorself._last_derivative=derivativeoutput+=self._kd*derivativeoutput*=scalerifabs(self._ki)>0anddt>0:self._integrator+=(error*self._ki)*scaler*delta_timeifself._integrator<-self._imax:self._integrator=-self._imaxelifself._integrator>self._imax:self._integrator=self._imaxoutput+=self._integratorreturnoutputdefreset_I(self):self._integrator=0self._last_derivative=float('nan')完整代碼：importsensor,image,timefrompidimportPIDfrompybimportUART,Pin,Timer,Servoimportpyb,ustructfrompybimportPinfrommachineimportSPI#定義小車狀態(tài)數(shù)據(jù)結(jié)構(gòu)classCARSTATE(object):enSTOP=0enRUN=1enBACK=2enLEFT=3enRIGHT=4enRELEASE=5enCATCH=6enMANUAL=7enTRACING=8enFLITING=9enAVOIDING=10#參數(shù)初始化uart1=UART(1,9600)#PA9,PA10#globalbluetooth_stutas,usr_key_event,key_event,G_FLAGbluetooth_stutas=0key_event=0usr_key_event=0G_FLAG=0spi=pyb.SPI(2,pyb.SPI.SLAVE,polarity=0,phase=0)pin=pyb.Pin("B11",pyb.Pin.IN,pull=pyb.Pin.PULL_UP)clock=time.clock()last_time=time.ticks()key=Pin('A0',Pin.IN,Pin.PULL_UP)usrkey=Pin('D8',Pin.IN,Pin.PULL_UP)#小車狀態(tài)全局變量#globalg_carstate,g_cardir,power,ball_sg_carstate=CARSTATE.enMANUALg_cardir=CARSTATE.enSTOPpower=800ball_s=0#舵機(jī)控制參數(shù)pan_servo=Servo(1)#左右控制tilt_servo=Servo(2)#上下控制pan_angle=0#上下運(yùn)動(dòng)控制，設(shè)置一個(gè)初始角度titl_angle=0#左右舵運(yùn)動(dòng)控制，初始角度THRESHOLD=(53,99,-13,46,29,57)#黃色乒乓球##red_threshold=(55,27,22,51,-2,27)#red_threshold=(61,23,21,67,-7,41)##紅色pan_pid=PID(p=0.2,i=0,imax=90)#脫機(jī)運(yùn)行或者禁用圖像傳輸，使用這個(gè)PIDtilt_pid=PID(p=0.15,i=0,imax=90)#脫機(jī)運(yùn)行或者禁用圖像傳輸，使用這個(gè)PIDdis_pid=PID(p=0.6,i=0.01)len_pid=PID(p=0.2,i=0.01)###############################小車PWM控制########################################如果上電時(shí)出現(xiàn)電機(jī)異常運(yùn)轉(zhuǎn)，是因?yàn)镻B4引腳默認(rèn)為JTAG功能，更新到最新日期的固件可解決這個(gè)問題pwmA1=Pin('B0')#TIM3_CH3pwmA2=Pin('B1')#TIM3_CH4pwmB1=Pin('B4')#TIM3_CH1pwmB2=Pin('B5')#TIM3_CH2#定義定時(shí)器3，頻率為1000Hz，即1ms一次定時(shí)器中斷tim=Timer(3,freq=1000)#初始化PWM，通過調(diào)節(jié)PWM控制左右輪速度，初試占空比為0（車靜止）Left1=tim.channel(4,Timer.PWM,pin=pwmA1,pulse_width_percent=0)Left2=tim.channel(3,Timer.PWM,pin=pwmA2,pulse_width_percent=0)Ritht1=tim.channel(1,Timer.PWM,pin=pwmB1,pulse_width_percent=0)Ritht2=tim.channel(2,Timer.PWM,pin=pwmB2,pulse_width_percent=0)defrun(left_speed,right_speed):#print("left_speed",left_speed)#print("right_speed",right_speed)ifleft_speed<-100:left_speed=-100elifleft_speed>100:left_speed=100ifright_speed<-100:right_speed=-100elifright_speed>100:right_speed=100ifleft_speed>0:Left1.pulse_width_percent(abs(left_speed))Left2.pulse_width_percent(abs(0))else:Left1.pulse_width_percent(abs(0))Left2.pulse_width_percent(abs(left_speed))ifright_speed>0:Ritht1.pulse_width_percent(abs(right_speed))Ritht2.pulse_width_percent(abs(0))else:Ritht1.pulse_width_percent(abs(0))Ritht2.pulse_width_percent(abs(right_speed))#定義機(jī)械爪舵機(jī)運(yùn)動(dòng)角度#輸入：舵機(jī)運(yùn)動(dòng)到目標(biāo)角度#輸出：無defservo_control_angle(servo,servo_angle):ifservo_angle<=0:servo_angle=0ifservo_angle>=180:servo_angle=180percent=(servo_angle+45)/18servo.pulse_width_percent(percent)#功能：目標(biāo)距離計(jì)算#輸入：目標(biāo)寬度或直徑，單位：像素值,real_Lm，目標(biāo)實(shí)際長(zhǎng)度或直徑，一般取長(zhǎng)+寬/2#輸出：距離mm#note:不同的鏡頭焦距和攝像頭芯片對(duì)應(yīng)的距離參數(shù)不同，具體可配合超聲波測(cè)距模塊進(jìn)行距離參數(shù)標(biāo)定defobj_distance(obj_Lm):distance=16000/obj_Lm#obj_Lm為目標(biāo)長(zhǎng)度，分子為距離系數(shù)，不同的攝像頭需要測(cè)定一下系數(shù)#print(distance)returndistance#功能：尋找最大的色塊，計(jì)算方式為像素面積排序#輸入：色塊鏈表blobs，中心坐標(biāo)X(blob[0]),中心坐標(biāo)y(blob[1]),（像素寬）blob[2],（像素高）blob[3],#輸出：最大色塊max_blobdeffind_max(blobs):max_size=0forblobinblobs:ifblob[2]*blob[3]>max_size:max_blob=blobmax_size=blob[2]*blob[3]returnmax_blobdefwifi_key_detect():globalkey_eventkey_state=0if(key_state==0):if(key.value()==0):key_state=key_state+1elif(key_state<2):key_state=key_state+1elif(key_state==2):key_state=key_state+1key_event=1else:if(key.value()==1):key_state=0returnkey_eventdefusr_key_detect():globalusr_key_eventusrkey_state=0if(usrkey_state==0):if(usrkey.value()==0):usrkey_state=usrkey_state+1elif(usrkey_state<2):usrkey_state=usrkey_state+1elif(key_state==2):usrkey_state=usrkey_state+1usr_key_event=1else:if(usrkey.value()==1):usrkey_state=0returnusr_key_eventdefbluetooth_deal():globalbluetooth_stutas,G_FLAG,g_cardir,g_carstateifuart1.any():rec=uart1.read(1)ifrec!=None:rec=bytes(rec)print(rec)ifrec==b'\r'and(bluetooth_stutas==0):bluetooth_stutas=1ifrec==b'\n'and(bluetooth_stutas==1):bluetooth_stutas=2ifbluetooth_stutas==2:ifrec==b'\x00':#停車g_cardir=CARSTATE.enSTOPifrec==b'\x01':#前進(jìn)g_cardir=CARSTATE.enRUNifrec==b'\x02':#后退g_cardir=CARSTATE.enBACKifrec==b'\x03':#左轉(zhuǎn)g_cardir=CARSTATE.enLEFTifrec==b'\x04':#右轉(zhuǎn)g_cardir=CARSTATE.enRIGHTifrec==b'\x05':#循跡g_carstate=CARSTATE.enTRACINGifrec==b'\x06':#跟隨g_carstate=CARSTATE.enFLITINGG_FLAG=2ifrec==b'\x07':#避障g_carstate=CARSTATE.enAVOIDINGG_FLAG=1ifrec==b'\x08':#關(guān)燈g_cardir=CARSTATE.enRELEASEifrec==b'\x09':#開燈g_cardir=CARSTATE.enCATCHBST_fLeftMotorOut=0BST_fRightMotorOut=0g_power=500if(g_carstate==CARSTATE.enMANUAL):#手動(dòng)控制小車if(g_cardir==CARSTATE.enSTOP):BST_fLeftMotorOut =0BST_fRightMotorOut=0elif(g_cardir==CARSTATE.enRUN):BST_fLeftMotorOut=g_powerBST_fRightMotorOut=g_powerelif(g_cardir==CARSTATE.enBACK):BST_fLeftMotorOut=-g_power/2BST_fRightMotorOut=-g_power/2elif(g_cardir==CARSTATE.enLEFT):BST_fLeftMotorOut=-g_power/2BST_fRightMotorOut