網球底線區(qū)域智能機器人系統(tǒng)設計

[9]內部特意設置了過熱保護與低壓檢測電路，PWM控制的頻率可高達100kHz121。如圖3.5所示。圖3.5電機H橋驅動部分電路3.7藍牙電路設計本設計通過藍牙將數據發(fā)送至手機，同時手機也可以發(fā)送指令到單片機進行控制。HC-05是一款經典的藍牙2.0模塊，該模塊具有性價比高、體積小巧、功耗低、收發(fā)數據穩(wěn)定的特點，其內部的芯片支持SPP藍牙協(xié)議。HC-05藍牙的實物圖如圖3.6所示，其具體的參數如下：(1)供電電壓：3.6V-6V，工作電流：10-50mA。(2)通信頻段：2.4GHz。(3)通信接口：TTL異步串口，默認波特率為9600。(4)配置方式：AT通信指令。圖3.6HC-05藍牙模塊實物圖HC-05藍牙模塊具有6個對外接口，這是設計藍牙通信電路的基礎，下面對模塊的通信接口進行介紹：(1)VCC/GND:模塊工作供電引腳，本設計使用的5V電源供電，供電的電流為500mA。(2)通過連接RX/TX模塊的串口輸入/輸出接口與單片機的串口，實現(xiàn)了數據通信的無縫銜接。(3)STATE：用于輸出系統(tǒng)工作狀態(tài)的模塊狀態(tài)指示引腳，以指示系統(tǒng)的運行狀態(tài)(4)ENB:模塊使能引腳，使能通過AT指令進行控制，當ENB接3.3V后，模塊進入AT配置模式。圖3.7是HC-05電路原理圖，圖中U30是HC-05藍牙模塊，模塊的VCC和GND與板子的5V連接。HC-05模塊的RX連接單片機的PA9口，HC-05的TX連接單片機的PA10口。圖3.7HC-05電路原理圖3.8收集裝置我設計的撿球機器人主要是由舵機與小框組成。當攝像頭識別到網球時，機械臂會自動將網球拾取進小框內。舵機電路如圖3.8所示。圖3.8舵機電路3.9處理器系統(tǒng)撿球機器人不光要看見識別網球更主要的是要讓他具有一定的自我選擇的權力，必須具有清晰的頭腦，所以我們給他安裝了較為強大的處理系統(tǒng)，能夠更加準確的識別網球從而進一步撿球。一般的處理系統(tǒng)由微控制器如DSP、單片機、ARM等組成。在撿球機器人的處理過程中，機器人的處理器系統(tǒng)包含了上機位和下位機。上機位可以更加清楚的收集數據和信息，從而更進一步的將控制指令發(fā)送到我們的運動系統(tǒng)上。軟件采用了對網球識別的圖像顯示。通過對撿球機器人的進一步處理（如上下移動、左右轉彎、機械臂撿球等）和一些傳感器的進一步處理和計算。通過串行連接等方式從而進行通信。3.10傳感器系統(tǒng)撿球機器人在對場上進行獲取到所有情況，故而我安裝了傳感器來獲取信息。對機器人的內部結構來說，傳感器是必不可上的一個重要組成部分。傳感器系統(tǒng)分別由內部傳感器和外部傳感器組成。內部傳感器我們通過編程來解決，外部傳感器是有視覺傳感器的重要組成部分。在機器人不清楚場上的情況時，我們通過視覺傳感器來獲取相應的信息。我設計的撿球機器人帶有一個攝像頭，用于進行的信息收集；兩個增量型光電編碼器，主要用于驅動輪進行速度檢測。2.11執(zhí)行機構對于我設計的這個撿球機器人來說，要完成我交給他的任務就必須有自我的執(zhí)行能力，撿球機器人的執(zhí)行方式主要由運動結構和撿球結構組成，目前的帶有輪轂的機器人有很多，他們一般都裝有一個電機，另一種就是有兩個輪轂的，它們是通過驅動電機來控制的一個點機用來轉動，另一個電機用來控制方向，而我設計的這個是通過履帶來控制的。第4章系統(tǒng)軟件設計4.1系統(tǒng)軟件總體設計本機器人由全金屬外殼機械臂，純鋁合金底盤，金屬齒馬達，履帶輪，金屬電機，大手爪，電流驅動，移動電源等部位組成，我用于設計的這一款撿球機器人是通過攝像頭來進行分辨網球，在第一時間發(fā)現(xiàn)網球的時候會把網球的位置通過控制軟件來反饋，控制軟件會通過履帶來推動機器人的運動從而進一步的靠近目標，當機器人靠近網球時會通過機械臂來控制并拾取網球，從而通過機械臂放進事先準備好的框里。當完成第一次拾取后，機器人會繼續(xù)進行的同樣的運動從而實現(xiàn)撿球工作。如圖4.1所示。圖4.1系統(tǒng)總體結構框圖4.2測距避障功能為了實現(xiàn)對網球的測距，需要通過單片機向傳感測距模塊控制端發(fā)出10μs的高電平信號，隨后從接收端檢測信號是否返回。當在設定好的時間段內沒有接收到信號時，單片機就會產生一個低電平信號。若有，則向接收端發(fā)出高電平信號并激活定時器計算高電平持續(xù)時間，再通過公式(距離=(時間*聲速)/2,聲速340m/s)進一步計算撿網球機器人與障礙物的距離。對于網球進球機器人避障功能，提前由程序設計規(guī)則將安全距離定為10cm。如果前方障礙物的距離在10cm以內，則立刻停止行進，這時舵機驅動傳感測距模塊旋轉以測量左右兩個障礙物之間的距離，以選擇朝向距離大的方向旋轉；如左、右障礙物間距均在20cm以內，作后退處理。如圖4.2所示為測距避障流程。圖4.2測距避障流程4.3網球識別功能實現(xiàn)OpenMV4攝像頭識別圖像并轉為灰度的圖像，規(guī)定的閾值并切割圖像。通過降噪等預處理加強網球圖像的重要特征，然后提取網球的輪廓、長度、寬度、面積、顏色等各種特征值。采取圖像梯度的方法來獲取網球輪廓，分別產生二值圖的邊緣來提取圖提取網球面積和周長。采集網球圖片的素材制作數據集并訓練模型，使用網絡ResNet來作為基石，模型的訓練要用生成器來將數據集循環(huán)進而寫入內存，同時圖像增強以泛化模型REF_Ref19312\r\h[12]。通過訓練能夠得到一種網球的識別規(guī)則來進行網球識別，模塊內部的處理器經過分類來進行決策后就能夠進一步識別到網球，從而得出網球的具體位置，采用視差測距法來計算網球的距離REF_Ref19241\r\h[13]，然后進一步傳遞給單片機。如圖4.3所示為網球識別流程。圖4.3網球識別流程4.4控制電路總體結構設計我設計的網球機器人的控制電路整體結構設計包括以下幾個部分：1.控制器：控制器是控制電路的核心部分，它的功能是用于接收用戶的指令，從而控制各個電器元件的工作。控制器也可以選擇單片機或者嵌入式系統(tǒng)，可以通過對機器人的復雜度和功能要求選擇不同的控制器型號和配置。2.電源管理模塊：通過為整個電路提供電源，他們中也包括自主供電和外部供電兩種模式，進而滿足機器人運行所需的電源需求。在電源模塊中，還需要添加過壓保護、電流保護、電源開關、電池充電等一系列功能。3.傳感器：傳感器是通過機器人的重要的硬件。通過對傳感器，機器人可以來感受運動過程中他們的位置、速度、方向等信息。在網球機器人中，需要加入重量傳感器、距離傳感器、方向傳感器等。4.驅動模塊：驅動模塊是機器人運動過程中所必不可少的一部分，包括電機驅動、機械臂驅動和輪子驅動等功能。要實現(xiàn)機器人運動的平穩(wěn)性和精確性，需要使用高精度的電機，可以選擇步進電機，直流電機等。保護模塊：保護模塊是機器人安全保障的最后一道保護措施，可以設置緊急停止按鈕和電路保護器等。4.5藍牙模塊藍牙通信軟件子程序是通過串口進行透傳發(fā)送接收數據的，通過調用串口發(fā)送函數printf()發(fā)送數據到HC-05模塊，如果HC-05模塊接收到數據后，單片機通過串口中斷函數進行接收，接收的數據放到串口結束數據緩存數組USART_RX_BUF中，主程序通過對數組進行校驗，如果校驗正確擇根據數組解析的數據，進行相應的程序執(zhí)行，之后子程序返回。

第5章系統(tǒng)調試5.1軟件調試軟件調試是以硬件調試為根本進行的，需要保證硬件電路沒有問題，是調試過程中相對復雜的過程，很多問題不容易被儀器測試出來。軟件調試的思路與硬件調試思路完全不同。既需要語言語法基礎又需要一定的編程經驗。本設計采用的是C語言，調試程序第一步就是將C語言程序中的語法錯誤部解決好。本設計使用的Keil軟件進行程序編寫，在編寫編譯過程中會進行語法檢測，可以實現(xiàn)大部分語法問題的調試。但是程序的驅動和程序的執(zhí)行邏輯是無法通過軟件進行識別的，要結合測試現(xiàn)象、調試結果和設計者經驗進行一步一步的修改嘗試，直到使程序正常運行。其中常見的問題比有數組越界、內存混亂、子程序之間相互干擾等問題。這些問題需結合硬軟件調試工具進行逐步的探索和解決問題。在對IIC子程序進行調試時，通過軟件編譯以后，顯沒有錯誤可以正常進行下到單片機中，單片機運行程序以后出現(xiàn)了系統(tǒng)失靈，顯示錯誤，系統(tǒng)死機，無法執(zhí)行后邊的程序。經過斷開電源重新啟動之后仍然現(xiàn)前面的現(xiàn)象。Keil軟件提供了程序調試器和單步執(zhí)行功能。在出問題的子程序上，通過設置斷點，判斷每一步執(zhí)行以后每個變量的數值是多少，通過watch窗口可以看到出現(xiàn)問題的變量是哪個，再結合變量的實際位置，進定位出現(xiàn)問題的程序語句，實現(xiàn)程序的寄存器、變量等數據的監(jiān)測。在程序運行一個for循環(huán)語句的時候，指針變量訪問到了數組規(guī)定范圍以外的位置，發(fā)生了越界，數組定義的偏離位置最高位8位，但指針在訪問過程卻訪問到了第9位，將其他變量的數據進行了異常修改。導致執(zhí)行到這條語句以后，使程序崩潰，無法進行運行。程序整個內存中數據發(fā)亂，系統(tǒng)無法正常執(zhí)行，導致程序指令混亂死機，圖5.1為程序調試過程的截圖，通過調試解決了程序中存在的數組越界問題，使程序正常運行，達到了的預期的效果。圖5.1子程序仿真調試圖通過使用軟件內置的調試器發(fā)現(xiàn)了這個問題。這個問題的解決方案是對讀取數組過程中的偏移變量進行排查，防止越界讀寫數據。數組越界是很常見的軟件編寫錯誤，造成這種問題的原因可能是程序編寫人員在輸入的時候輸入了錯誤的計數值，也有可能是沒有確定好數組的具體范圍。本次設計出現(xiàn)了數組越界以后對程序其他代碼也進行了檢查，尤其是對使用循環(huán)語句訪問數組，進行重點關注，確保程序中其他位置不會出現(xiàn)類似的問題。5.2實物調試圖5.2是撿球機器人的實物圖。機器人上方通過機械臂的表面來安裝攝像頭與藍牙模塊，尾部由機械臂的底部來支撐所有的機械設備，從而保證各個模塊直接不會發(fā)生故障，進而可以更好的完成測試工作。通過測試，我設計撿球機器人可以通過攝像頭來精準的抓取網球，同時，在路線上有一定的障礙時，我們的機器人可以正確的判斷出正確的路線，從而實現(xiàn)自主避障。此外，我們可以通過藍牙模塊來連接手機，從而進行遠程操作。圖5.2是撿球機器人的實物圖圖5.3時撿球機器人自主避障的測試結果。通過表上的數據我們可以直觀的發(fā)現(xiàn)機器人的避障功能有了更好的提高，圖5.4為撿球率的測試。通過測試表明，我們的機器人在通過對光照的強度、網球的密度、網球的位置等環(huán)境方面的影響，進而我們進行了更加便利的改變。圖5.3自主避障的測試結果圖5.4為網球撿拾率測試結果隨機在場館放40個球進行多次實驗，來記錄機器人開始工作及工作后的時間、拾球個數，測試結果如圖8所示．場館網球數越多，機器人移動速度就會越慢，隨著地上網球數量的逐步減少，網球機器人的移動速度會進一步增加．實驗結果來表明拾球機器人抓取精度能達到97%．如圖5.5所示。圖5.5測試結果第6章總結與展望由于目前市面上的智能網球收集機器人還是寥寥無幾，已經有的設備大多是簡單的輔助撿球器，而不能實現(xiàn)高效自主收集網球，對于正在訓練的運動員和業(yè)余愛好者來說，拾取散落在球場上的網球是繁重的體力勞動，雇傭球童費用又較高。針對拾球難的問題，進而對網球收集機器人的關鍵技術研究，試圖研制一款基于視覺的智能網球收集機器人，代替人工實現(xiàn)自主收集網球。網球底線區(qū)域智能撿球機器人系統(tǒng)設計雖然已有一定的研究成果，但仍然存在一些問題和挑戰(zhàn)。以下是一些主要的問題和發(fā)展趨勢：1.精度問題：當前技術雖然可以精確識別網球的位置，但在撿球過程中機器人的機械臂和抓取器的精度還存在一定的問題，撿球不夠靈活和準確。2.移動速度問題：機器人在撿球過程中需要快速移動，否則會影響比賽的進程，但目前機器人的移動速度還不能達到專業(yè)選手和人類的速度。目前智能撿球機器人的研發(fā)和制造成本較高，需要更多的技術創(chuàng)新和成本優(yōu)化。未來，智能撿球機器人的發(fā)展趨勢將有以下幾個方面：1.技術創(chuàng)新：機器人技術將不斷創(chuàng)新和完善，更加精準的識別技術、更高效的移動速度或更靈活的機器臂等新技術將不斷涌現(xiàn)。2.應用拓展：初步的研究表明，智能撿球機器人不僅可以在網球比賽中使用，還可以在其他球類和運動比賽中使用，例如，足球、籃球和棒球等。3.集成化應用：隨著人工智能、云計算和大數據技術的發(fā)展，智能撿球機器人將與其他解決方案集成，為運動員和球迷提供更多智能服務，如實時數據分析、球員排名和比賽回放等。綜上所述，雖然智能撿球機器人的研發(fā)還面臨許多問題和挑戰(zhàn)，但其發(fā)展前景廣闊，具有無限的潛力和探索空間。網球底線區(qū)域智能撿球機器人系統(tǒng)設計有以下幾個發(fā)展趨勢：1.多功能化：未來的智能撿球機器人將不僅僅是處理網球的撿球任務，而是要具備更多的功能，例如，可以幫助球員訓練、提供數據分析和反饋等，這將為球員提供更好的訓練和比賽體驗。2.自主導航技術的提高：自主導航技術是智能撿球機器人實現(xiàn)自主行動的核心技術，未來的智能撿球機器人將具備更加先進的自主導航技術，可以在不同的場地和環(huán)境下，更加靈活地完成任務。3.環(huán)?；涂沙掷m(xù)性：環(huán)?；涂沙掷m(xù)性是未來機器人發(fā)展的趨勢，未來的智能撿球機器人將越來越注重環(huán)保，采用更加環(huán)保和節(jié)能的技術，同時具備更長的使用壽命，減少對環(huán)境的影響。4.互聯(lián)網+：智能撿球機器人將與互聯(lián)網技術緊密結合，未來可以通過云計算和大數據技術，實現(xiàn)數據的共享和分析，打造更加智能化、集成化的服務系統(tǒng)，為用戶提供更好的服務和體驗。5.個性化和定制化：未來的智能撿球機器人將越來越注重個性化和定制化，可以根據不同用戶的需求和場地環(huán)境進行個性化定制，提供更加適合的服務和解決方案。綜上所述，未來網球底線區(qū)域智能撿球機器人系統(tǒng)設計將不斷發(fā)展和進步，不僅具備更高的精度、速度和自主性，還將具備更多的功能。參考文獻段元奕,劉家博,沈怡平.基于嵌入式系統(tǒng)的撿球機器人設計[J件,2022,43(05):162-165.覃靜,孫霞.基于STM32單片機的撿球機器人設計[J].計算機與網絡,2021,47(22):60-62.馬飛.一種智能撿球機器人的多傳感器數據采集系統(tǒng)設計[J].信息技術與信息化,2021(09):243-245.章明干.AI撿球機器人[J].中國科技教育,2021(07):64-65.任云青.智能乒乓球自動撿球機器人的設計與實現(xiàn)[D].南京郵電大學,2020.陳青華,左太虎,徐一紅.基于OpenMV的智能撿球機器人的設計與實現(xiàn)[J].信息技術與信息化,2020(10):210-213.陳章寶,侯勇.網球撿球機器人視覺導航系統(tǒng)設計[J].廊坊師范學院學報(自然科學版),2020,20(02):42-45.劉永兵.基于SoC的羽毛球撿拾機器人設計與實現(xiàn)[D].重慶大學,2020.普邑,楊哲,王新勇.基于OpenMV的撿球機器人系統(tǒng)設計[J].山西電子技術,2020(01):70-73.陳衛(wèi),鄧志良.撿球機器人路徑規(guī)劃算法研究及仿真[J].計算機仿真,2020,37(10):291-296.彭磊.視覺導航的自主撿球機器人設計與開發(fā)[D].東南大學,2019.DOI:10.27014/ki.gdnau.2019.003512.鄭植,席先鵬,王楠,蔣欣燎,夏山淋.機器視覺在網球撿球機器人中的應用研究[J].科技創(chuàng)新與應用,2019(16):34-35.馬天月.基于ESP8266WiFi芯片的物聯(lián)網模塊的設計與實現(xiàn)[D].上海：上海交通大學，2017.蓋榮麗，蔡建榮，王詩宇，等.卷積神經網絡在圖像識別中的應用研究綜述[J].小型微型計算機系統(tǒng)，2021，42（9）：1980-1984.姜映舟.基于計算機視覺的測距技術研究[D].桂林：桂林電子科技大學，2019.黃玉金，楊越，薛偉，等.無線模塊的AT指令UDP透傳設計[J].電子產品世界，2018，25（1）：34-36.LiJiaxin.DesignofIntelligentDaylilyPickingRobot[J].HanJinghai.MultimediaMotionofPickingRobotBasedonCooperativeRelationshipofMatchingGradientAlgorithm[J].AdvancesinMultimedia,2021,2021.TangZuoliang,XuLijia,WangYuchao,KangZhiliang,XieHong.Collision-FreeMotionPlanningofaSix-LinkManipulatorUsedinaCitrusPickingRobot[J].AppliedSciences,2021,11(23).Zhang,Huan,Yu,Li.Cantilevermulti-axiscontrolmethodforanewarmpickingrobot[J].InternationalJournalofSystemAssuranceEngineeringandManagement,2021(prepublish).LiuPeng,QiaoXinzhou,ZhangXuhui.Stabilitysensitivityforacable-basedcoal–ganguepickingrobotbasedongreyrelationalanalysis[J].InternationalJournalofAdvancedRoboticSystems,2021,18(6).Miki,Kohei,Nagata,Fusaomi,Ikeda,Takeshi,Watanabe,Keigo,Habib,MakiK..Moldedarticlepickingrobotusingimageprocessingtechniqueandpixel-basedvisualfeedbackcontrol[J].ArtificialLifeandRobotics,2021,26(4).TheveninSimon,MebarkiNasser,ChatellierPauline.Dynamicschedulingofapickingrobotwithlimitedbufferandrejection:anindustrialcasestudy[J].InternationalTransactionsinOperationalResearch,2021,29(3).附錄程序#Untitled-By:mengfei-周一2月132023importsensor,time,image,lcd,mathfrompybimportServo,UART,Pin,TimerfrompidimportPID#自動搬運小車數據協(xié)議：0x00停止;0x01前進（機械臂向上）;0x02后退（機械臂向下）；0x03左轉（機械臂向左）；0x04右轉（機械臂向右）；#0x05循跡（自動搬運）；0x06跟隨（自動撿球）；0x07機械臂控制（上下左右按鈕用來控制機械臂）；0x08釋放；0x09抓取#小車狀態(tài)全局變量#定義小車狀態(tài)數據結構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)]#黃綠色網球#THRESHOLD=[(53,99,-13,46,29,57)]#黃色乒乓球##生成50HZ方波，使用TIM3，channels1,2#控制舵機需要輸出2.5%-12.5%的PWM#0度0.5/20*100=2.5;(要看實際的舵機是否支持0度，一般運動不到0度，需要把這個值調大一點)#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方波#控制舵機需要輸出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#定義機械爪中心和攝像頭圖像中心偏移位置claw_offset=(-20)claw_release=160claw_catch=10count=0#控制舵機需要輸出2.5%-12.5%的PWMpan_servo=Servo(1)#左右控制tilt_servo=Servo(2)#上下控制#PID云臺PID,小車跟隨PID，小車循跡PIDpan_pid=PID(p=0.2,i=0,imax=90)#脫機運行或者禁用圖像傳輸，使用這個PIDtilt_pid=PID(p=0.15,i=0,imax=90)#脫機運行或者禁用圖像傳輸，使用這個PIDdis_pid=PID(p=0.9,i=0.01)len_pid=PID(p=0.6,i=0.01)#Untitled-By:mengfan_zheng-周日5月82022############################電機驅動部分#############################如果上電時出現(xiàn)電機異常運轉，是因為PB4引腳默認為JTAG功能，更新到最新日期的固件可解決這個問題pwmA1=Pin('B0')#TIM3_CH3pwmA2=Pin('B1')#TIM3_CH4pwmB1=Pin('B4')#TIM3_CH1pwmB2=Pin('B5')#TIM3_CH2#定義定時器3，頻率為1000Hz，即1ms一次定時器中斷tim=Timer(3,freq=1000)#初始化PWM，通過調節(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)#定義機械爪舵機運動角度#輸入：舵機運動到目標角度#輸出：無defclaw_angle(servo_angle):ifservo_angle<=0:servo_angle=0ifservo_angle>=180:servo_angle=180percent=(servo_angle+45)/18claw.pulse_width_percent(percent)#定義舵機運動#輸入：舵機運動的起止角度#輸出：無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))#功能：尋找最大的目標，計算方式為像素面積排序#輸入：鏈表objects，坐標X,坐標y,像素寬,像素高#輸出：最大目標元組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)#關閉左右追蹤title_angle=tilt_servo.angle()-tilt_output#pan_servo.angle(pan_servo.angle()+pan_output)tilt_servo.angle(title_angle)############################抓取#############################抓取得策略是攝像頭與夾子呈一定角度安裝，大概45度，讓一半得夾子在圖像中，目的是方便判斷目標是否一直在夾子上#然后修訂目標在夾子中得位置，當修訂好之后再執(zhí)行夾取動作。if(ball_s>=60andball_s<=100):run(0,0)count=count+1ifcount>=3:count=0run(0,0)claw_angle(claw_catch)#夾取time.sleep(1000)servo_move(tilt_servo,tilt_servo.angle(),45)#抬起time.sleep(1000)run(60,60)#前進time.sleep(3000)run(0,0)servo_move(tilt_servo,tilt_servo.angle(),60)#向下time.sleep(1000)claw_angle(claw_release)#釋放time.sleep(1000)run(-60,-60)#后退time.sleep(5000)run(0,0)if(ball_s>100)and(ball_s<800):############################云臺追蹤############################count=0claw_angle(claw_release)#############################小車追蹤############################power=0power_s=0power_l=0#前進與后退控制，#沒有做后退，而是采用先大致接近目標，再修訂距離得方式#前進與后退控制，這里采用的是像素面積計算距離，而抓取采用的是距離參數，#兩者可以統(tǒng)一一下，具體可以根據實際物體大小測定后調整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)#電機PWM限幅，防止最后一點距離走不到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:#如果上一張圖像中找到了色塊，就在色塊周邊擴展，然后在擴展后的ROI區(qū)域進行尋找色塊，這樣有利于追蹤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:#如果沒有找到色塊，就重新計算全圖的色塊，并將全圖色塊信息保存到上一張圖像上，這樣下一次就可以再進行ROI區(qū)域尋找色塊objects=img.find_blobs(THRESHOLD,area_threshold=200)last_objects=objectsifobjects:#如果找到了色塊，就計算最大的色塊位置，并做舵機追蹤max_blob=find_max_object(objects)#色塊的位置和大小需要滿足圖像本身大小的要求，以免越界ball_s=20000/(max_blob[2]+max_blob[3])#計算距離img.draw_rectangle(max_blob.rect())#rect目標區(qū)域畫矩形img.draw_cross(max_blob.cx(),max_blob.cy())#cx,cy畫中心點img.draw_string(max_blob.cx(),max_blob.cy(),"%.2fmm"%(ball_s))#顯示目標的距離參數claw_auto_catch(img,ball_s,max_blob)else:run(0,0)if(tilt_servo.angle()<=45):tilt_servo.angle(45)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':#前進g_cardir=CARSTATE.enRUNelifrec==b'\x02':#后退g_cardir=CARSTATE.enBACKelifrec==b'\x03':#左轉g_cardir=CARSTATE.enLEFTelifrec==b'\x04':#右轉g_cardir=CARSTATE.enRIGHTelifrec==b'\x05':#循跡g_carstate=CARSTATE.enTRACINGelifrec==b'\x06':#跟隨g_carstate=CARSTATE.enTRAKINGelifrec==b'\x07':#避障#按兩次就回到手動模式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上是關燈按鈕，自定義釋放g_cardir=CARSTATE.enRELEASEelifrec==b'\x09':#app上是開燈，自定義抓取g_cardir=CARSTATE.enCATCHelifrec==b'\xF0':#一鍵切換到手動模式g_carstate=CARSTATE.enMANUALBST_fLeftMotorOut,BST_fRightMotorOut,g_power=0,0,800if(g_carstate==CARSTATE.enMANUAL):#手動控制小車if(g_cardir==CARSTATE.enSTOP):BST_fLeftMot