網(wǎng)球識(shí)別撿球機(jī)器人

網(wǎng)球識(shí)別撿球機(jī)器人摘要伴隨社會(huì)的快速進(jìn)步，人民的生活水平日益提高，人們非常的重視自身的健康狀況。網(wǎng)球運(yùn)動(dòng)是一種形式優(yōu)雅的運(yùn)動(dòng)，但運(yùn)動(dòng)過程劇烈，網(wǎng)球在世界范圍內(nèi)受的關(guān)注度很高。隨著網(wǎng)球運(yùn)動(dòng)的不斷普及和推廣，運(yùn)動(dòng)的過程中對(duì)于掉落在地上的網(wǎng)球，需要進(jìn)行重復(fù)的收集，這種無意義的體力消耗，不僅枯燥而且非常浪費(fèi)時(shí)間。而且由于網(wǎng)球是在地面上進(jìn)行的，運(yùn)動(dòng)者在移動(dòng)時(shí)很有可能踩到網(wǎng)球，從而導(dǎo)致摔倒，并且可能遭受更大的身體傷害的概率也增加。然而目前市場(chǎng)上的網(wǎng)球收集裝置種類較少，許多裝置大部分都依賴人工操作，因此無法從根本上解決撿球過程中的人工參與問題。文中詳細(xì)介紹了一種基于STM32單片機(jī)的網(wǎng)球識(shí)別撿球機(jī)器人系統(tǒng)的設(shè)計(jì)，通過與傳感器和攝像頭配合可以實(shí)現(xiàn)網(wǎng)球的目標(biāo)識(shí)別、路徑的規(guī)劃及機(jī)械臂的抓取，同時(shí)可以實(shí)現(xiàn)手機(jī)的遠(yuǎn)程控制及參數(shù)讀取。這一系統(tǒng)對(duì)降低人工勞動(dòng)成本、提升撿球效率有著廣泛的市場(chǎng)價(jià)值。關(guān)鍵詞：撿球機(jī)器人;圖像識(shí)別;循跡;避障;網(wǎng)球抓取

目錄第1章緒論 圖3.4蜂鳴器警報(bào)電路3.5攝像頭模塊OV2640是1/4寸的圖像傳感器的一種型號(hào)。整個(gè)傳感器本身可以自動(dòng)控制，支持多種數(shù)據(jù)形式的傳輸。特點(diǎn)：（1）支持UXGA,SXGA,SVGA以及從SXGA按比例縮小到40*30的任何尺寸（2）高靈敏度、低電壓適合嵌入式應(yīng)用（3）標(biāo)準(zhǔn)的SCCB接口，兼容IIC接口（4）支持圖像縮放和閃光燈（5）支持圖像壓縮，即可輸出JPEG圖像數(shù)據(jù)圖3.5攝像頭模塊電路圖圖3.6攝像頭模塊實(shí)物圖3.6SG90舵機(jī)舵機(jī)，也被叫做伺服電機(jī)，本質(zhì)上是一種具有反饋結(jié)構(gòu)形式的電機(jī)，它本身的轉(zhuǎn)速很快，可以快速的完成任務(wù)，還能控制時(shí)進(jìn)行準(zhǔn)確的定向。一般應(yīng)用舵機(jī)，主要是在一些需要進(jìn)行方向變化但可以保持的控制系統(tǒng)中使用。舵機(jī)可以通過輸入的PWM波的占空比來改變角度的變化方向，撿球機(jī)器人使用了SG90舵機(jī)，可以完成0-180度的角度變化。通過使用舵機(jī)來控制機(jī)器人底盤的運(yùn)動(dòng)和機(jī)械臂的抓取控制。如圖3.8所示。圖3.7舵機(jī)實(shí)物圖圖3.8舵機(jī)原理圖3.7本章小結(jié)本章對(duì)每個(gè)單元進(jìn)行了詳細(xì)的敘述。熟悉蜂鳴器報(bào)警單元、STM32單元、圖像識(shí)別單元，以及不同單元之間聯(lián)系。通過各系統(tǒng)的聯(lián)動(dòng)，對(duì)機(jī)器人進(jìn)行監(jiān)控。在本章中，在焊接電路時(shí)必須熟悉每個(gè)設(shè)備的引腳，并且在焊接時(shí)要小心，以避免出現(xiàn)操作錯(cuò)誤，損壞器件。

第4章系統(tǒng)軟件部分設(shè)計(jì)因?yàn)橹谱鞯木W(wǎng)球識(shí)別撿球機(jī)器人，依賴手機(jī)和單片機(jī)相互通信。所以，對(duì)于手機(jī)調(diào)試，需要考慮對(duì)應(yīng)APP程序設(shè)計(jì)。這里先使用藍(lán)牙，讓手機(jī)和單片機(jī)連接。然后進(jìn)行上電，進(jìn)行功能實(shí)驗(yàn)，判斷功能滿足與否。4.1手機(jī)客戶端APP設(shè)計(jì)考慮到手機(jī)端設(shè)計(jì)，本文使用了個(gè)游戲遙控圖標(biāo)，作為手機(jī)APP圖標(biāo)。本身設(shè)計(jì)的APP界面簡(jiǎn)單，只是增加了功能設(shè)計(jì)，然后進(jìn)行了界面美觀，包含了一些指令，這樣可以讓人很容易操作。圖4.1手機(jī)APP界面4.1.1使藍(lán)牙模塊進(jìn)行APP進(jìn)行通信確定要進(jìn)行連接，首先先要打開手機(jī)上的藍(lán)牙，然后把單片機(jī)的藍(lán)牙模塊接入。手機(jī)APP上出來登錄界面，輸入藍(lán)牙連接密碼，這樣就把手機(jī)和單片機(jī)連在一起。藍(lán)牙的連接密碼，可以通過程序設(shè)定，這里使用初設(shè)的密碼。登錄頁面如圖所示：圖4.2登錄界面登錄頁面的設(shè)計(jì)很簡(jiǎn)單，包括Android移動(dòng)客戶端，瀏覽藍(lán)牙手機(jī)的IP地址和端口。該設(shè)計(jì)選擇藍(lán)色作為界面的主背景色，方向控制的圖標(biāo)選擇綠色背景，功能框描述為黑色字體。頁面布局合理，整體主要展示攝像范圍。APP的設(shè)計(jì)功能，完全可以滿足使用的要求。4.1.2控制頁面圖4.3參數(shù)控制界面4.1.3顯示屏程序設(shè)計(jì)如圖10就是手機(jī)端屏幕顯示的攝像頭拍攝范圍，通過攝像頭，可以操控機(jī)械手，完成指定目標(biāo)的抓取動(dòng)作。顯示屏顯示界面清晰，便于操作人員控制。圖4.4手機(jī)端實(shí)物顯示4.2藍(lán)牙模塊程序設(shè)計(jì)本次設(shè)計(jì)的網(wǎng)球識(shí)別機(jī)器人，整個(gè)的流程為設(shè)備首先要初始化，然后調(diào)用藍(lán)牙模塊，通過是否連接成功進(jìn)行判斷，如果連接成功，就在手機(jī)上有圖像顯示，反之沒有?？偟某绦蛉绺戒浰荆鞒虉D如圖4.2所示：圖4.5通信主流程圖4.3紅外避障循跡模塊程序設(shè)計(jì) 本次設(shè)計(jì)的機(jī)器人小車接通電源后，首先要初始化，然后開始讀取循跡模塊數(shù)據(jù)，當(dāng)中間檢測(cè)到黑線時(shí)會(huì)前進(jìn)，左邊檢測(cè)到黑線時(shí)會(huì)左轉(zhuǎn)，右邊檢測(cè)到黑線時(shí)則會(huì)右轉(zhuǎn)，流程圖如下圖4.3所示：圖4.6紅外避障循跡模塊流程圖4.4攝像頭模塊流程圖機(jī)器人小車進(jìn)行單片機(jī)初始化后，通過攝像頭采集網(wǎng)球的圖像數(shù)據(jù)，然后進(jìn)行傳輸?shù)絾纹瑱C(jī)，單片機(jī)會(huì)驅(qū)動(dòng)機(jī)械臂完成抓取。流程圖如圖4.7所示：圖4.7攝像頭模塊流程圖4.5舵機(jī)流程圖當(dāng)小車進(jìn)行單片機(jī)初始化后，通過攝像頭檢測(cè)數(shù)據(jù)，將信息傳到舵機(jī)中，從而舵機(jī)動(dòng)作帶動(dòng)機(jī)械臂，完成抓取。圖4.8舵機(jī)流程圖4.6本章小結(jié)本章介紹了手機(jī)APP界面設(shè)計(jì)，以及藍(lán)牙的程序流程分析，以及手機(jī)和單片機(jī)通信的主流程圖，最終通過正確編寫代碼，完成了網(wǎng)球識(shí)別撿球機(jī)器人的軟件設(shè)計(jì)，實(shí)現(xiàn)了控制機(jī)器人的運(yùn)動(dòng)和抓取過程，并具有了遠(yuǎn)程控制的功能。第5章系統(tǒng)調(diào)試5.1硬件檢查購買完所需的元器件，按照電路圖檢查安裝的線路，根據(jù)電路連線，進(jìn)行逐步檢查。按照原理圖，放置好元件，然后根據(jù)連通的順序，進(jìn)行焊接。剛開始焊接溫度掌控不好，焊接過程中，焊點(diǎn)焊的不牢固，有的焊點(diǎn)過大，和周圍粘連，出現(xiàn)線路短路，導(dǎo)致電路錯(cuò)誤。如果電路中出現(xiàn)短路，會(huì)造成器件燒壞，又或者更嚴(yán)重的后果。當(dāng)電源連接沒問題后，然后觀察焊接的電路板看有沒有少焊、多焊的位置，這樣就可以保證硬件電路板正常了。5.2軟件軟件調(diào)試相對(duì)來說，更加的困難。首先，先要把程序通過下載線，下載到單片機(jī)中，然后給機(jī)器人通電，觀察電源指示正常與否，然后驗(yàn)證機(jī)器人能否做指令動(dòng)作；再次把手機(jī)和單片機(jī)連接，通過初始密碼配對(duì)，然后手機(jī)控制，按方向按鈕，觀察是否動(dòng)作，以及動(dòng)作方向是否正確。這些就是本次實(shí)驗(yàn)在軟件調(diào)試的時(shí)候遇到的問題。5.3實(shí)物調(diào)試圖5.1設(shè)計(jì)實(shí)物（1）首先網(wǎng)球識(shí)別撿球機(jī)器人在通電后，小車可以通過紅外循跡傳感器在黑色標(biāo)記下進(jìn)行自動(dòng)循跡模式，當(dāng)小車通過識(shí)別模塊和超聲波模塊可以測(cè)出相應(yīng)距離，識(shí)別到目標(biāo)網(wǎng)球，這時(shí)候抓取機(jī)器人可以自動(dòng)走到網(wǎng)球面前抬動(dòng)機(jī)械臂并且張開爪子進(jìn)行網(wǎng)球抓取。圖5.2小車循跡圖5.3抓取網(wǎng)球（2）除此之外，小車還可以實(shí)現(xiàn)無線通信，利用小車上的藍(lán)牙模塊，可以用手機(jī)與小車進(jìn)行連接。第一步，拿出手機(jī)打開視頻小車的軟件APP，這個(gè)時(shí)候屏幕上會(huì)出現(xiàn)一個(gè)搜索設(shè)備圖標(biāo)，點(diǎn)擊設(shè)備找到小車設(shè)備名稱，就可以進(jìn)行連接。圖5.4藍(lán)牙連接界面（3）在連接成功之后，通過小車的攝像頭模塊軟件上會(huì)顯示小車前方的界面，這時(shí)可以點(diǎn)擊跟隨模式，使用方向鍵進(jìn)行移動(dòng)，當(dāng)小車移動(dòng)到網(wǎng)球的網(wǎng)球的跟前時(shí)再次點(diǎn)擊跟隨模式，就可以使用方向鍵來控制機(jī)械臂和爪子了。圖5.5手機(jī)顯示界面（4）在小車抓住網(wǎng)球的時(shí)候，再一次通過方向鍵控制小車的機(jī)械臂，將網(wǎng)球放在指定位置。圖5.6機(jī)器人抓球后運(yùn)動(dòng)圖第6章結(jié)論與展望6.1結(jié)論與展望本次設(shè)計(jì)為網(wǎng)球識(shí)別撿球機(jī)器人系統(tǒng)設(shè)計(jì)，硬件部分的設(shè)計(jì)主要是藍(lán)牙模塊、電源電路，還有單片機(jī)設(shè)計(jì)控制回路制作，然后根據(jù)手機(jī)，進(jìn)行相應(yīng)的運(yùn)動(dòng)遠(yuǎn)程控制。選用主板芯片STM32微處理器，搭配紅外傳感器、圖像識(shí)別模塊、超聲波模塊、無線通信模塊控制機(jī)器人，完成對(duì)網(wǎng)球的目標(biāo)識(shí)別與相對(duì)定位，并通過機(jī)械臂抓取，完成撿球動(dòng)作。手機(jī)APP可以表明，藍(lán)牙連接信息的上傳和接受量。除此之外，手機(jī)上APP可以設(shè)定控制參數(shù)范疇。超范圍外，單片機(jī)設(shè)計(jì)蜂鳴器警報(bào)，手機(jī)上APP警報(bào)。經(jīng)過這次網(wǎng)球識(shí)別機(jī)器人的制作，讓自己一方面鍛煉了動(dòng)手能力，另一方面也把課本知識(shí)進(jìn)行了應(yīng)用。設(shè)計(jì)時(shí)，先研究文字資料，了解人工智能技術(shù)，然后實(shí)際操作，感知能力的提升。制作網(wǎng)球識(shí)別撿球機(jī)器人系統(tǒng)，真正的可以解放雙手。通過對(duì)系統(tǒng)的設(shè)計(jì)和完善，最終完成的網(wǎng)球識(shí)別撿球機(jī)器人預(yù)期有如下成果：1.圖像識(shí)別模塊完成網(wǎng)球的目標(biāo)識(shí)別,不漏球；2.使用紅外傳感器實(shí)現(xiàn)循跡；超聲波傳感器用來進(jìn)行測(cè)距，以及移動(dòng)中的避障。3.網(wǎng)球抓取，利用圖像信息的獲取，機(jī)械臂完成抓取命令。4.無線通信，實(shí)現(xiàn)人機(jī)交互可實(shí)現(xiàn)遠(yuǎn)程控制。本文對(duì)網(wǎng)球的識(shí)別、網(wǎng)球的抓取以及機(jī)器人的行動(dòng)過程進(jìn)行了分析研究，但是未來可以考慮對(duì)網(wǎng)球的輪廓和相應(yīng)的識(shí)別算法進(jìn)行進(jìn)一步的研究，以不斷提升拾取網(wǎng)球的效率，更加智能化。參考文獻(xiàn)[1]馬飛.一種多傳感器數(shù)據(jù)融合的智能撿球機(jī)器人結(jié)構(gòu)設(shè)計(jì)[J].內(nèi)蒙古師范大學(xué)學(xué)報(bào)，2017,46（04）:517-519+523.[2]樊東,陳津徽,張?jiān)?機(jī)器視覺技術(shù)在工業(yè)中的應(yīng)用[J].科技風(fēng),2020（13）.[3]周旭,周海波，陳睿等.基于雙目視覺的機(jī)械手撿球機(jī)器人設(shè)計(jì)[J].機(jī)床與液壓,2020(03).[4]張文勇,張儒良,李云松.基于視覺分析的智能拾網(wǎng)球機(jī)器人研究[J].電子測(cè)試,2020（02）.[5]鄭植,席先鵬,王楠等.機(jī)器視覺在網(wǎng)球撿球機(jī)器人中的應(yīng)用研究[J].科技創(chuàng)新與應(yīng)用,2019(16).[6]朱珂皓.基于上位機(jī)視覺的自搜尋撿網(wǎng)球機(jī)器人[J].工業(yè)控制計(jì)算機(jī).2018（10）.[7]張幃.視覺分揀機(jī)器人系統(tǒng)設(shè)計(jì)與實(shí)現(xiàn)[D].黑龍江大學(xué),2019.[8]宿增迪.基于視覺識(shí)別的智能撿網(wǎng)球機(jī)器人開發(fā)[D].大連海事大學(xué).2018.[9]WangYong,HanZunpu.Antcolonyoptimizationfortravelingsalesmanproblembasedonparametersoptimization[J].AppliedSoftComputing,2021.[10]VasconcelosFrancisco,BarretoJoaoP,BoyerEdmond.AutomaticCameraCalibrationUsingMultipleSetsofPairwiseCorrespondences.[J]IEEEtransactionsonpatternanalysisandmachineintelligence,2018(4)[11]BolinCai,YuweiWang,KeyiWang,MengchaoMa,XiangchengChen.CameraCalibrationRobusttoDefocusUsingPhase-ShiftingPatterns[J].Sensors,2017(10)[12]楊青.Halcon機(jī)器視覺算法原理與編程實(shí)戰(zhàn)[M].北京大學(xué)出版社,2019.[13]李亞龍.改進(jìn)量子蟻群算法的研究及應(yīng)用[D].安徽大學(xué),2014.[14]OleksandrSemeniuta.AnalysisofCameraCalibrationwithRespecttoMeasurementAccuracy[J].ProcediaCIRP.2016.[15]范昭君.\o"智能控制在自動(dòng)撿球機(jī)器人中的應(yīng)用"智能控制在自動(dòng)撿球機(jī)器人中的應(yīng)用[J].\o"自動(dòng)化技術(shù)與應(yīng)用"自動(dòng)化技術(shù)與應(yīng)用,\o"2020(05)"2020(05).[16]陳衛(wèi);鄧志良.\o"撿球機(jī)器人路徑規(guī)劃算法研究及仿真"撿球機(jī)器人路徑規(guī)劃算法研究及仿真[J].\o"計(jì)算機(jī)仿真"計(jì)算機(jī)仿真,\o"2020(10)"2020(10).[17]徐霽;沈茜.\o"基于Android控制的乒乓球撿拾機(jī)器人設(shè)計(jì)"基于Android控制的乒乓球撿拾機(jī)器人設(shè)計(jì)[J].\o"信息與電腦(理論版)"信息與電腦(理論版),\o"2019(05)"2019(05).[18]陳青華;左太虎;徐一紅.\o"基于OpenMV的智能撿球機(jī)器人的設(shè)計(jì)與實(shí)現(xiàn)"基于OpenMV的智能撿球機(jī)器人的設(shè)計(jì)與實(shí)現(xiàn)[J].\o"信息技術(shù)與信息化"信息技術(shù)與信息化,\o"2020(10)"2020(10).[19]章明干.\o"AI撿球機(jī)器人"AI撿球機(jī)器人[J].\o"中國科技教育"中國科技教育,\o"2021(07)"2021(07).[20]BakerL,MillsS,LanglotzT,etal.PowerlinedetectionusingHoughtransformandlinetracingtechniques[C]//InternationalConferenceonImageandVisionComputingNewZealand.IEEE,2017:1-6.[21]NINOPEREIRA,FERNANDORIBEIRO,GILLOPESI,DANIELWHITNEY,JORGELINO.AutonomousGolfBallPickingRobotDesignandDevelopment[J].IndustralRobot:AnInternationalJournal.2012,(39).

附錄主程序importsensor,time,image,lcd,mathfrompybimportServo,UART,Pin,TimerfrompidimportPID#主體功能：#視覺追蹤小車：藍(lán)牙控制小車，循跡，目標(biāo)跟隨（顏色/人臉識(shí)別跟隨），避障（循跡+目標(biāo)識(shí)別避障，暫時(shí)用來做舵機(jī)控制指令）#機(jī)械臂自動(dòng)搬運(yùn)小車：藍(lán)牙控制小車，循跡目標(biāo)識(shí)別自動(dòng)搬運(yùn)，目標(biāo)自動(dòng)跟隨撿球，機(jī)械臂控制#藍(lán)牙協(xié)議：#藍(lán)牙數(shù)據(jù)發(fā)送規(guī)則：操作按鈕按下發(fā)送數(shù)據(jù)，抬起發(fā)送0x00，#控制模式切換方式：按一下選中功能，再按一次退出功能，如按下循跡，進(jìn)入自動(dòng)循跡功能，再按一下循跡就退出到手動(dòng)控制#自動(dòng)搬運(yùn)小車數(shù)據(jù)協(xié)議：0x00停止;0x01前進(jìn)（機(jī)械臂向上）;0x02后退（機(jī)械臂向下）；0x03左轉(zhuǎn)（機(jī)械臂向左）；0x04右轉(zhuǎn)（機(jī)械臂向右）；#0x05循跡（自動(dòng)搬運(yùn)）；0x06跟隨（自動(dòng)撿球）；0x07機(jī)械臂控制（上下左右按鈕用來控制機(jī)械臂）；0x08釋放；0x09抓取#小車狀態(tài)全局變量#定義小車狀態(tài)數(shù)據(jù)結(jié)構(gòu)classCARSTATE(object):enSTOP=0enRUN=1enBACK=2enLEFT=3enRIGHT=4enRELEASE=5enCATCH=6enMANUAL=7enTRACING=8enTRAKING=9enAVOIDING=10#globalg_carstate,g_cardir,power,ball_sg_carstate=CARSTATE.enTRAKINGg_cardir=CARSTATE.enSTOPpower=800ball_s=0#串口uart1=UART(1,9600)#PA9,PA10#globalbluetooth_stutas,key_state,key_event,G_FLAGbluetooth_stutas=0G_FLAG=0key_state=0key_event=0key=Pin('D8',Pin.IN,Pin.PULL_UP)THRESHOLD=[(40,96,-9,-38,20,57)]#黃綠色網(wǎng)球#THRESHOLD=[(53,99,-13,46,29,57)]#黃色乒乓球##生成50HZ方波，使用TIM3，channels1,2#控制舵機(jī)需要輸出2.5%-12.5%的PWM#0度0.5/20*100=2.5;(要看實(shí)際的舵機(jī)是否支持0度，一般運(yùn)動(dòng)不到0度，需要把這個(gè)值調(diào)大一點(diǎn))#45度1.0/20*100=5;#90度1.5/20*100=7.5;#135度2.0/20*100=10;#180度2.5/20*100=12.5;#生成50HZ方波#控制舵機(jī)需要輸出2.5%-12.5%的PWMtim=Timer(2,freq=50)#Frequencyin50Hz(20ms)claw=tim.channel(3,Timer.PWM,pin=Pin("A2"),pulse_width_percent=7.5)#初始角度90度globalclaw_offset#定義機(jī)械爪中心和攝像頭圖像中心偏移位置claw_offset=(-25)claw_release=140claw_catch=10count=0#控制舵機(jī)需要輸出2.5%-12.5%的PWMpan_servo=Servo(1)#左右控制tilt_servo=Servo(2)#上下控制#PID云臺(tái)PID,小車跟隨PID，小車循跡PIDpan_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.9,i=0.01)len_pid=PID(p=0.6,i=0.01)############################電機(jī)驅(qū)動(dòng)部分#############################如果上電時(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)#定義機(jī)械爪舵機(jī)運(yùn)動(dòng)角度#輸入：舵機(jī)運(yùn)動(dòng)到目標(biāo)角度#輸出：無defclaw_angle(servo_angle):ifservo_angle<=0:servo_angle=0ifservo_angle>=180:servo_angle=180percent=(servo_angle+45)/18claw.pulse_width_percent(percent)#定義舵機(jī)運(yùn)動(dòng)#輸入：舵機(jī)運(yùn)動(dòng)的起止角度#輸出：無defservo_move(servo_n,start_angle,end_angle):ifstart_angle>=end_angle:while(start_angle>=end_angle):servo_n.angle(start_angle-2)time.sleep(50)start_angle=start_angle-2return0ifstart_angle<=end_angle:while(start_angle<=end_angle):servo_n.angle(start_angle+2)time.sleep(50)start_angle=start_angle+2return0#功能：小車控制#輸入：左，右速度#輸出：無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))#功能：尋找最大的目標(biāo)，計(jì)算方式為像素面積排序#輸入：鏈表objects，坐標(biāo)X,坐標(biāo)y,像素寬,像素高#輸出：最大目標(biāo)元組deffind_max_object(objects):max_size=0forobjectinobjects:ifobject[2]*object[3]>max_size:max_object=objectmax_size=object[2]*object[3]returnmax_objectdefclaw_auto_catch(img,ball_s,max_blob):globalcountpan_error=0tilt_output=0title_angle=0pan_error=img.width()/2-max_blob.cx()tilt_error=(img.height()/2-claw_offset)-max_blob.cy()print("pan_error:",pan_error)#pan_output=pan_pid.get_pid(pan_error,1)/2tilt_output=tilt_pid.get_pid(tilt_error,1)#關(guān)閉左右追蹤title_angle=tilt_servo.angle()-tilt_output#pan_servo.angle(pan_servo.angle()+pan_output)tilt_servo.angle(title_angle)############################抓取#############################抓取得策略是攝像頭與夾子呈一定角度安裝，大概45度，讓一半得夾子在圖像中，目的是方便判斷目標(biāo)是否一直在夾子上#然后修訂目標(biāo)在夾子中得位置，當(dāng)修訂好之后再執(zhí)行夾取動(dòng)作。if(ball_s>=60andball_s<=100):run(0,0)count=count+1ifcount>=3:count=0run(0,0)servo_move(tilt_servo,tilt_servo.angle(),20)claw_angle(claw_catch)#夾取time.sleep(1000)servo_move(tilt_servo,tilt_servo.angle(),-20)#抬起time.sleep(1000)run(60,60)#前進(jìn)time.sleep(3000)run(0,0)servo_move(tilt_servo,tilt_servo.angle(),10)#向下time.sleep(1000)claw_angle(claw_release)#釋放time.sleep(1000)run(-60,-60)#后退time.sleep(4000)run(0,0)if(ball_s>100)and(ball_s<800):############################云臺(tái)追蹤############################count=0claw_angle(claw_release)#############################小車追蹤############################power=0power_s=0power_l=0#前進(jìn)與后退控制，#沒有做后退，而是采用先大致接近目標(biāo)，再修訂距離得方式#前進(jìn)與后退控制，這里采用的是像素面積計(jì)算距離，而抓取采用的是距離參數(shù)，#兩者可以統(tǒng)一一下，具體可以根據(jù)實(shí)際物體大小測(cè)定后調(diào)整ifball_s>300andball_s<=800:dis_error=ball_s-300power_s=int(dis_pid.get_pid(dis_error,1)/2)if(power_s>100):power_s=100if(power_s<60):power_s=60elifball_s>100andball_s<=300:dis_error=ball_s-100power_s=int(dis_pid.get_pid(dis_error,1)/2)#電機(jī)PWM限幅，防止最后一點(diǎn)距離走不到if(power_s<60):power_s=60else:power_s=0power_l=int(len_pid.get_pid(pan_error,1))print("power_s:",power_s)print("power_l:",power_l)run(power_s-power_l,power_s+power_l)defcolor_detect(img):globallast_objects,objectsball_s=0iflast_objects:#如果上一張圖像中找到了色塊，就在色塊周邊擴(kuò)展，然后在擴(kuò)展后的ROI區(qū)域進(jìn)行尋找色塊，這樣有利于追蹤forbinobjects:x1=b[0]-7y1=b[1]-7w1=b[2]+12h1=b[3]+12#print(b.cx(),b.cy())roi2=(x1,y1,w1,h1)#print(roi2)objects=img.find_blobs(THRESHOLD,roi=roi2,area_threshold=200)last_objects=objectselse:#如果沒有找到色塊，就重新計(jì)算全圖的色塊，并將全圖色塊信息保存到上一張圖像上，這樣下一次就可以再進(jìn)行ROI區(qū)域?qū)ふ疑珘Kobjects=img.find_blobs(THRESHOLD,area_threshold=200)last_objects=objectsifobjects:#如果找到了色塊，就計(jì)算最大的色塊位置，并做舵機(jī)追蹤max_blob=find_max_object(objects)#色塊的位置和大小需要滿足圖像本身大小的要求，以免越界ball_s=19500/(max_blob[2]+max_blob[3])#計(jì)算距離img.draw_rectangle(max_blob.rect())#rect目標(biāo)區(qū)域畫矩形img.draw_cross(max_blob.cx(),max_blob.cy())#cx,cy畫中心點(diǎn)img.draw_string(max_blob.cx(),max_blob.cy(),"%.2fmm"%(ball_s))#顯示目標(biāo)的距離參數(shù)claw_auto_catch(img,ball_s,max_blob)else:run(0,0)if(tilt_servo.angle()<=10):tilt_servo.angle(10)else:tilt_servo.angle(tilt_servo.angle())claw_angle(claw_release)defbluetooth_deal(img):globalbluetooth_stutas,G_FLAG,g_cardir,g_carstateifuart1.any():rec=uart1.read(1)ifrec!=None:rec=bytes(rec)print(rec)ifrec==b'\x00':#停車g_cardir=CARSTATE.enSTOPelifrec==b'\x01':#前進(jìn)g_cardir=CARSTATE.enRUNelifrec==b'\x02':#后退g_cardir=CARSTATE.enBACKelifrec==b'\x03':#左轉(zhuǎn)g_cardir=CARSTATE.enLEFTelifrec==b'\x04':#右轉(zhuǎn)g_cardir=CARSTATE.enRIGHTelifrec==b'\x05':#循跡g_carstate=CARSTATE.enTRACINGelifrec==b'\x06':#跟隨g_carstate=CARSTATE.enTRAKINGelifrec==b'\x07':#避障#按兩次就回到手動(dòng)模式bluetooth_stutas=bluetooth_stutas+1ifbluetooth_stutas==1:g_carstate=CARSTATE.enAVOIDINGG_FLAG=1ifbluetooth_stutas==2:g_carstate=CARSTATE.enMANUALG_FLAG=0bluetooth_stutas=0elifrec==b'\x08':#app上是關(guān)燈按鈕，自定義釋放g_cardir=CARSTATE.enRELEASEelifrec==b'\x09':#app上是開燈，自定義抓取g_cardir=CARSTATE.enCATCHelifrec==b'\xF0':#一鍵切換到手動(dòng)模式g_carstate=CARSTATE.enMANUALBST_fLeftMotorOut,BST_fRightMotorOut,g_power=0,0,800if(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):