基于AVR單片機碼垛機器人設(shè)計

基于AVR單片機碼垛機器人設(shè)計目錄TOC\o"1-3"\h\u7155第一章緒論 461871.1課程設(shè)計主要目的與任務(wù) 4281371.2課程設(shè)計要求 4189281.3課程設(shè)計的意義 410435第二章硬件設(shè)計 6213802.1硬件電路的系統(tǒng)概述 6240022.2主芯片 7161172.3多自由度機械手 9320701.認(rèn)識角度舵機 9142142.零點定義 1062403.姿態(tài)調(diào)試 10147094.機械手應(yīng)用 12296945.角度舵機與Atmega328p的接線 13289192.4伺服電機 1388281.認(rèn)識伺服電機 13189622.伺服電機的調(diào)零 13160183.伺服電機與Atmega328p的接線 14572.5QTI傳感器 14314921.認(rèn)識QTI傳感器 14132692.QTI傳感器的引腳定義 1452433.QTI傳感器的性能參數(shù) 15320404.QTI傳感器與Atmega328p的接線 15244292.6顏色傳感器 15284581.認(rèn)識TCS230顏色傳感器 15210622.TCS230傳感器的引腳定義 16108283.TCS230傳感器的特點 16241084.TCS230顏色傳感器的工作原理 16295425.白平衡 17130786.TCS230顏色傳感器與Atmega328p的接線 18327、TCS230顏色傳感器測試 1856672.7超聲波傳感器 19144331.認(rèn)識超聲波傳感器 19209332.超聲波傳感器的工作原理 19159863.超聲波傳感器與Atmega328p的接線 2025163第三章軟件設(shè)計 21153103.1項目的主流程圖 2161593.2Arduino語言及它專門定義的函數(shù) 22248653.3顏色傳感器的檢測程序 24255603.4QTI傳感器的檢測程序 2570753.5超聲波傳感器的檢測程序 2626465附錄1實物圖 2720983附錄2程序 294941、主程序 28197502、#include"MsTimer2.h" 10192753、顏色傳感器的應(yīng)用程序 105221434、QTI傳感器的應(yīng)用 11198775、機械手的應(yīng)用程序 11264936、伺服電機的應(yīng)用程序 12043137、QTI傳感器的應(yīng)用程序 120第一章緒論1.1課程設(shè)計主要目的與任務(wù)本課題是基于AVR單片機碼垛機器人設(shè)計，主要是硬件電路的設(shè)計和軟件電路的設(shè)計。主要的目的就是掌握ATmega328p處理器為核心的Openduino控制板中硬件電路的設(shè)計方法；單片機軟件程序的編寫和調(diào)試方法；軟件Eclipse、串口調(diào)試助手、OpenMachineHand的使用方法。本課題的主要任務(wù)就是通過傳感器的使用，設(shè)計一個智能的搬運機器人。主要的任務(wù)是通過各種傳感器的使用使得機器人更加智能化。劃分為這幾個模塊：1、能夠沿規(guī)定的路線或者尋跡直行；2、機械手臂能夠抓去物塊；3、能夠識別出物塊的顏色；4、能夠測量物塊與自身的距離，并作出相應(yīng)的判斷；5、能夠把9個色塊，搬運到目標(biāo)區(qū)域；搬運完成能回到起始區(qū)域。1.2課程設(shè)計要求1、掌握ATmega328p處理器為核心的Openduino控制板等硬件電路設(shè)計方法。2、熟練掌握單片機軟件程序的編寫和調(diào)試方法。3、掌握軟件Eclipse、串口調(diào)試助手、OpenMachineHand的使用。1.3課程設(shè)計的意義本課程設(shè)計是培養(yǎng)學(xué)生綜合運用所學(xué)理論知識、發(fā)現(xiàn)問題、提出問題、分析問題、解決實際問題、鍛煉實踐能力的重要環(huán)節(jié),是對學(xué)生實際工作能力的具體訓(xùn)練和考察過程。隨著科學(xué)技術(shù)發(fā)展的日新日異，單片機已經(jīng)成為當(dāng)今計算機應(yīng)用中空前活躍的領(lǐng)域，在生活中可以說得是無處不在。因此作為二十一世紀(jì)的大學(xué)來說掌握單片機的開發(fā)技術(shù)是十分重要的?；仡櫰鸫舜握n程設(shè)計，我仍感慨頗多，從選題到定稿，從理論到實踐，這幾個星期，可以說得是苦多于甜，但是可以學(xué)到很多很多的的東西，同時不僅可以鞏固了以前所學(xué)過的知識，而且學(xué)到了很多在書本上所沒有學(xué)到過的知識。通過這次課程設(shè)計使我懂得了理論與實際相結(jié)合是很重要的，只有理論知識是遠(yuǎn)遠(yuǎn)不夠的，只有把所學(xué)的理論知識與實踐相結(jié)合起來，從理論中得出結(jié)論，才能真正為社會服務(wù)，從而提高自己的實際動手能力和獨立思考的能力。在設(shè)計的過程中遇到問題，可以說得是困難重重，這畢竟第一次做的，難免會遇到過各種各樣的問題，同時在設(shè)計的過程中發(fā)現(xiàn)了自己的不足之處，對以前所學(xué)過的知識理解得不夠深刻，掌握得不夠牢固，比如說不懂一些元器件的使用方法，對單片機匯編語言掌握得不好……通過這次課程設(shè)計之后，一定把以前所學(xué)過的知識重新溫故。通過這次單片機課程設(shè)計，我不僅加深了對單片機理論的理解，將理論很好地應(yīng)用到實際當(dāng)中去，而且我還學(xué)會了如何去培養(yǎng)我們的創(chuàng)新精神，從而不斷地戰(zhàn)勝自己，超越自己。創(chuàng)新可以是在原有的基礎(chǔ)上進行改進，使之功能不斷完善，成為真己的東西。硬件設(shè)計2.1硬件電路的系統(tǒng)概述相對于智能搬運機器人來說，碼垛機器人系統(tǒng)地用到多種傳感器來完成碼垛色塊任務(wù)，有更好的穩(wěn)定性。Openduino控制器的核心是ATmega328p處理器，可以滿足系統(tǒng)的復(fù)雜性要求。系統(tǒng)中用到的傳感器有用來循跡的QTI傳感器，用來測距的超聲波傳感器，用來識別色塊顏色的顏色傳感器。碼垛機器人控制平臺的架構(gòu)如下圖所示。圖2-1碼垛機器人系統(tǒng)架構(gòu)碼垛機器人需要執(zhí)行循線，測距和抓取色塊等動作，循線過程中需要測距，抓取色塊后才進行顏色識別，并把物體搬運到目標(biāo)區(qū)域。特色描述：▲支持USB連接線在集成開發(fā)環(huán)境下調(diào)試和下載程序；▲既可用USB接口供電，不需外接電源，也可以使用外部6VDC輸入；▲獨立的電機驅(qū)動電源設(shè)計；▲各種傳感器和電子元器件標(biāo)準(zhǔn)接口（例如：溫濕度、紅外線、超聲波、熱敏電阻、光敏電阻、GPS等）；▲CMUcam攝像頭和液晶顯示屏接口；▲16路數(shù)字舵機接口；▲兩路直流電機和兩路微型步進電機接口（無需額外驅(qū)動）；▲8路專用數(shù)字IO擴展口、6路PWM輸出、8路10位的ADC擴展接口（電機占用資源包括在內(nèi)）；▲可以獨立于PC機運行等。技術(shù)參數(shù)：▲主控芯片Atmega328▲晶振：16M▲控制電源：4~6VDC，電機驅(qū)動電源：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個指令字?！じ呒壘喼噶罴w系結(jié)構(gòu)▲131條強大指令大多數(shù)是單一時鐘周期執(zhí)行▲32個（8bit）通用工作寄存器▲完全靜態(tài)操作▲在20MHz下，數(shù)據(jù)吞吐率達(dá)到每秒20百萬條指令▲循環(huán)乘法器·高耐力非易失性內(nèi)存片段▲系統(tǒng)內(nèi)置4/8/16/32K字節(jié)自行Flash程序存儲器▲56/512/512/1K字節(jié)電可擦只讀存儲器▲512/1K/1K/2K字節(jié)內(nèi)部靜態(tài)隨機存儲器▲寫入/擦除周期：10000Flash/100000EEPROM▲數(shù)據(jù)保持：20年·外圍設(shè)備特征▲2個獨立預(yù)分頻器和比較模式的8位定時器/計數(shù)器▲1個獨立預(yù)分頻器，比較模式和捕獲模式的16位定時器/計數(shù)器▲獨立振蕩器的實時計數(shù)器▲6個PWM通道▲8通道的10位ADC在MLF封裝溫度測量▲可編程串行通用同步異步收發(fā)器▲主/從設(shè)備串行外部接口▲按字節(jié)2線串行接口▲可編程的獨立片上振蕩器的看門狗定時器▲片上模擬比較器▲引腳改變的中端與喚醒·特殊微處理器功能▲電源復(fù)位和可編程低功率欠壓檢測▲內(nèi)部校準(zhǔn)振蕩器▲外部/內(nèi)部中斷源▲6種睡眠模式2.3多自由度機械手1.認(rèn)識角度舵機本設(shè)計是利用5個角度舵機相互協(xié)作、共同作用來夾取物塊。簡單地說，通過機械手姿態(tài)調(diào)試助手改變舵機的脈沖值，使機械手的所在的位置發(fā)生改變，從而實現(xiàn)抓取物塊，放置物塊的能力。如圖3-2所示：圖2-3多自由度機械手1號舵機接SVO1接口，2號舵機接SVO2接口，3號舵機接SVO3接口，4號舵機接SVO4接口，5號舵機接SVO5接口。如下圖所示：圖2-4舵機接口2.零點定義手爪舵機的零點：當(dāng)兩個手爪旋轉(zhuǎn)部位成180度時，舵機對應(yīng)的角度就是手爪的零點位置。關(guān)節(jié)舵機的零點：當(dāng)機械臂與安裝底盤面成90度夾角時，每個舵機的角度就是對應(yīng)舵機的零點位置。每個舵機均可從0度位置旋轉(zhuǎn)到180度，從圖上看，舵機偏左轉(zhuǎn)（相對機器人是后仰）是從90度向0度轉(zhuǎn)，舵機偏右轉(zhuǎn)（相對機器人是前俯）是從90度向180度轉(zhuǎn)。注意：上述的90度是視覺上相對的90度，不一定是每個舵機90度脈沖對應(yīng)的位置。圖2-5機械手臂零點位置姿態(tài)調(diào)試為了方便機械手姿態(tài)控制，編寫調(diào)試機械手姿態(tài)對應(yīng)的上位機和下位機軟件，軟件代碼開放，控制板使用的是OpenduinoBoardVer3.0，上位機軟件是使用在WindowsXP上運行的VC++6.0開發(fā)環(huán)境編寫的機械手姿勢調(diào)試助手，下位機軟件是使用配置成可開發(fā)AVR程序的Eclipse開發(fā)環(huán)境編寫的，上、下位機必須配合使用才可完成調(diào)試。將機械手上各個舵機正確連接到控制板（SVO1~SVO8接口），給控制板接上外部電源（鋰電池或電源適配器），按一下系統(tǒng)復(fù)位鍵，再將電源開關(guān)撥到ON2位置，這時候可以發(fā)現(xiàn)機械手被控制到一個基本處于零點的姿態(tài)。機械手姿勢調(diào)試助手的使用步驟：1）點擊“連接串口”按鈕，若成功打開串口，則按鈕會顯示“斷開串口”2）串口打開后就可以嘗試點擊“握手設(shè)備”按鈕，若握手成功，如否則檢查串口是否正確，是否下載對應(yīng)下位機程序，再次握手。3）首先測試調(diào)節(jié)兩個并聯(lián)的舵機角度，它們的角度必須互補，否則容易損壞舵機，因此上位機軟件只用一個調(diào)節(jié)閥來控制，只給出其中一個舵機的脈寬值，另一個舵機的脈寬值自然就可以得到了。4)鼠標(biāo)左鍵點擊并按住SVO1&2對應(yīng)的調(diào)節(jié)閥，可以左右調(diào)節(jié)，單位增量為27，右側(cè)的編輯框中的數(shù)字也相應(yīng)改變，數(shù)字表示控制通道SVOn（n=1~8）的PWM信號的正脈寬值，單位是0.5us，如圖2-6中調(diào)節(jié)并聯(lián)舵機的脈寬為3135，那么就是3135*0.5us=1567.5us≈1.6ms，然后點擊“刷新舵機”按鈕，脈寬數(shù)據(jù)3135才會被發(fā)送到控制板，機械手會立即轉(zhuǎn)動到脈寬對應(yīng)的角度。其他通道的舵機控制方法一樣。當(dāng)點擊“復(fù)位機械手”按鈕，所有脈寬值將變?yōu)槟J(rèn)值3000。圖2-6機械手姿態(tài)調(diào)試助手機械手應(yīng)用思路：每個機械手的初始化位置都是通過上位機軟件調(diào)節(jié)到零點位置，之后的每個姿態(tài)都是以零點位置為參考位置，偏離零點位置的角度值也是通過上位機軟件調(diào)節(jié)而來，脈寬值以27為單位增減。例如某個三自由度機械手的零點位置對應(yīng)脈寬值為：3135（SVO1&2）、3162（SVO3）、3081（SVO4）、2919（SVO5）。通過軟件調(diào)節(jié)到某個目的姿態(tài)，其對應(yīng)脈寬值為2703（SVO1&2）、2270（SVO3）、3757（SVO4）、3622（SVO5）。如圖2-7所示：圖2-7則：變化量=（零點值-目標(biāo)值）/27即：SVO1&2的變化量=（3135-2703）/27=16；SVO3的變化量=（3162-2270）/27=33；SVO4的變化量=（3081-3757）/27=-25；SVO5的變化量=（2919-2703）/27=-26；角度舵機與Atmega328p的接線表1角度舵機與Atmega328p的接線角度舵機Atmega328p引腳角度舵機Atmega328p引腳1號舵機SVO14號舵機SVO42號舵機SVO25號舵機SVO53號舵機SVO32.4伺服電機認(rèn)識伺服電機伺服電機(servomotor)又稱連續(xù)旋轉(zhuǎn)電機，是指在伺服系統(tǒng)中控制機械元件運轉(zhuǎn)的發(fā)動機，是一種補助馬達(dá)間接變速裝置。伺服電機可使控制速度，位置精度非常準(zhǔn)確，可以將電壓信號轉(zhuǎn)化為轉(zhuǎn)矩和轉(zhuǎn)速以驅(qū)動控制對象。伺服電機轉(zhuǎn)子轉(zhuǎn)速受輸入信號控制，并能快速反應(yīng)，在自動控制系統(tǒng)中，用作執(zhí)行元件，且具有機電時間常數(shù)小、線性度高、始動電壓等特性，可把所收到的電信號轉(zhuǎn)換成電動機軸上的角位移或角速度輸出。伺服電機的調(diào)零所謂伺服電機的調(diào)零是指發(fā)送一個特定的控制信號（零點標(biāo)定信號）給伺服電機時，讓電機處于靜止的狀態(tài)。由于伺服電機在工廠沒有預(yù)先調(diào)整，它們在接收零點標(biāo)定信號時可能會轉(zhuǎn)動，因此要用螺絲刀調(diào)節(jié)伺服電機模塊內(nèi)的調(diào)節(jié)電阻讓伺服電機保持靜止，這就是伺服電機的調(diào)零過程。圖2-8、圖2-9就是伺服電機電位器的零點調(diào)節(jié)以及零點標(biāo)定信號的時序圖。圖2-8伺服電機電位器的零點調(diào)節(jié) 圖2-9零點標(biāo)定信號的時序圖如果電機沒有進行零點標(biāo)定，它的連接喉就會轉(zhuǎn)動，而且你會聽到里面馬達(dá)的響聲，否則就不會轉(zhuǎn)動且沒有響聲。當(dāng)電機沒有進行零點標(biāo)定時，將螺絲刀插入伺服電機的電位器的調(diào)節(jié)孔，輕輕地轉(zhuǎn)動螺絲刀調(diào)節(jié)電位器進行零點標(biāo)定。3.伺服電機與Atmega328p的接線表2伺服電機與Atmega328p的接線序號伺服電機Atmega328p的引腳1左電機2｜PD22右電機19｜A52.5QTI傳感器1.認(rèn)識QTI傳感器本設(shè)計使用的QTI(QuickTrackInfrared)傳感器是一種使用光電接收管來探測物體表面反射光強度的傳感器，當(dāng)QTI傳感器面對暗淡的物體表面時，反射光強度很低。當(dāng)QTI傳感器面對明亮的物體表面時，反射光強度很高，不同程度反射光對應(yīng)傳感器不同的輸出值，即對應(yīng)不同亮度的物體時QTI的信號引腳會輸出不同的電平信號。本設(shè)計中應(yīng)用到的QTI傳感器探測到黑色物體時輸出為高電平（+5V），探測到白色物體時輸出為低電平（0V），如圖2-10所示。圖2-10QTI傳感器2.QTI傳感器的引腳定義1）SIG：信號輸出；2）VCC：5V直流電源；3）GND：電源地線；3.QTI傳感器的性能參數(shù)▲工作溫度：-40℃-85℃；▲工作電壓：5V；▲連續(xù)電流：50mA；▲功耗：100mW；▲佳探測距離：5~10mm；▲佳距離下的大散射角度：65o；▲響應(yīng)時間：上升沿時間10us,下降沿時間50us。QTI傳感器與Atmega328p的接線表3QTI傳感器與Atmega328p的接線QTI的個數(shù)QTI傳感器Atmega328p引腳1靠近左電機14｜A02靠近超聲波左側(cè)15｜A13靠近超聲波右側(cè)16｜A24靠近右電機17｜A35靠近電池盒18｜A42.6顏色傳感器1.認(rèn)識TCS230顏色傳感器顏色傳感器就是通過檢測某個顏色中三原色的比例來識別該顏色的，本設(shè)計中使用的是TCS230顏色傳感器，顏色傳感器的外觀如圖2-11所示；顏色傳感器的輸出信號（由OUT引腳輸出）是數(shù)字量，可以實現(xiàn)每個顏色信道10位（5V/210）以上的轉(zhuǎn)換精度，可以驅(qū)動標(biāo)準(zhǔn)的TTL或者CMOS邏輯輸入，因此可直接與微控制器的I/O引腳連接。顏色傳感器實物顏色傳感器上自帶了兩個高亮白色LED，有助于提高顏色識別的準(zhǔn)確性。圖2-11TCS230顏色傳感器2.TCS230傳感器的引腳定義1）S0、S1：用于選擇輸出比例因子或者電源關(guān)斷模式，不同的輸出比例因子可調(diào)整輸出頻率，以適應(yīng)不同的需求；2）S2、S3：用于選擇濾波器的類型；3）LED：用于點亮兩個LED燈；4）OUT：頻率輸出引腳，該顏色傳感器的典型輸出頻率范圍為2Hz（0.5s）～500kHz（2us）；5）GND：芯片接地引腳；6）+5V、VDD：5V電源。3.TCS230傳感器的特點▲可完成高分辨率的光照度/頻率轉(zhuǎn)換；▲色彩和輸出頻率可編程調(diào)整；▲單電源工作，工作電壓范圍為2.7V-5.5V；▲具備掉電恢復(fù)功能；▲50KHZ時非線性誤差的典型值為0.2%。4.TCS230顏色傳感器的工作原理顏色傳感器就是通過檢測某個顏色中三原色的比例來識別該顏色的，首先打開LED燈，選擇輸出比例因子，然后依次選定不同的顏色濾波器，每選通一個濾波器就檢測其輸出引腳輸出的不同頻率（即光強）的方波脈沖數(shù)，最后根據(jù)得到的三原色的脈沖數(shù)比例判斷顏色。S0、S1及S2、S3的不同組合如表4所示：表4S0、S1、S2、S3組合選項S0S1輸出比例因子S2S3顏色濾波器類型00關(guān)閉電源00紅色（R）011:5001藍(lán)色（B）101:510無111:111綠色（G）從理論上來講，白色是等量的紅色、綠色和藍(lán)色混合而成的，但TCS230傳感器對這三種顏色的敏感度是不同的，導(dǎo)致TCS230傳感器的RGB輸出并不相等，因此在使用前必須進行白平衡調(diào)整。5.白平衡在本設(shè)計中所謂的白平衡就是告訴機器人什么是白色，使得顏色傳感器對“白色”中的三原色RGB輸出是相等的。通過白平衡調(diào)整可以得到在顏色識別時需要用到的通信道的時間基準(zhǔn)。白平衡的過程為：在顏色傳感器前適當(dāng)位置放置一個白色物塊，打開LED燈，選擇輸出比例因子，依次通過紅色、綠色和藍(lán)色濾波器，對每個通道的脈沖計數(shù)到255后關(guān)閉通道，最后分別得到每個通道所用的時間，這個時間就是識別顏色時要用的基準(zhǔn)時間。白平衡的控制流程如圖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顏色傳感器測試根據(jù)顏色傳感器的特性，在傳感器的使用過程中主要需要知道時間基準(zhǔn)和脈沖數(shù)，所以在控制程序中可以使用定時器0來統(tǒng)計時間，外部中斷1來判斷一次脈沖。定時器0是8位定時器，而定時的基本單位不能過大，在統(tǒng)計時間的過程中可以采用定時器0多次累計的方式計算時長。在代碼中，對定時器0進行8分頻設(shè)置，從而定時的基本單位為0.5us。2.7超聲波傳感器認(rèn)識超聲波傳感器本設(shè)計中使用的是DM-S28018-B,與普通的超聲波一樣都是利用聲波反射原理。DM-S28018-B提供了精確的、非接觸式的距離測量，測量的范圍從2cm到3.3m,它非常方便與微控制器連接，只需要兩個I/O口進行控制。左邊是發(fā)射頭右邊是接收頭，其工作頻率為12M，從左到右，VCC接5V電源，Trig是接收啟動信號引腳，Echo是信號輸出引腳，Out是空腳（防盜），GND接地。DM-S28018-B超聲波測距傳感器的實物圖如圖2-13所示。圖2-13DM-S28018-B超聲波傳感器的工作原理超聲波模塊上電后，首先在Trig引腳給超聲波模塊發(fā)送一啟動信號（10us），超聲波模塊接收到啟動信號后，就在發(fā)射頭發(fā)出超聲波信號，同時將Echo引腳置為高電平，這時控制器就必須開始計時，超聲波信號在空氣中遇到障礙物后反射回來，當(dāng)接收頭檢測到反射回來的超聲波信號時，馬上將Echo引腳置為低電平，Echo引腳高電平持續(xù)的時間t用到轉(zhuǎn)換為距離的計算中。若超過18.5ms還沒檢測到超聲波信號，也將Echo引腳置為低電平，準(zhǔn)備下一次測量。超聲波測距原理如圖2-14所示。圖2-14超聲波測距原理超聲波測距原理超聲波在空氣中傳播速度為340m/s，則測量距離為（t*340）/2。超聲波傳感器與Atmega328p的接線表６超聲波傳感器與Atmega328p的接線超聲波模塊引腳連接Atmega328P引腳ＶＣＣＶＣＣＧＮＤＧＮＤTRIGＰＢ２ECHOＰＢ０ＯＵＴ空軟件設(shè)計3.1項目的主流程圖由碼垛機器人的場地圖可以看出碼垛機器人執(zhí)行的任務(wù)復(fù)雜，需要控制的傳感器較多，軟件編程自然就會有點難度，所以軟件架構(gòu)設(shè)計非常重要。在硬件工作正常和連線正確的前提下，軟件編程決定任務(wù)能否被更穩(wěn)定、更快速地完成。碼垛機器人需要沿著直線走，轉(zhuǎn)彎，抓取色塊，放下色塊等等多種動作，如果軟件結(jié)構(gòu)是使用順序結(jié)構(gòu)，顯然行不通。碼垛機器人軟件系統(tǒng)架構(gòu)是采用分支結(jié)構(gòu)來執(zhí)行，使得系統(tǒng)程序編程變得簡單，有條理。碼垛機器人的場地圖、軟件架構(gòu)流程圖如下圖所示。圖3-1場地圖圖3-2軟件架構(gòu)流程圖3.2Arduino語言及它專門定義的函數(shù)Arduino語言建立在C/C++基礎(chǔ)上，并把相關(guān)的一些參數(shù)設(shè)置函數(shù)化，不用我們?nèi)チ私庥布牡讓?，讓不了解AVR單片機（微控制器）的朋友也能輕松上手。下面簡單的注釋一下Arduino語言以及它專門定義的函數(shù)。數(shù)據(jù)類型：1boolean布爾類型7char字符類型2byte字節(jié)類型8int整數(shù)類型3array數(shù)組9float浮點實數(shù)類型4long長整型10double雙浮點實數(shù)5string字符串11unsignedint無符號整型6void空12unsignedlong無符號長整型常量：▲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）?！陨蠟榛A(chǔ)c語言的關(guān)鍵字和符號，大家可以了解，具體使用可以結(jié)合實驗的程序。結(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?！鴇igitalWrite(pin,value)：數(shù)字IO口輸出電平函數(shù)，pin表示為0～13，value表示為HIGH或LOW。比如定義HIGH可以驅(qū)動LED。▲intdigitalRead(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?？捎糜陔姍CPWM調(diào)速或音樂播放。時間函數(shù)▲delay(ms)：延時函數(shù)（單位ms）?！鴇elayMicroseconds(us)：延時函數(shù)（單位us）。數(shù)學(xué)函數(shù)：▲min(x,y)：求最小值▲max(x,y)：求最大值▲abs(x)：計算絕對值▲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)：開根號3.3顏色傳感器的檢測程序圖識別顏色的流程圖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傳感器的檢測程序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超聲波傳感器的檢測程序intmain(void){init();setup();delay(200);while(1){dis=DistanceDetection();Serial.println((float)dis,1);delay(2000);}}附錄1實物圖附錄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)前色塊個數(shù)uint8_trednum=0;uint8_tblacknum=0;uint8_tbluenum=0;uint8_tAPnum=3;//A點疊放的色塊個數(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個是前方QTI，后面是最后緊挨一起的QTIconstinttrig=12;//超聲波Trig引腳constintecho=10;//超聲波Echo引腳charfunc=1;//顏色傳感器變量intrefer_time[3]={0,0,0};//白平衡得到的基準(zhǔn)時間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è)置的舵機角度，servoangle[0]存放并聯(lián)舵機角度，單位增量為27uint16_tservoangle[7]={3000,3000,3000,3000,3000,3000,3000};//存放用戶設(shè)置的舵機角度，servoangle[0]存放并聯(lián)舵機角度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)（兩個輪子一起） 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; }}/**//檢測中間左QTI是否在黑線內(nèi)*/booleanIsMLeftQtiBlack(void){ if(GetQtiStatus(qtipin[1])==true) { delay(2); if(GetQtiStatus(qtipin[1])==true) { returntrue; } else { returnfalse; } } else { returnfalse; }}/**//檢測中間右QTI是否在黑線內(nèi)*/booleanIsMRightQtiBlack(void){ if(GetQtiStatus(qtipin[2])==true) { delay(2); if(GetQtiStatus(qtipin[2])==true) { returntrue; } else { returnfalse; } } else { returnfalse; }}/**//檢測輪子左QTI是否在黑線內(nèi)*/booleanIsELeftQtiBlack(void){ if(GetQtiStatus(qtipin[0])==true) { delay(2); if(GetQtiStatus(qtipin[0])==true) { returntrue; } else { returnfalse; } } else { returnfalse; }}/**//檢測輪子右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())//左電機qti在白色區(qū)域 { SpinLeft();//左轉(zhuǎn) } while(!IsERightQtiBlack())//右電機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){//機械手抓取頂部物體 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){//機械手抓取中部物體 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){//機械手抓取底部物體 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){//機械手放頂部物體 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){//機械手放中部物體 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){//機械手放底部物體 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){//顏色識別角度舵機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){//識別顏色角度舵機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){//識別顏色完成手臂狀態(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()){//檢測輪子右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點搬運白色塊 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點搬運黑色塊 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點頂端有兩種情況 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