基于AVR單片機(jī)碼垛機(jī)器人設(shè)計(jì)目錄TOC\o"1-3"\h\u7155第一章緒論 461871.1課程設(shè)計(jì)主要目的與任務(wù) 4281371.2課程設(shè)計(jì)要求 4189281.3課程設(shè)計(jì)的意義 410435第二章硬件設(shè)計(jì) 6213802.1硬件電路的系統(tǒng)概述 6240022.2主芯片 7161172.3多自由度機(jī)械手 9320701.認(rèn)識(shí)角度舵機(jī) 9142142.零點(diǎn)定義 1062403.姿態(tài)調(diào)試 10147094.機(jī)械手應(yīng)用 12296945.角度舵機(jī)與Atmega328p的接線 13289192.4伺服電機(jī) 1388281.認(rèn)識(shí)伺服電機(jī) 13189622.伺服電機(jī)的調(diào)零 13160183.伺服電機(jī)與Atmega328p的接線 14572.5QTI傳感器 14314921.認(rèn)識(shí)QTI傳感器 14132692.QTI傳感器的引腳定義 1452433.QTI傳感器的性能參數(shù) 15320404.QTI傳感器與Atmega328p的接線 15244292.6顏色傳感器 15284581.認(rèn)識(shí)TCS230顏色傳感器 15210622.TCS230傳感器的引腳定義 16108283.TCS230傳感器的特點(diǎn) 16241084.TCS230顏色傳感器的工作原理 16295425.白平衡 17130786.TCS230顏色傳感器與Atmega328p的接線 18327、TCS230顏色傳感器測(cè)試 1856672.7超聲波傳感器 19144331.認(rèn)識(shí)超聲波傳感器 19209332.超聲波傳感器的工作原理 19159863.超聲波傳感器與Atmega328p的接線 2025163第三章軟件設(shè)計(jì) 21153103.1項(xiàng)目的主流程圖 2161593.2Arduino語言及它專門定義的函數(shù) 22248653.3顏色傳感器的檢測(cè)程序 24255603.4QTI傳感器的檢測(cè)程序 2570753.5超聲波傳感器的檢測(cè)程序 2626465附錄1實(shí)物圖 2720983附錄2程序 294941、主程序 28197502、#include"MsTimer2.h" 10192753、顏色傳感器的應(yīng)用程序 105221434、QTI傳感器的應(yīng)用 11198775、機(jī)械手的應(yīng)用程序 11264936、伺服電機(jī)的應(yīng)用程序 12043137、QTI傳感器的應(yīng)用程序 120第一章緒論1.1課程設(shè)計(jì)主要目的與任務(wù)本課題是基于AVR單片機(jī)碼垛機(jī)器人設(shè)計(jì)，主要是硬件電路的設(shè)計(jì)和軟件電路的設(shè)計(jì)。主要的目的就是掌握ATmega328p處理器為核心的Openduino控制板中硬件電路的設(shè)計(jì)方法；單片機(jī)軟件程序的編寫和調(diào)試方法；軟件Eclipse、串口調(diào)試助手、OpenMachineHand的使用方法。本課題的主要任務(wù)就是通過傳感器的使用，設(shè)計(jì)一個(gè)智能的搬運(yùn)機(jī)器人。主要的任務(wù)是通過各種傳感器的使用使得機(jī)器人更加智能化。劃分為這幾個(gè)模塊：1、能夠沿規(guī)定的路線或者尋跡直行；2、機(jī)械手臂能夠抓去物塊；3、能夠識(shí)別出物塊的顏色；4、能夠測(cè)量物塊與自身的距離，并作出相應(yīng)的判斷；5、能夠把9個(gè)色塊，搬運(yùn)到目標(biāo)區(qū)域；搬運(yùn)完成能回到起始區(qū)域。1.2課程設(shè)計(jì)要求1、掌握ATmega328p處理器為核心的Openduino控制板等硬件電路設(shè)計(jì)方法。2、熟練掌握單片機(jī)軟件程序的編寫和調(diào)試方法。3、掌握軟件Eclipse、串口調(diào)試助手、OpenMachineHand的使用。1.3課程設(shè)計(jì)的意義本課程設(shè)計(jì)是培養(yǎng)學(xué)生綜合運(yùn)用所學(xué)理論知識(shí)、發(fā)現(xiàn)問題、提出問題、分析問題、解決實(shí)際問題、鍛煉實(shí)踐能力的重要環(huán)節(jié),是對(duì)學(xué)生實(shí)際工作能力的具體訓(xùn)練和考察過程。隨著科學(xué)技術(shù)發(fā)展的日新日異，單片機(jī)已經(jīng)成為當(dāng)今計(jì)算機(jī)應(yīng)用中空前活躍的領(lǐng)域，在生活中可以說得是無處不在。因此作為二十一世紀(jì)的大學(xué)來說掌握單片機(jī)的開發(fā)技術(shù)是十分重要的。回顧起此次課程設(shè)計(jì)，我仍感慨頗多，從選題到定稿，從理論到實(shí)踐，這幾個(gè)星期，可以說得是苦多于甜，但是可以學(xué)到很多很多的的東西，同時(shí)不僅可以鞏固了以前所學(xué)過的知識(shí)，而且學(xué)到了很多在書本上所沒有學(xué)到過的知識(shí)。通過這次課程設(shè)計(jì)使我懂得了理論與實(shí)際相結(jié)合是很重要的，只有理論知識(shí)是遠(yuǎn)遠(yuǎn)不夠的，只有把所學(xué)的理論知識(shí)與實(shí)踐相結(jié)合起來，從理論中得出結(jié)論，才能真正為社會(huì)服務(wù)，從而提高自己的實(shí)際動(dòng)手能力和獨(dú)立思考的能力。在設(shè)計(jì)的過程中遇到問題，可以說得是困難重重，這畢竟第一次做的，難免會(huì)遇到過各種各樣的問題，同時(shí)在設(shè)計(jì)的過程中發(fā)現(xiàn)了自己的不足之處，對(duì)以前所學(xué)過的知識(shí)理解得不夠深刻，掌握得不夠牢固，比如說不懂一些元器件的使用方法，對(duì)單片機(jī)匯編語言掌握得不好……通過這次課程設(shè)計(jì)之后，一定把以前所學(xué)過的知識(shí)重新溫故。通過這次單片機(jī)課程設(shè)計(jì)，我不僅加深了對(duì)單片機(jī)理論的理解，將理論很好地應(yīng)用到實(shí)際當(dāng)中去，而且我還學(xué)會(huì)了如何去培養(yǎng)我們的創(chuàng)新精神，從而不斷地戰(zhàn)勝自己，超越自己。創(chuàng)新可以是在原有的基礎(chǔ)上進(jìn)行改進(jìn)，使之功能不斷完善，成為真己的東西。硬件設(shè)計(jì)2.1硬件電路的系統(tǒng)概述相對(duì)于智能搬運(yùn)機(jī)器人來說，碼垛機(jī)器人系統(tǒng)地用到多種傳感器來完成碼垛色塊任務(wù)，有更好的穩(wěn)定性。Openduino控制器的核心是ATmega328p處理器，可以滿足系統(tǒng)的復(fù)雜性要求。系統(tǒng)中用到的傳感器有用來循跡的QTI傳感器，用來測(cè)距的超聲波傳感器，用來識(shí)別色塊顏色的顏色傳感器。碼垛機(jī)器人控制平臺(tái)的架構(gòu)如下圖所示。圖2-1碼垛機(jī)器人系統(tǒng)架構(gòu)碼垛機(jī)器人需要執(zhí)行循線，測(cè)距和抓取色塊等動(dòng)作，循線過程中需要測(cè)距，抓取色塊后才進(jìn)行顏色識(shí)別，并把物體搬運(yùn)到目標(biāo)區(qū)域。特色描述：▲支持USB連接線在集成開發(fā)環(huán)境下調(diào)試和下載程序；▲既可用USB接口供電，不需外接電源，也可以使用外部6VDC輸入；▲獨(dú)立的電機(jī)驅(qū)動(dòng)電源設(shè)計(jì)；▲各種傳感器和電子元器件標(biāo)準(zhǔn)接口（例如：溫濕度、紅外線、超聲波、熱敏電阻、光敏電阻、GPS等）；▲CMUcam攝像頭和液晶顯示屏接口；▲16路數(shù)字舵機(jī)接口；▲兩路直流電機(jī)和兩路微型步進(jìn)電機(jī)接口（無需額外驅(qū)動(dòng)）；▲8路專用數(shù)字IO擴(kuò)展口、6路PWM輸出、8路10位的ADC擴(kuò)展接口（電機(jī)占用資源包括在內(nèi)）；▲可以獨(dú)立于PC機(jī)運(yùn)行等。技術(shù)參數(shù)：▲主控芯片Atmega328▲晶振：16M▲控制電源：4~6VDC，電機(jī)驅(qū)動(dòng)電源：9~24VDC▲通過開關(guān)轉(zhuǎn)換的USB編程接口▲主板外形尺寸：130mmx120mm▲適用環(huán)境：-40~85°C2.2主芯片圖2-2ATmega328p特征：·高性能，低功耗AVR8位微處理器·32K字節(jié)Flash，1K字節(jié)EEPROM，2K字節(jié)RAM，中斷向量大小為2個(gè)指令字?！じ呒?jí)精簡(jiǎn)指令集體系結(jié)構(gòu)▲131條強(qiáng)大指令大多數(shù)是單一時(shí)鐘周期執(zhí)行▲32個(gè)（8bit）通用工作寄存器▲完全靜態(tài)操作▲在20MHz下，數(shù)據(jù)吞吐率達(dá)到每秒20百萬條指令▲循環(huán)乘法器·高耐力非易失性內(nèi)存片段▲系統(tǒng)內(nèi)置4/8/16/32K字節(jié)自行Flash程序存儲(chǔ)器▲56/512/512/1K字節(jié)電可擦只讀存儲(chǔ)器▲512/1K/1K/2K字節(jié)內(nèi)部靜態(tài)隨機(jī)存儲(chǔ)器▲寫入/擦除周期：10000Flash/100000EEPROM▲數(shù)據(jù)保持：20年·外圍設(shè)備特征▲2個(gè)獨(dú)立預(yù)分頻器和比較模式的8位定時(shí)器/計(jì)數(shù)器▲1個(gè)獨(dú)立預(yù)分頻器，比較模式和捕獲模式的16位定時(shí)器/計(jì)數(shù)器▲獨(dú)立振蕩器的實(shí)時(shí)計(jì)數(shù)器▲6個(gè)PWM通道▲8通道的10位ADC在MLF封裝溫度測(cè)量▲可編程串行通用同步異步收發(fā)器▲主/從設(shè)備串行外部接口▲按字節(jié)2線串行接口▲可編程的獨(dú)立片上振蕩器的看門狗定時(shí)器▲片上模擬比較器▲引腳改變的中端與喚醒·特殊微處理器功能▲電源復(fù)位和可編程低功率欠壓檢測(cè)▲內(nèi)部校準(zhǔn)振蕩器▲外部/內(nèi)部中斷源▲6種睡眠模式2.3多自由度機(jī)械手1.認(rèn)識(shí)角度舵機(jī)本設(shè)計(jì)是利用5個(gè)角度舵機(jī)相互協(xié)作、共同作用來夾取物塊。簡(jiǎn)單地說，通過機(jī)械手姿態(tài)調(diào)試助手改變舵機(jī)的脈沖值，使機(jī)械手的所在的位置發(fā)生改變，從而實(shí)現(xiàn)抓取物塊，放置物塊的能力。如圖3-2所示：圖2-3多自由度機(jī)械手1號(hào)舵機(jī)接SVO1接口，2號(hào)舵機(jī)接SVO2接口，3號(hào)舵機(jī)接SVO3接口，4號(hào)舵機(jī)接SVO4接口，5號(hào)舵機(jī)接SVO5接口。如下圖所示：圖2-4舵機(jī)接口2.零點(diǎn)定義手爪舵機(jī)的零點(diǎn)：當(dāng)兩個(gè)手爪旋轉(zhuǎn)部位成180度時(shí)，舵機(jī)對(duì)應(yīng)的角度就是手爪的零點(diǎn)位置。關(guān)節(jié)舵機(jī)的零點(diǎn)：當(dāng)機(jī)械臂與安裝底盤面成90度夾角時(shí)，每個(gè)舵機(jī)的角度就是對(duì)應(yīng)舵機(jī)的零點(diǎn)位置。每個(gè)舵機(jī)均可從0度位置旋轉(zhuǎn)到180度，從圖上看，舵機(jī)偏左轉(zhuǎn)（相對(duì)機(jī)器人是后仰）是從90度向0度轉(zhuǎn)，舵機(jī)偏右轉(zhuǎn)（相對(duì)機(jī)器人是前俯）是從90度向180度轉(zhuǎn)。注意：上述的90度是視覺上相對(duì)的90度，不一定是每個(gè)舵機(jī)90度脈沖對(duì)應(yīng)的位置。圖2-5機(jī)械手臂零點(diǎn)位置姿態(tài)調(diào)試為了方便機(jī)械手姿態(tài)控制，編寫調(diào)試機(jī)械手姿態(tài)對(duì)應(yīng)的上位機(jī)和下位機(jī)軟件，軟件代碼開放，控制板使用的是OpenduinoBoardVer3.0，上位機(jī)軟件是使用在WindowsXP上運(yùn)行的VC++6.0開發(fā)環(huán)境編寫的機(jī)械手姿勢(shì)調(diào)試助手，下位機(jī)軟件是使用配置成可開發(fā)AVR程序的Eclipse開發(fā)環(huán)境編寫的，上、下位機(jī)必須配合使用才可完成調(diào)試。將機(jī)械手上各個(gè)舵機(jī)正確連接到控制板（SVO1~SVO8接口），給控制板接上外部電源（鋰電池或電源適配器），按一下系統(tǒng)復(fù)位鍵，再將電源開關(guān)撥到ON2位置，這時(shí)候可以發(fā)現(xiàn)機(jī)械手被控制到一個(gè)基本處于零點(diǎn)的姿態(tài)。機(jī)械手姿勢(shì)調(diào)試助手的使用步驟：1）點(diǎn)擊“連接串口”按鈕，若成功打開串口，則按鈕會(huì)顯示“斷開串口”2）串口打開后就可以嘗試點(diǎn)擊“握手設(shè)備”按鈕，若握手成功，如否則檢查串口是否正確，是否下載對(duì)應(yīng)下位機(jī)程序，再次握手。3）首先測(cè)試調(diào)節(jié)兩個(gè)并聯(lián)的舵機(jī)角度，它們的角度必須互補(bǔ)，否則容易損壞舵機(jī)，因此上位機(jī)軟件只用一個(gè)調(diào)節(jié)閥來控制，只給出其中一個(gè)舵機(jī)的脈寬值，另一個(gè)舵機(jī)的脈寬值自然就可以得到了。4)鼠標(biāo)左鍵點(diǎn)擊并按住SVO1&2對(duì)應(yīng)的調(diào)節(jié)閥，可以左右調(diào)節(jié)，單位增量為27，右側(cè)的編輯框中的數(shù)字也相應(yīng)改變，數(shù)字表示控制通道SVOn（n=1~8）的PWM信號(hào)的正脈寬值，單位是0.5us，如圖2-6中調(diào)節(jié)并聯(lián)舵機(jī)的脈寬為3135，那么就是3135*0.5us=1567.5us≈1.6ms，然后點(diǎn)擊“刷新舵機(jī)”按鈕，脈寬數(shù)據(jù)3135才會(huì)被發(fā)送到控制板，機(jī)械手會(huì)立即轉(zhuǎn)動(dòng)到脈寬對(duì)應(yīng)的角度。其他通道的舵機(jī)控制方法一樣。當(dāng)點(diǎn)擊“復(fù)位機(jī)械手”按鈕，所有脈寬值將變?yōu)槟J(rèn)值3000。圖2-6機(jī)械手姿態(tài)調(diào)試助手機(jī)械手應(yīng)用思路：每個(gè)機(jī)械手的初始化位置都是通過上位機(jī)軟件調(diào)節(jié)到零點(diǎn)位置，之后的每個(gè)姿態(tài)都是以零點(diǎn)位置為參考位置，偏離零點(diǎn)位置的角度值也是通過上位機(jī)軟件調(diào)節(jié)而來，脈寬值以27為單位增減。例如某個(gè)三自由度機(jī)械手的零點(diǎn)位置對(duì)應(yīng)脈寬值為：3135（SVO1&2）、3162（SVO3）、3081（SVO4）、2919（SVO5）。通過軟件調(diào)節(jié)到某個(gè)目的姿態(tài)，其對(duì)應(yīng)脈寬值為2703（SVO1&2）、2270（SVO3）、3757（SVO4）、3622（SVO5）。如圖2-7所示：圖2-7則：變化量=（零點(diǎn)值-目標(biāo)值）/27即：SVO1&2的變化量=（3135-2703）/27=16；SVO3的變化量=（3162-2270）/27=33；SVO4的變化量=（3081-3757）/27=-25；SVO5的變化量=（2919-2703）/27=-26；角度舵機(jī)與Atmega328p的接線表1角度舵機(jī)與Atmega328p的接線角度舵機(jī)Atmega328p引腳角度舵機(jī)Atmega328p引腳1號(hào)舵機(jī)SVO14號(hào)舵機(jī)SVO42號(hào)舵機(jī)SVO25號(hào)舵機(jī)SVO53號(hào)舵機(jī)SVO32.4伺服電機(jī)認(rèn)識(shí)伺服電機(jī)伺服電機(jī)(servomotor)又稱連續(xù)旋轉(zhuǎn)電機(jī)，是指在伺服系統(tǒng)中控制機(jī)械元件運(yùn)轉(zhuǎn)的發(fā)動(dòng)機(jī)，是一種補(bǔ)助馬達(dá)間接變速裝置。伺服電機(jī)可使控制速度，位置精度非常準(zhǔn)確，可以將電壓信號(hào)轉(zhuǎn)化為轉(zhuǎn)矩和轉(zhuǎn)速以驅(qū)動(dòng)控制對(duì)象。伺服電機(jī)轉(zhuǎn)子轉(zhuǎn)速受輸入信號(hào)控制，并能快速反應(yīng)，在自動(dòng)控制系統(tǒng)中，用作執(zhí)行元件，且具有機(jī)電時(shí)間常數(shù)小、線性度高、始動(dòng)電壓等特性，可把所收到的電信號(hào)轉(zhuǎn)換成電動(dòng)機(jī)軸上的角位移或角速度輸出。伺服電機(jī)的調(diào)零所謂伺服電機(jī)的調(diào)零是指發(fā)送一個(gè)特定的控制信號(hào)（零點(diǎn)標(biāo)定信號(hào)）給伺服電機(jī)時(shí)，讓電機(jī)處于靜止的狀態(tài)。由于伺服電機(jī)在工廠沒有預(yù)先調(diào)整，它們?cè)诮邮樟泓c(diǎn)標(biāo)定信號(hào)時(shí)可能會(huì)轉(zhuǎn)動(dòng)，因此要用螺絲刀調(diào)節(jié)伺服電機(jī)模塊內(nèi)的調(diào)節(jié)電阻讓伺服電機(jī)保持靜止，這就是伺服電機(jī)的調(diào)零過程。圖2-8、圖2-9就是伺服電機(jī)電位器的零點(diǎn)調(diào)節(jié)以及零點(diǎn)標(biāo)定信號(hào)的時(shí)序圖。圖2-8伺服電機(jī)電位器的零點(diǎn)調(diào)節(jié) 圖2-9零點(diǎn)標(biāo)定信號(hào)的時(shí)序圖如果電機(jī)沒有進(jìn)行零點(diǎn)標(biāo)定，它的連接喉就會(huì)轉(zhuǎn)動(dòng)，而且你會(huì)聽到里面馬達(dá)的響聲，否則就不會(huì)轉(zhuǎn)動(dòng)且沒有響聲。當(dāng)電機(jī)沒有進(jìn)行零點(diǎn)標(biāo)定時(shí)，將螺絲刀插入伺服電機(jī)的電位器的調(diào)節(jié)孔，輕輕地轉(zhuǎn)動(dòng)螺絲刀調(diào)節(jié)電位器進(jìn)行零點(diǎn)標(biāo)定。3.伺服電機(jī)與Atmega328p的接線表2伺服電機(jī)與Atmega328p的接線序號(hào)伺服電機(jī)Atmega328p的引腳1左電機(jī)2｜PD22右電機(jī)19｜A52.5QTI傳感器1.認(rèn)識(shí)QTI傳感器本設(shè)計(jì)使用的QTI(QuickTrackInfrared)傳感器是一種使用光電接收管來探測(cè)物體表面反射光強(qiáng)度的傳感器，當(dāng)QTI傳感器面對(duì)暗淡的物體表面時(shí)，反射光強(qiáng)度很低。當(dāng)QTI傳感器面對(duì)明亮的物體表面時(shí)，反射光強(qiáng)度很高，不同程度反射光對(duì)應(yīng)傳感器不同的輸出值，即對(duì)應(yīng)不同亮度的物體時(shí)QTI的信號(hào)引腳會(huì)輸出不同的電平信號(hào)。本設(shè)計(jì)中應(yīng)用到的QTI傳感器探測(cè)到黑色物體時(shí)輸出為高電平（+5V），探測(cè)到白色物體時(shí)輸出為低電平（0V），如圖2-10所示。圖2-10QTI傳感器2.QTI傳感器的引腳定義1）SIG：信號(hào)輸出；2）VCC：5V直流電源；3）GND：電源地線；3.QTI傳感器的性能參數(shù)▲工作溫度：-40℃-85℃；▲工作電壓：5V；▲連續(xù)電流：50mA；▲功耗：100mW；▲佳探測(cè)距離：5~10mm；▲佳距離下的大散射角度：65o；▲響應(yīng)時(shí)間：上升沿時(shí)間10us,下降沿時(shí)間50us。QTI傳感器與Atmega328p的接線表3QTI傳感器與Atmega328p的接線QTI的個(gè)數(shù)QTI傳感器Atmega328p引腳1靠近左電機(jī)14｜A02靠近超聲波左側(cè)15｜A13靠近超聲波右側(cè)16｜A24靠近右電機(jī)17｜A35靠近電池盒18｜A42.6顏色傳感器1.認(rèn)識(shí)TCS230顏色傳感器顏色傳感器就是通過檢測(cè)某個(gè)顏色中三原色的比例來識(shí)別該顏色的，本設(shè)計(jì)中使用的是TCS230顏色傳感器，顏色傳感器的外觀如圖2-11所示；顏色傳感器的輸出信號(hào)（由OUT引腳輸出）是數(shù)字量，可以實(shí)現(xiàn)每個(gè)顏色信道10位（5V/210）以上的轉(zhuǎn)換精度，可以驅(qū)動(dòng)標(biāo)準(zhǔn)的TTL或者CMOS邏輯輸入，因此可直接與微控制器的I/O引腳連接。顏色傳感器實(shí)物顏色傳感器上自帶了兩個(gè)高亮白色LED，有助于提高顏色識(shí)別的準(zhǔn)確性。圖2-11TCS230顏色傳感器2.TCS230傳感器的引腳定義1）S0、S1：用于選擇輸出比例因子或者電源關(guān)斷模式，不同的輸出比例因子可調(diào)整輸出頻率，以適應(yīng)不同的需求；2）S2、S3：用于選擇濾波器的類型；3）LED：用于點(diǎn)亮兩個(gè)LED燈；4）OUT：頻率輸出引腳，該顏色傳感器的典型輸出頻率范圍為2Hz（0.5s）～500kHz（2us）；5）GND：芯片接地引腳；6）+5V、VDD：5V電源。3.TCS230傳感器的特點(diǎn)▲可完成高分辨率的光照度/頻率轉(zhuǎn)換；▲色彩和輸出頻率可編程調(diào)整；▲單電源工作，工作電壓范圍為2.7V-5.5V；▲具備掉電恢復(fù)功能；▲50KHZ時(shí)非線性誤差的典型值為0.2%。4.TCS230顏色傳感器的工作原理顏色傳感器就是通過檢測(cè)某個(gè)顏色中三原色的比例來識(shí)別該顏色的，首先打開LED燈，選擇輸出比例因子，然后依次選定不同的顏色濾波器，每選通一個(gè)濾波器就檢測(cè)其輸出引腳輸出的不同頻率（即光強(qiáng)）的方波脈沖數(shù)，最后根據(jù)得到的三原色的脈沖數(shù)比例判斷顏色。S0、S1及S2、S3的不同組合如表4所示：表4S0、S1、S2、S3組合選項(xiàng)S0S1輸出比例因子S2S3顏色濾波器類型00關(guān)閉電源00紅色（R）011:5001藍(lán)色（B）101:510無111:111綠色（G）從理論上來講，白色是等量的紅色、綠色和藍(lán)色混合而成的，但TCS230傳感器對(duì)這三種顏色的敏感度是不同的，導(dǎo)致TCS230傳感器的RGB輸出并不相等，因此在使用前必須進(jìn)行白平衡調(diào)整。5.白平衡在本設(shè)計(jì)中所謂的白平衡就是告訴機(jī)器人什么是白色，使得顏色傳感器對(duì)“白色”中的三原色RGB輸出是相等的。通過白平衡調(diào)整可以得到在顏色識(shí)別時(shí)需要用到的通信道的時(shí)間基準(zhǔn)。白平衡的過程為：在顏色傳感器前適當(dāng)位置放置一個(gè)白色物塊，打開LED燈，選擇輸出比例因子，依次通過紅色、綠色和藍(lán)色濾波器，對(duì)每個(gè)通道的脈沖計(jì)數(shù)到255后關(guān)閉通道，最后分別得到每個(gè)通道所用的時(shí)間，這個(gè)時(shí)間就是識(shí)別顏色時(shí)要用的基準(zhǔn)時(shí)間。白平衡的控制流程如圖2-12所示。圖2-12白平衡的控制流程TCS230顏色傳感器與Atmega328p的接線表5TCS230顏色傳感器與Atmega328p的接線TCS230引腳Atmega328P引腳TCS230引腳Atmega328P引腳S07｜PD7OUT3｜PD3S16｜PD6VDD+5（R）S25｜PD5GNDGND（B）S34｜PD4+5+5（R）LED8｜PB0OUT3｜PD3TCS230顏色傳感器測(cè)試根據(jù)顏色傳感器的特性，在傳感器的使用過程中主要需要知道時(shí)間基準(zhǔn)和脈沖數(shù)，所以在控制程序中可以使用定時(shí)器0來統(tǒng)計(jì)時(shí)間，外部中斷1來判斷一次脈沖。定時(shí)器0是8位定時(shí)器，而定時(shí)的基本單位不能過大，在統(tǒng)計(jì)時(shí)間的過程中可以采用定時(shí)器0多次累計(jì)的方式計(jì)算時(shí)長。在代碼中，對(duì)定時(shí)器0進(jìn)行8分頻設(shè)置，從而定時(shí)的基本單位為0.5us。2.7超聲波傳感器認(rèn)識(shí)超聲波傳感器本設(shè)計(jì)中使用的是DM-S28018-B,與普通的超聲波一樣都是利用聲波反射原理。DM-S28018-B提供了精確的、非接觸式的距離測(cè)量，測(cè)量的范圍從2cm到3.3m,它非常方便與微控制器連接，只需要兩個(gè)I/O口進(jìn)行控制。左邊是發(fā)射頭右邊是接收頭，其工作頻率為12M，從左到右，VCC接5V電源，Trig是接收啟動(dòng)信號(hào)引腳，Echo是信號(hào)輸出引腳，Out是空腳（防盜），GND接地。DM-S28018-B超聲波測(cè)距傳感器的實(shí)物圖如圖2-13所示。圖2-13DM-S28018-B超聲波傳感器的工作原理超聲波模塊上電后，首先在Trig引腳給超聲波模塊發(fā)送一啟動(dòng)信號(hào)（10us），超聲波模塊接收到啟動(dòng)信號(hào)后，就在發(fā)射頭發(fā)出超聲波信號(hào)，同時(shí)將Echo引腳置為高電平，這時(shí)控制器就必須開始計(jì)時(shí)，超聲波信號(hào)在空氣中遇到障礙物后反射回來，當(dāng)接收頭檢測(cè)到反射回來的超聲波信號(hào)時(shí)，馬上將Echo引腳置為低電平，Echo引腳高電平持續(xù)的時(shí)間t用到轉(zhuǎn)換為距離的計(jì)算中。若超過18.5ms還沒檢測(cè)到超聲波信號(hào)，也將Echo引腳置為低電平，準(zhǔn)備下一次測(cè)量。超聲波測(cè)距原理如圖2-14所示。圖2-14超聲波測(cè)距原理超聲波測(cè)距原理超聲波在空氣中傳播速度為340m/s，則測(cè)量距離為（t*340）/2。超聲波傳感器與Atmega328p的接線表６超聲波傳感器與Atmega328p的接線超聲波模塊引腳連接Atmega328P引腳ＶＣＣＶＣＣＧＮＤＧＮＤTRIGＰＢ２ECHOＰＢ０ＯＵＴ空軟件設(shè)計(jì)3.1項(xiàng)目的主流程圖由碼垛機(jī)器人的場(chǎng)地圖可以看出碼垛機(jī)器人執(zhí)行的任務(wù)復(fù)雜，需要控制的傳感器較多，軟件編程自然就會(huì)有點(diǎn)難度，所以軟件架構(gòu)設(shè)計(jì)非常重要。在硬件工作正常和連線正確的前提下，軟件編程決定任務(wù)能否被更穩(wěn)定、更快速地完成。碼垛機(jī)器人需要沿著直線走，轉(zhuǎn)彎，抓取色塊，放下色塊等等多種動(dòng)作，如果軟件結(jié)構(gòu)是使用順序結(jié)構(gòu)，顯然行不通。碼垛機(jī)器人軟件系統(tǒng)架構(gòu)是采用分支結(jié)構(gòu)來執(zhí)行，使得系統(tǒng)程序編程變得簡(jiǎn)單，有條理。碼垛機(jī)器人的場(chǎng)地圖、軟件架構(gòu)流程圖如下圖所示。圖3-1場(chǎng)地圖圖3-2軟件架構(gòu)流程圖3.2Arduino語言及它專門定義的函數(shù)Arduino語言建立在C/C++基礎(chǔ)上，并把相關(guān)的一些參數(shù)設(shè)置函數(shù)化，不用我們?nèi)チ私庥布牡讓樱尣涣私釧VR單片機(jī)（微控制器）的朋友也能輕松上手。下面簡(jiǎn)單的注釋一下Arduino語言以及它專門定義的函數(shù)。數(shù)據(jù)類型：1boolean布爾類型7char字符類型2byte字節(jié)類型8int整數(shù)類型3array數(shù)組9float浮點(diǎn)實(shí)數(shù)類型4long長整型10double雙浮點(diǎn)實(shí)數(shù)5string字符串11unsignedint無符號(hào)整型6void空12unsignedlong無符號(hào)長整型常量：▲HIGH和LOW：表示數(shù)字IO口的電平，HIGH表示高電平（1），LOW表示低電平（0）?！鳬NPUT和OUTPUT：表示數(shù)字IO口的方向，INPUT表示輸入（高阻態(tài)）,OUTPU表示輸出（AVR能提供5V電壓40mA電流）?！鴗rue和false：true表示真（1），false表示假（0）。▲以上為基礎(chǔ)c語言的關(guān)鍵字和符號(hào)，大家可以了解，具體使用可以結(jié)合實(shí)驗(yàn)的程序。結(jié)構(gòu)：▲voidsetup()初始化變量、管腳模式，調(diào)用庫函數(shù)等▲voidloop()主程序，連續(xù)執(zhí)行函數(shù)內(nèi)的語句▲IO功能操作函數(shù)▲數(shù)字I/O設(shè)定和操作函數(shù)▲pinMode(pin,mode)：數(shù)字IO口輸入輸出模式定義函數(shù)，pin表示為0～13，mode為INPUT或OUTPUT。▲digitalWrite(pin,value)：數(shù)字IO口輸出電平函數(shù)，pin表示為0～13，value表示為HIGH或LOW。比如定義HIGH可以驅(qū)動(dòng)LED?！鴌ntdigitalRead(pin)：數(shù)字IO口讀輸入電平函數(shù)，pin表示為0～13，value表示為HIGH或LOW。比如可以讀數(shù)字傳感器?！MI/O設(shè)定和操作函數(shù)▲intanalogRead(pin)：模擬IO口讀函數(shù)，pin表示為0～5（ArduinoDiecimila為0～5，Arduinonano為0～7）。比如可以讀模擬傳感器（10位AD，0～5V表示為0～1023）?！鴄nalogWrite(pin,value)：PWM數(shù)字IO口，PWM輸出函數(shù)，Arduino數(shù)字IO口標(biāo)注了PWM的IO口可使用該函數(shù)，pin表示3,5,6,9,10,11，value表示為0～255。可用于電機(jī)PWM調(diào)速或音樂播放。時(shí)間函數(shù)▲delay(ms)：延時(shí)函數(shù)（單位ms）?！鴇elayMicroseconds(us)：延時(shí)函數(shù)（單位us）。數(shù)學(xué)函數(shù)：▲min(x,y)：求最小值▲max(x,y)：求最大值▲abs(x)：計(jì)算絕對(duì)值▲constrain(x,a,b)：約束函數(shù)，下限a，上限b，x必須在ab之間才能返回?！鴐ap(value,fromLow,fromHigh,toLow,toHigh)：約束函數(shù)，value必須在fromLow與toLow之間和fromHigh與toHigh之間?！鴓ow(base,exponent)：指數(shù)函數(shù)，base的exponent次方。▲sq(x)：平方▲sqrt(x)：開根號(hào)3.3顏色傳感器的檢測(cè)程序圖識(shí)別顏色的流程圖intmain(void){init();setup();delay(10);RecogColor();for(i=0;i<3;i++){WhiteBalance(refer_time);//白平衡delay(500);}delay(20);while(1){CurrentColor();Serial.print("R:");Serial.print(clrpulses[0]);Serial.print("");Serial.print("B:");Serial.print(clrpulses[1]);Serial.print("");Serial.print("G:");Serial.println(clrpulses[2]);Serial.println(currentcolor);delay(2000);}}3.4QTI傳感器的檢測(cè)程序intmain(void){init();setup();delay(200);while(1){Serial.print(GetQtiStatus(qtipin[0]));Serial.print(GetQtiStatus(qtipin[1]));Serial.print(GetQtiStatus(qtipin[2]));Serial.print(GetQtiStatus(qtipin[3]));Serial.println(GetQtiStatus(qtipin[4]));}}3.5超聲波傳感器的檢測(cè)程序intmain(void){init();setup();delay(200);while(1){dis=DistanceDetection();Serial.println((float)dis,1);delay(2000);}}附錄1實(shí)物圖附錄2程序主程序#include<WProgram.h>#include<main.h>#include"OpenMultiServo.h"#include"OpenContinMotor.h"#include"OpenColorSensor.h"#include"OpenUltrasonic.h"#include"OpenQti.h"#defineleftservo2#definerightservo19#defineDownDis150#defineYellow1//黃#defineWhite 2//白#defineRed 3//紅#defineBlack 4//黑#defineBlue5//藍(lán)#defineUint27uint8_tyellownum=0;uint8_twhitenum=0;//白區(qū)當(dāng)前色塊個(gè)數(shù)uint8_trednum=0;uint8_tblacknum=0;uint8_tbluenum=0;uint8_tAPnum=3;//A點(diǎn)疊放的色塊個(gè)數(shù)uint8_tBPnum=3;uint8_tCPnum=3;uint8_tDPnum=3;uint8_tEPnum=3;uint8_tfangsum=0;uint8_tremainsum=0;intdot;//傳感器引腳定義constintcolorpin[6]={7,6,5,4,8,3};//顏色傳感器引腳連接constintqtipin[6]={14,15,16,17,18};//前4個(gè)是前方QTI，后面是最后緊挨一起的QTIconstinttrig=12;//超聲波Trig引腳constintecho=10;//超聲波Echo引腳charfunc=1;//顏色傳感器變量intrefer_time[3]={0,0,0};//白平衡得到的基準(zhǔn)時(shí)間intclrpulses[3]={0,0,0};intcurrentcolor;//超聲波變量unsignedlongdis=0,mindis=65535;charcnts1=0,cnts2=0;chari=0;charrecog_times=0;uint16_tInit_servoangle[7]={3000,3054,3378,3000,3000,3000,3000};//存放用戶設(shè)置的舵機(jī)角度，servoangle[0]存放并聯(lián)舵機(jī)角度，單位增量為27uint16_tservoangle[7]={3000,3000,3000,3000,3000,3000,3000};//存放用戶設(shè)置的舵機(jī)角度，servoangle[0]存放并聯(lián)舵機(jī)角度voidCurrentColor(void){ ColorreCognt(refer_time,clrpulses); if(clrpulses[0]>100){//紅色濾波器大于150 if(clrpulses[1]>200&&clrpulses[2]>200){//藍(lán)色和綠色濾波器都大于200 currentcolor=White;//白色 } elseif(abs(clrpulses[2]-clrpulses[1]>75)){ currentcolor=Yellow; } else{ currentcolor=Red; } } else{ if(abs(clrpulses[0]-clrpulses[1]>0)){ currentcolor=Black; } else{ currentcolor=Blue; } } delay(1000);}voidFastGoForward(){ PulseOut(leftservo,1700); PulseOut(rightservo,1300); delay(20);}voidSlowGoForward(){ PulseOut(leftservo,1550); PulseOut(rightservo,1450); delay(15);}voidGoBack(void){ PulseOut(leftservo,1480); PulseOut(rightservo,1520); delay(15);}/**左旋轉(zhuǎn)*/voidSpinLeft(void){ PulseOut(leftservo,1400); PulseOut(rightservo,1400); delay(20);}/**右旋轉(zhuǎn)*/voidSpinRight(void){ PulseOut(leftservo,1600);//1540 PulseOut(rightservo,1600);//右旋轉(zhuǎn)（兩個(gè)輪子一起） delay(20);}voidBlackBack(intdot){ while(dot--){ PulseOut(leftservo,1480); PulseOut(rightservo,1520); delay(15); }}voidTurnLeft(void){ PulseOut(leftservo,1500); PulseOut(rightservo,1350); delay(10);}voidTurnRight(void){ PulseOut(leftservo,1650); PulseOut(rightservo,1500); delay(10);}voidStop(void){ PulseOut(leftservo,1500); PulseOut(rightservo,1500); delay(20);}voidTurnLeftAnyPulse(intpulses){ while(pulses--) { SpinLeft(); delay(2); }}voidTurnRightAnyPulse(intpulses){ while(pulses--) { SpinRight(); delay(2); }}voidCrossblk(intpulses){ while(pulses--) { FastGoForward(); delay(2); }}booleanIsTailQtiBlack(void){ if(GetQtiStatus(qtipin[4])==false) { delay(2); if(GetQtiStatus(qtipin[4])==false) { returnfalse; } else { returntrue; } } else { returntrue; }}/**//檢測(cè)中間左QTI是否在黑線內(nèi)*/booleanIsMLeftQtiBlack(void){ if(GetQtiStatus(qtipin[1])==true) { delay(2); if(GetQtiStatus(qtipin[1])==true) { returntrue; } else { returnfalse; } } else { returnfalse; }}/**//檢測(cè)中間右QTI是否在黑線內(nèi)*/booleanIsMRightQtiBlack(void){ if(GetQtiStatus(qtipin[2])==true) { delay(2); if(GetQtiStatus(qtipin[2])==true) { returntrue; } else { returnfalse; } } else { returnfalse; }}/**//檢測(cè)輪子左QTI是否在黑線內(nèi)*/booleanIsELeftQtiBlack(void){ if(GetQtiStatus(qtipin[0])==true) { delay(2); if(GetQtiStatus(qtipin[0])==true) { returntrue; } else { returnfalse; } } else { returnfalse; }}/**//檢測(cè)輪子右QTI是否在黑線內(nèi)*/booleanIsERightQtiBlack(void){ if(GetQtiStatus(qtipin[3])==true) { delay(2); if(GetQtiStatus(qtipin[3])==true) { returntrue; } else { returnfalse; } } else { returnfalse; }}/**前方中間QTI循線*/booleanMoveMiddleQti(void){ if(GetQtiStatus(qtipin[1])==false&&GetQtiStatus(qtipin[2])==false) { FastGoForward(); } elseif(GetQtiStatus(qtipin[1])==false&&GetQtiStatus(qtipin[2])==true) { TurnRight(); } elseif(GetQtiStatus(qtipin[1])==true&&GetQtiStatus(qtipin[2])==false) { TurnLeft(); } elseif(GetQtiStatus(qtipin[1])==true&&GetQtiStatus(qtipin[2])==true) { FastGoForward(); returntrue; } returnfalse;}/**前方中間QTI循線*/booleanMoveSlowQti(void){ if(GetQtiStatus(qtipin[1])==false&&GetQtiStatus(qtipin[2])==false) { PulseOut(leftservo,1540); PulseOut(rightservo,1460); delay(15); } elseif(GetQtiStatus(qtipin[1])==false&&GetQtiStatus(qtipin[2])==true) { PulseOut(leftservo,1520); PulseOut(rightservo,1500); delay(10); } elseif(GetQtiStatus(qtipin[1])==true&&GetQtiStatus(qtipin[2])==false) { PulseOut(leftservo,1500); PulseOut(rightservo,1480); delay(10); } elseif(GetQtiStatus(qtipin[1])==true&&GetQtiStatus(qtipin[2])==true) { FastGoForward(); returntrue; } returnfalse;}/**輪子附近QTI循線*/booleanMoveEdgeQti(void){ if(GetQtiStatus(qtipin[0])==false&&GetQtiStatus(qtipin[3])==false) { FastGoForward(); } elseif(GetQtiStatus(qtipin[0])==false&&GetQtiStatus(qtipin[3])==true) { TurnRight(); } elseif(GetQtiStatus(qtipin[0])==true&&GetQtiStatus(qtipin[3])==false) { TurnLeft(); } elseif(GetQtiStatus(qtipin[0])==true&&GetQtiStatus(qtipin[3])==true) { FastGoForward(); returntrue; } returnfalse;}/**固定超聲波與物件的距離（140-150）*/voidSetDistance(void){ while(1){ dis=DistanceDetection(); if(dis>150){ MoveSlowQti(); delay(20); } elseif(dis<140){ GoBack(); delay(20); } else{ Stop(); break;} }}voidSetDistanceCTop(void){//超聲波在放c,e底部物體的距離 while(1){ dis=DistanceDetection(); Serial.println((float)dis,1); if(dis>260){ MoveSlowQti(); delay(20); } elseif(dis<180){ GoBack(); delay(20); } else{ Stop(); break;} }}/**糾正車頭方位*/voidCorrectPosition(void){ if(IsMLeftQtiBlack()) { while(IsMLeftQtiBlack())//超聲波左側(cè)qti在黑色區(qū)域 { SpinLeft();//左轉(zhuǎn) }} elseif(IsMRightQtiBlack()) { while(IsMRightQtiBlack())//超聲波左側(cè)qti在黑色區(qū)域 { SpinRight();//右轉(zhuǎn) }} Stop();}voidOAtoOB(void){ TurnRightAnyPulse(10); while(!IsMRightQtiBlack()){ SpinRight(); } while(!IsMLeftQtiBlack()){ SpinRight(); } Stop();}voidOBtoOC(void){ TurnRightAnyPulse(10); while(!IsMRightQtiBlack()){ SpinRight(); } while(!IsMLeftQtiBlack()){ SpinRight(); } Stop();}voidOCtoOD(void){ TurnRightAnyPulse(10); while(!IsMRightQtiBlack()){ SpinRight(); } while(!IsMLeftQtiBlack()){ SpinRight(); } Stop();}voidOEtoOD(void){ TurnLeftAnyPulse(10);//左轉(zhuǎn)角度15 while(!IsMLeftQtiBlack()){//超聲波左側(cè)qti在白色區(qū)域 SpinLeft(); } while(!IsMRightQtiBlack()){//超聲波右側(cè)qti在白色區(qū)域 SpinLeft(); } Stop();}voidODtoOC(void){ TurnLeftAnyPulse(10);//左轉(zhuǎn)角度 while(!IsMLeftQtiBlack()){//超聲波左側(cè)qti在白色區(qū)域 SpinLeft(); } while(!IsMRightQtiBlack()){//超聲波右側(cè)qti在白色區(qū)域 SpinLeft(); } Stop();}voidOCtoOB(void){ TurnLeftAnyPulse(10);//左轉(zhuǎn)角度 while(!IsMLeftQtiBlack()){//超聲波左側(cè)qti在白色區(qū)域 SpinLeft(); } while(!IsMRightQtiBlack()){//超聲波右側(cè)qti在白色區(qū)域 SpinLeft(); } Stop();}voidOBtoOA(void){ TurnLeftAnyPulse(10); while(!IsMLeftQtiBlack()){ SpinLeft(); } while(!IsMRightQtiBlack()){ SpinLeft(); } Stop();}voidOCtoOA(void){ TurnLeftAnyPulse(10);//左轉(zhuǎn)角度 while(!IsELeftQtiBlack())//左電機(jī)qti在白色區(qū)域 { SpinLeft();//左轉(zhuǎn) } while(!IsERightQtiBlack())//右電機(jī)qti在白色區(qū)域 { SpinLeft(); } while(!IsTailQtiBlack())//中間qti在白色區(qū)域 { SpinLeft();//左轉(zhuǎn) }}voidOCtoOE(void){ TurnRightAnyPulse(10);//右轉(zhuǎn)角度 while(!IsERightQtiBlack())//右輪qti在白色區(qū)域 { SpinRight();//右轉(zhuǎn) } while(!IsELeftQtiBlack())//左輪qti在白色區(qū)域 { SpinRight();//右轉(zhuǎn) } while(!IsTailQtiBlack())//盒子中間qti在白色區(qū)域 { SpinRight();//右轉(zhuǎn) } Stop(); }voidOEtoOO(void){ TurnRightAnyPulse(15); while(!IsMRightQtiBlack()){ SpinRight(); } while(!IsMLeftQtiBlack()){ SpinRight(); } Stop();}voidOAtoOO(void){ TurnLeftAnyPulse(15); while(!IsMLeftQtiBlack()){ SpinLeft(); } while(!IsMRightQtiBlack()){ SpinLeft(); } Stop();}voidFirstOtoMarkArea(void){ for(i=0;i<55;i++){ MoveMiddleQti(); } while(IsTailQtiBlack()){ MoveMiddleQti(); } for(i=0;i<7;i++){ MoveMiddleQti(); } Stop();}voidNoFirstOtoMarkArea(void){ for(i=0;i<50;i++){ MoveMiddleQti(); } while(IsTailQtiBlack()){ MoveMiddleQti(); } SetDistance(); CorrectPosition(); Stop();}voidTurn180(void){ i=0; delay(50); TurnRightAnyPulse(20);//右轉(zhuǎn)角度 while(1){ SpinRight(); if(IsMRightQtiBlack()){//超聲波右側(cè)qti在黑色區(qū)域 i++; if(i==2){ i=0; Stop(); break; } } } if(IsMRightQtiBlack()){ while(IsMRightQtiBlack()){//超聲波右側(cè)qti在黑色區(qū)域 SpinRight(); } } elseif(IsMLeftQtiBlack()){ while(IsMLeftQtiBlack()){//超聲波左側(cè)qti在黑色區(qū)域 SpinLeft(); } } Stop();}voidStartRun(void){ Crossblk(25); while(!MoveEdgeQti());//執(zhí)行輪子兩端qti狀態(tài) while(!MoveMiddleQti());//執(zhí)行超聲波兩端qti狀態(tài) Stop();//停止}voidEndRun(void){ while(!MoveMiddleQti()); while(!MoveEdgeQti()); Crossblk(40); Stop(); while(1);}voidCatchTop(void){//機(jī)械手抓取頂部物體 servoangle[0]=Init_servoangle[0]-Uint*37; servoangle[1]=Init_servoangle[1]-Uint*50; servoangle[2]=Init_servoangle[2]-Uint*6; servoangle[3]=Init_servoangle[3]+Uint*0; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(10);//20 servoangle[3]=Init_servoangle[3]+Uint*49; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(5);//10 servoangle[0]=Init_servoangle[0]; servoangle[1]=Init_servoangle[1]; servoangle[2]=Init_servoangle[2]; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(10);//20}voidCatchMiddle(void){//機(jī)械手抓取中部物體 servoangle[0]=Init_servoangle[0]-Uint*49; servoangle[1]=Init_servoangle[1]-Uint*54; servoangle[2]=Init_servoangle[2]-Uint*23; servoangle[3]=Init_servoangle[3]-Uint*0; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(10);//20 servoangle[3]=Init_servoangle[3]+Uint*49; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(5);//15 servoangle[0]=Init_servoangle[0]; servoangle[1]=Init_servoangle[1]; servoangle[2]=Init_servoangle[2]; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(10);//20}voidCatchBottom(void){//機(jī)械手抓取底部物體 servoangle[0]=Init_servoangle[0]-Uint*70; servoangle[1]=Init_servoangle[1]-Uint*52; servoangle[2]=Init_servoangle[2]-Uint*31; servoangle[3]=Init_servoangle[3]-Uint*0; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(10);//20 servoangle[3]=Init_servoangle[3]+Uint*49; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(5);//10 servoangle[0]=Init_servoangle[0]; servoangle[1]=Init_servoangle[1]; servoangle[2]=Init_servoangle[2]; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(10);//20}voidDownTop(void){//機(jī)械手放頂部物體 servoangle[0]=Init_servoangle[0]-Uint*37;servoangle[1]=Init_servoangle[1]-Uint*50;servoangle[2]=Init_servoangle[2]-Uint*6; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(10); servoangle[3]=Init_servoangle[3]-Uint*0; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(5); servoangle[0]=Init_servoangle[0]; servoangle[1]=Init_servoangle[1]; servoangle[2]=Init_servoangle[2]; servoangle[3]=Init_servoangle[3]; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(10);}voidDownMiddle(void){//機(jī)械手放中部物體 servoangle[0]=Init_servoangle[0]-Uint*49; servoangle[1]=Init_servoangle[1]-Uint*54; servoangle[2]=Init_servoangle[2]-Uint*23; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(10); servoangle[3]=Init_servoangle[3]-Uint*0; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(5); servoangle[0]=Init_servoangle[0]; servoangle[1]=Init_servoangle[1]; servoangle[2]=Init_servoangle[2]; servoangle[3]=Init_servoangle[3]; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(10);}voidDownBottom(void){//機(jī)械手放底部物體 servoangle[0]=Init_servoangle[0]-Uint*70; servoangle[1]=Init_servoangle[1]-Uint*52; servoangle[2]=Init_servoangle[2]-Uint*31; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(10); servoangle[3]=Init_servoangle[3]-Uint*0; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(5); servoangle[0]=Init_servoangle[0]; servoangle[1]=Init_servoangle[1]; servoangle[2]=Init_servoangle[2]; servoangle[3]=Init_servoangle[3]; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(10);}voidResetHand(void){//初始化手臂 servoangle[0]=Init_servoangle[0]-Uint*30; servoangle[1]=Init_servoangle[1]+Uint*39; servoangle[2]=Init_servoangle[2]-Uint*58; servoangle[3]=Init_servoangle[3]-Uint*10; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(20);}voidRecogColorAction(void){//顏色識(shí)別角度舵機(jī)5抓緊 servoangle[0]=Init_servoangle[0]-Uint*32; servoangle[1]=Init_servoangle[1]+Uint*33; servoangle[2]=Init_servoangle[2]-Uint*64; servoangle[3]=Init_servoangle[3]+Uint*49; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(10); servoangle[3]=Init_servoangle[3]+Uint*42; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(5);}voidRecogColor(void){//識(shí)別顏色角度舵機(jī)5松開 servoangle[0]=Init_servoangle[0]-Uint*32; servoangle[1]=Init_servoangle[1]+Uint*33; servoangle[2]=Init_servoangle[2]-Uint*64; servoangle[3]=Init_servoangle[3]+Uint*15; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(10);}voidRecogColorFinish(void){//識(shí)別顏色完成手臂狀態(tài) servoangle[3]=Init_servoangle[3]+Uint*49; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(5);}voidAOtoStartLine(void){ while(!IsERightQtiBlack()){ MoveMiddleQti(); } OEtoOO(); Stop();}voidBOtoStartLine(void){ while(!MoveMiddleQti()); while(!MoveEdgeQti()); Stop(); Crossblk(20); i=0; while(!IsELeftQtiBlack()){ FastGoForward(); } Stop(); TurnRightAnyPulse(10); while(!IsMLeftQtiBlack()){ SpinRight(); } Stop();}voidCOtoStartLine(void){ while(!MoveMiddleQti()); while(!MoveEdgeQti()); Crossblk(5); while(!MoveEdgeQti()); Stop();}voidDOtoStartLine(void){ while(!MoveMiddleQti()); while(!MoveEdgeQti()); Stop(); delay(100); Crossblk(20); while(!IsERightQtiBlack()){//檢測(cè)輪子右QTI是否在白色區(qū)域 FastGoForward(); } Stop(); TurnLeftAnyPulse(10); while(!IsMLeftQtiBlack()){//超聲波左側(cè)qti SpinLeft(); } while(!IsMRightQtiBlack()){//超聲波右側(cè)qti SpinLeft(); } Stop();}voidEOtoStartLine(void){ while(!IsELeftQtiBlack()){ MoveMiddleQti(); } OAtoOO(); Stop();}voidACarryWhite(void){ //A點(diǎn)搬運(yùn)白色塊 OAtoOB(); if(whitenum==0){ FirstOtoMarkArea(); } else{ NoFirstOtoMarkArea(); } delay(10); switch(whitenum){ case0: DownBottom(); break; case1: DownMiddle(); break; case2: DownTop(); break; default: Stop(); break; } for(i=0;i<7;i++){ MoveMiddleQti(); } Turn180(); BOtoStartLine(); for(i=0;i<50;i++){ MoveMiddleQti(); } Turn180(); whitenum++;}voidACarryBlack(void){ //A點(diǎn)搬運(yùn)黑色塊 OAtoOB(); OBtoOC(); OCtoOD(); if(blacknum==0){ FirstOtoMarkArea(); } else{ NoFirstOtoMarkArea(); } delay(10); switch(blacknum){ case0: DownBottom(); break; case1: DownMiddle(); break; case2: DownTop(); break; default: Stop(); break; } for(i=0;i<7;i++){ MoveMiddleQti(); } Turn180(); DOtoStartLine(); for(i=0;i<50;i++){ MoveMiddleQti(); } Turn180(); blacknum++;}voidACarryYellow(void){FirstOtoMarkArea(); DownBottom();for(i=0;i<5;i++){ MoveMiddleQti();}Turn180();AOtoStartLine();for(i=0;i<50;i++){ MoveMiddleQti();}Turn180();Stop();yellownum++;}voidCarryATop(void){ while(!MoveMiddleQti()); while(!MoveEdgeQti()); BlackBack(10); Stop(); OCtoOA(); SetDistance(); CatchTop(); RecogColorAction(); delay(200); CurrentColor(); RecogColorFinish(); switch(currentcolor){ caseWhite: ACarryWhite(); break; caseBlack: ACarryBlack();//拿a點(diǎn)頂端有兩種情況 break; default: OAtoOO(); for(i=0;i<90;i++){ MoveMiddleQti(); } Stop(); CorrectPosition(); Stop(); DownBottom(); Turn180(); fangsum++; break; } currentcolor=0;}//其他情況voidCarryAMiddle(void){ while(!MoveMiddleQti()); while(!MoveEdgeQti()); BlackBack(10); Stop(); OCtoOA(); SetDistance(); CatchMiddle(); RecogColorAction(); delay(500); CurrentColor(); RecogColorFinish(); switch(currentcolor){ caseWhite: ACarryWhite(); break; caseBlack: ACarryBlack();//兩種情況 break; default: OAtoOO(); for(i=0;i<80;i++){ MoveMiddleQti(); } switch(fangsum){ case0: for(i=0;i<10;i++){ MoveMiddl