光電組-中南大學(xué)比亞迪雙魚座2012技術(shù)報(bào)告(盧紅海)

第七屆“飛思卡爾”杯全國大學(xué)生 IX第七屆“飛思卡爾”杯全國大學(xué)生智能汽車競賽技術(shù)報(bào)告學(xué)校：中南大學(xué)隊(duì)伍名稱：比亞迪雙魚座2012參賽隊(duì)員：熊凱邵明明盧紅海帶隊(duì)教師：王擊徐德剛關(guān)于技術(shù)報(bào)告和研究論文使用授權(quán)的說明 本人完全了解第七屆“飛思卡爾”杯全國大學(xué)生智能汽車競賽關(guān)保留、使用技術(shù)報(bào)告和研究論文的規(guī)定，即：參賽作品著作權(quán)歸參賽者本人，比賽組委會(huì)和飛思卡爾半導(dǎo)體公司可以在相關(guān)主頁上收錄并公開參賽作品的設(shè)計(jì)方案、技術(shù)報(bào)告以及參賽模型車的視頻、圖像資料，并將相關(guān)內(nèi)容編纂收錄在組委會(huì)出版論文集中。參賽隊(duì)員簽名： 帶隊(duì)教師簽名： 日期：

目錄第一章引言 -1-1.1背景介紹 -1-1.2整車設(shè)計(jì)思路 -1-1.3文章結(jié)構(gòu) -2-第二章機(jī)械結(jié)構(gòu)設(shè)計(jì) -3-2.1總體思路 -3-2.2前輪的調(diào)節(jié) -4-2.3底盤調(diào)節(jié) -4-2.4傳感器的安裝 -5-2.3舵機(jī)的安裝 -5-2.4測速傳感器的安裝 -6-2.5差速機(jī)構(gòu)調(diào)整 -7-2.6硬件電路板的安裝 -8-第三章硬件電路設(shè)計(jì) -9-3.1硬件設(shè)計(jì)總體方案 -9-3.2電源管理模塊 -10-3.3主控制器電路 -11-3.4激光傳感器電路 -11-3.5電機(jī)驅(qū)動(dòng)電路 -12-3.6人機(jī)交互電路 -13-第四章軟件設(shè)計(jì) -14-4.1總體設(shè)計(jì) -14-4.1.1層次設(shè)計(jì) -14-4.1.2總體流程 -14-4.2位置確定 -15-4.3舵機(jī)控制 -16-4.3.1搖頭舵機(jī)變結(jié)構(gòu)控制 -16-4.4速度控制 -18-4.4.1電機(jī)控制 -18-4.4.2速度給定策略 -19-4.5特殊情況處理 -20-4.5.1十字彎 -20-4.5.2坡道 -20-4.5.3起跑線 -21-5.2調(diào)試 -22-5.2.1系統(tǒng)硬件調(diào)試 -22-5.2.2傳感器調(diào)試 -23-5.2.3軟件系統(tǒng)調(diào)試 -23-第六章賽車具體參數(shù) -25-第七章總結(jié)與展望 -26-7.1總結(jié) -26-7.2展望 -26-參考文獻(xiàn) -28-致謝 -28-HYPERLINK附錄：程序代碼 Ⅰ第一章引言1.1背景介紹全國大學(xué)生智能汽車競賽是以智能汽車為競賽平臺(tái)的多學(xué)科專業(yè)交叉的創(chuàng)意性科技競賽，是面向全國大學(xué)生的一種具有探索性的工程實(shí)踐活動(dòng)，該競賽是在規(guī)定的模型汽車平臺(tái)上，使用飛思卡爾半導(dǎo)體公司的8位、16位和32位微控制器作為核心控制模塊，通過增加道路傳感器、電機(jī)驅(qū)動(dòng)電路以及編寫相應(yīng)軟件，制作一個(gè)能夠自主識(shí)別道路的模型汽車，按照規(guī)定路線行進(jìn)，以完成時(shí)間最短者為優(yōu)勝。[1]全國大學(xué)生智能汽車競賽已成功舉辦了六屆，得到了高校師生的高度評(píng)價(jià)，已發(fā)展成全國30個(gè)省市自治區(qū)170余所高校廣泛參與的全國大學(xué)生智能汽車競賽。已被教育部批準(zhǔn)列入國家教學(xué)質(zhì)量與教學(xué)改革工程資助項(xiàng)目中9個(gè)科技人文競賽之一。該競賽激發(fā)大學(xué)生從事科學(xué)研究與探索的興趣和潛能,倡導(dǎo)理論聯(lián)系實(shí)際、求真務(wù)實(shí)的學(xué)風(fēng)和團(tuán)隊(duì)協(xié)作的人文精神。[1]1.2整車設(shè)計(jì)思路智能車競賽要求設(shè)計(jì)一輛以組委會(huì)提供車模為主體的可以自主尋線的模型車，根據(jù)比賽規(guī)則和要求，本智能汽車采用光電方案，用650nm激光發(fā)射、180KHz接收模塊作為路徑檢測傳感器，飛卡爾公司32位微控制器MK60DN512[2]為系統(tǒng)控制核心，通過光電編碼器檢測速度，由微控制器通過數(shù)據(jù)處理得出賽道信息并根據(jù)當(dāng)前情況給出舵機(jī)轉(zhuǎn)角和電機(jī)轉(zhuǎn)速，使智能車能在賽道上快速平穩(wěn)的行駛。系統(tǒng)由電源模塊、控制模塊、激光傳感器模塊、測速模塊、電機(jī)驅(qū)動(dòng)模塊、舵機(jī)模塊、無線模塊等組成。系統(tǒng)總體結(jié)構(gòu)框圖如圖1-1。路經(jīng)檢測路經(jīng)檢測微控制器模塊PC機(jī)無線模塊速度檢測舵機(jī)指示燈及撥碼開關(guān)電機(jī)圖1-1系統(tǒng)總體結(jié)構(gòu)框圖1.3文章結(jié)構(gòu)技術(shù)報(bào)告分七個(gè)章節(jié)：第一章節(jié)主要是對(duì)模型車設(shè)計(jì)制作的主要思路以及實(shí)現(xiàn)的技術(shù)方案概要說明，提出技術(shù)報(bào)告的行文框架。第二章介紹了賽車機(jī)械改造的總體思路，并詳細(xì)說明了機(jī)械結(jié)構(gòu)調(diào)整情況。第三章說明系統(tǒng)的硬件實(shí)現(xiàn)。第四章說明方向控制和速度控制算法設(shè)計(jì)。第五章介紹仿真和調(diào)試的方法。第六章是賽車的具體參數(shù)。第七章總結(jié)了整個(gè)制作過程中的創(chuàng)新點(diǎn)和不足之處，提出了下屆備賽過程的努力方向。第二章機(jī)械結(jié)構(gòu)設(shè)計(jì)隨著智能車的速度不斷提升，智能車的機(jī)械結(jié)構(gòu)越來越被重視，智能車機(jī)械結(jié)構(gòu)的好壞對(duì)車的速度有著很大的影響。本智能車如圖2-1。圖2-1智能汽車外貌2.1總體思路關(guān)于賽車機(jī)械結(jié)構(gòu)的調(diào)整，我們主要從以下幾個(gè)方面考慮：車體重量：比賽規(guī)則規(guī)定，智能車的驅(qū)動(dòng)電機(jī)和傳動(dòng)機(jī)構(gòu)不允許更改，意味著賽車的最大驅(qū)動(dòng)能力是一定的。要想提高賽車的平均速度，必須提高其加速和制動(dòng)性能，在驅(qū)動(dòng)力一定的情況下，盡量減輕車體的重量會(huì)提高賽車的加減速性能。另外，更輕的重量也會(huì)使賽車轉(zhuǎn)向更加靈活。重心高低：導(dǎo)引線彎道的最小半徑為0.5米，要使得智能車能在彎道上高速通過，必須防止側(cè)滑和側(cè)翻，尤其是側(cè)翻。重心位置是影響側(cè)翻的最關(guān)鍵的因素，所以設(shè)計(jì)中應(yīng)盡量降低智能車的重心。為此，我們采用了低位電路板的布局，同時(shí)設(shè)計(jì)了強(qiáng)度高質(zhì)量輕的合金材料作激光傳感器安裝架，且前盡量減輕前排管的重量。轉(zhuǎn)向靈活性：轉(zhuǎn)向的靈活性會(huì)直接的影響到智能車的性能，由于舵機(jī)具有機(jī)械延時(shí)，另外加上控制舵機(jī)的周期，舵機(jī)要轉(zhuǎn)到給定的目標(biāo)角度會(huì)有一定的延時(shí)，要使轉(zhuǎn)向快，就要盡量減小舵機(jī)的延時(shí)。另外，后輪的差速的性能對(duì)轉(zhuǎn)向性能的影響也很大。2.2前輪的調(diào)節(jié)光電組使用的是組委會(huì)規(guī)定的B車模，這種車模前輪只有三個(gè)可調(diào)部位：Camberlink，前束及前懸掛。 Camberlink是指上搖臂(或者推桿)兩端的固定位置。Camberlink決定了車輪的Camber角度，以及懸掛移動(dòng)時(shí)Camber角度的改變量，和懸掛系統(tǒng)的幾何特性(假設(shè)下?lián)u臂不變)。Camberlink位置會(huì)影響車輛的穩(wěn)定性和抓地力。Camber角度極大地影響小車的彎道性能。調(diào)節(jié)時(shí)要配合地盤左右高度進(jìn)行還有主銷傾角進(jìn)行調(diào)節(jié)。以四分之一或更小的步進(jìn)調(diào)節(jié)連桿，每調(diào)整一次，正反跑十字彎一次，直到過彎半徑最小為止。Toe角度(束角)是描述從車的正上方看，車輪的前端和車輛縱線的夾角。車輪前端向內(nèi)束(內(nèi)八字)，稱為Toe-in;車輪前端向外張(外八字)，稱為Toe-out。少量的前輪Toe-in(內(nèi)八字)可以使車在加速時(shí)保持穩(wěn)定。但這樣做也會(huì)減少車子在進(jìn)入彎角時(shí)的轉(zhuǎn)向反應(yīng)，同時(shí)增加加速出彎時(shí)的轉(zhuǎn)向反應(yīng)。如果需要在進(jìn)入彎角時(shí)獲得更多的轉(zhuǎn)向，可以使用Toe-out(外八字)。但這樣做會(huì)使車子在加速時(shí)、或者通過起伏路面時(shí)，變得不穩(wěn)定，偏離直線。越大角度Toe-in或Toe-out，越會(huì)減低車輛在直線行走的速度。為了使車輛行駛穩(wěn)定我們將束角調(diào)成Toe-in。前懸掛的松緊和前輪的抓地力有一定關(guān)系。在調(diào)車過程中，我們發(fā)現(xiàn)將懸掛彈簧擰松一點(diǎn)，可以解決車輛轉(zhuǎn)彎前輪跳的問題。2.3底盤調(diào)節(jié)為了降低重心，防止車輛在高速行駛時(shí)出現(xiàn)側(cè)翻，側(cè)滑等失控現(xiàn)象，我們將底盤盡可能的降低，剛好過坡道不擦跑道。底盤調(diào)節(jié)還要注意前后的高低配合和左右高度的平衡。適當(dāng)?shù)母叩团浜嫌欣谠黾忧拜喌淖サ亓Γ乐顾ξ?。左右高度平衡可以改善車輛的過彎性能。一般來講，底盤向那邊傾斜，該方向的過彎性能就好，這也是左右不對(duì)稱的原因。很多隊(duì)伍利用這種現(xiàn)象，在尾部加裝懸掛舵機(jī)來提高車輛的過彎性能，我們也試過，確實(shí)效果很明顯，特別是以前的A車。但有利就有弊，目前出現(xiàn)的主動(dòng)懸掛裝置都是開環(huán)的，其適應(yīng)性不是很好，不同的賽道，參數(shù)差異可能很大，而且一旦掛開得不合理，帶來的后果是致命的。2.4傳感器的安裝 我們的智能車采用激光作為路徑檢測傳感器。為了提高其前瞻性，我們采圖2-2智能車傳圖2-2感器架設(shè)用了高架式架設(shè)方法，這種方法會(huì)一定程度上使智能車的重心靠上，所以我們盡量命使用質(zhì)量輕又結(jié)實(shí)的合金材料作為支架。傳感器的有效檢測距離和它離地的高度及水平面的夾角直接相關(guān)，架設(shè)的越高，夾角越大，理論上檢測距離越遠(yuǎn)，但是考慮到車的整體結(jié)構(gòu)穩(wěn)定性和重心，所以其架設(shè)高度和角度只能在一定范圍內(nèi)調(diào)節(jié)，我們前排傳感器高度為50CM，和水平面夾角為10°左右。2.3舵機(jī)的安裝舵機(jī)響應(yīng)速度是整車過彎速度的一個(gè)瓶頸。為了加快車輪轉(zhuǎn)向速度，我們設(shè)計(jì)并安裝了舵機(jī)轉(zhuǎn)向機(jī)構(gòu)。在并非改變舵機(jī)本身結(jié)構(gòu)的條件下，改變了舵機(jī)的安裝位置，安裝更長的舵機(jī)輸出臂。采用杠桿原理，在舵機(jī)的輸出舵盤上安裝一個(gè)較長的輸出臂，將轉(zhuǎn)向傳動(dòng)桿連接在輸出臂末端。這樣就可以在舵機(jī)輸出較小的轉(zhuǎn)角下，取得較大的前輪轉(zhuǎn)角，從而提高了整個(gè)車模轉(zhuǎn)向控制的速度[2]。如圖2-3所示，這種方法是通過機(jī)械方式，利用舵機(jī)的輸出轉(zhuǎn)距余量，將角度進(jìn)行放大，加快了舵機(jī)響應(yīng)速度。圖2-3轉(zhuǎn)向舵機(jī)連接2.4測速傳感器的安裝本智能車采用光電編碼器作為測速傳感器，它能提供較高的精度并且通過一定的處理后可以識(shí)別正反轉(zhuǎn)。光電編碼器通過齒輪和后軸齒輪相連，檢測車輪轉(zhuǎn)速。圖2-23編碼器固定2.5差速機(jī)構(gòu)調(diào)整后輪差速機(jī)構(gòu)的作用是在車模轉(zhuǎn)彎的時(shí)候，減少后輪與地面之間的滑動(dòng)，使轉(zhuǎn)向輕便。車輛在正常的過彎行駛時(shí)四個(gè)輪子的轉(zhuǎn)速皆不相同，順序?yàn)椋和鈧?cè)前輪＞外側(cè)后輪＞內(nèi)側(cè)前輪＞內(nèi)側(cè)后輪。差速器的作用便是在起到傳動(dòng)的同時(shí)允許該速度差的存在[2]。差速器的調(diào)整中要注意滾珠輪盤間的間隙，過松過緊都會(huì)使差速器性能降低。過松會(huì)導(dǎo)致傳動(dòng)力不夠，即主電機(jī)轉(zhuǎn)動(dòng)時(shí)后輪不動(dòng)，只要有這種情況出現(xiàn)，會(huì)嚴(yán)重影響到智能車的加速和制動(dòng)性能，即出現(xiàn)智能車速度上升慢，制動(dòng)力不夠的情況。過緊會(huì)使轉(zhuǎn)向不輕便，甚至出現(xiàn)轉(zhuǎn)彎時(shí)后輪滑動(dòng)的現(xiàn)象，導(dǎo)致過彎速度由于差速器沒調(diào)整好而變慢。轉(zhuǎn)向不靈活還會(huì)增加前輪側(cè)滑的概率，因?yàn)椴钏倨髡{(diào)得過緊智能車在過彎時(shí)會(huì)給前輪一個(gè)向外的力，從而導(dǎo)致前輪有向外側(cè)滑的趨勢[3]。調(diào)整原則是：差速器應(yīng)調(diào)的足夠的松，但是要保證在任何測試情況下都不會(huì)出現(xiàn)傳動(dòng)力不夠的現(xiàn)象，而且為了保證測速的準(zhǔn)確性，不能出現(xiàn)兩個(gè)車輪同時(shí)都和后軸齒輪轉(zhuǎn)向相反的情況。2.6硬件電路板的安裝我們的智能車系統(tǒng)除了傳感器部分之外，電路板分兩部分，一是電機(jī)驅(qū)動(dòng)，另一塊主要含控制部分和電源管理部分，我們稱之為主控板。我們把主控板固定在舵機(jī)和和電池字之間，安裝時(shí)離底盤3MM,并且用螺絲固定，既牢固又便于散熱。電機(jī)驅(qū)動(dòng)部分則固定在電機(jī)上方，這樣可以盡量減小電機(jī)驅(qū)動(dòng)對(duì)單片機(jī)產(chǎn)生干擾。第三章硬件電路設(shè)計(jì)硬件電路是智能車的基礎(chǔ)，只有完成了穩(wěn)定可靠科學(xué)的硬件電路，才能在這個(gè)基礎(chǔ)上進(jìn)行軟件算法的設(shè)計(jì)，做出優(yōu)秀的智能車。我們的智能車的硬件設(shè)計(jì)的原則是：盡量精減，采用簡單可靠的電路，選用性能優(yōu)秀而且性價(jià)比好的芯片，設(shè)計(jì)穩(wěn)定可靠兼容性好的電路板。3.1硬件設(shè)計(jì)總體方案硬件電路主要由電源管理模塊、控制模塊、激光傳感器模塊、電機(jī)驅(qū)動(dòng)模塊、舵機(jī)驅(qū)動(dòng)、人機(jī)接口模塊等組成。采用飛思卡爾的32位微控制器MK60DN512作主控芯片，工作總線頻率100M。微控制器通過I/O口獲得激光傳感器采集的賽道信息，經(jīng)過處理后給出舵機(jī)轉(zhuǎn)角和電機(jī)轉(zhuǎn)速。通過人機(jī)接口可以方便的設(shè)置智能車運(yùn)行的參數(shù)，并獲得智能運(yùn)行狀況。電機(jī)驅(qū)動(dòng)采用用MOSFET管搭成的內(nèi)阻很小的H橋電路。下面將較詳細(xì)的介紹各模塊的電路設(shè)計(jì)。電機(jī)驅(qū)動(dòng)電機(jī)驅(qū)動(dòng)模塊光電傳感器模塊電源光電傳感器模塊電源管理模塊K60核心控制器K60核心控制器檢測速度模塊檢測速度模塊舵機(jī)控制模塊無線串口無線串口模塊3.2電源管理模塊系統(tǒng)通過標(biāo)準(zhǔn)車模用7.2V2000mAhNi-cd蓄電池供電[2]，實(shí)際充滿電后電壓則為8.2～8.5V。由于電路中的不同電路模塊所需要的工作電壓和電流容量各不相同，因此電源模塊應(yīng)該包含多個(gè)穩(wěn)壓電路，將充電電池電壓轉(zhuǎn)換成各個(gè)模塊所需要的電壓。整個(gè)系統(tǒng)采用了兩種芯片作為其電源管理芯片，分別為LINEAD的LT1117-3.3和TI的TL1963A。LT1117具有紋波小，低壓差，電流容量大的特性。TL1963為低壓差穩(wěn)壓芯片，一般穩(wěn)壓壓差僅需340mV，輸出電流能達(dá)到1.5A。主要電壓參數(shù)及使用的穩(wěn)壓芯片如下：7.2V電壓。直接取自蓄電池兩端電壓，給主電機(jī)提供電源。3.3電壓。為單片機(jī)系統(tǒng)電壓要求穩(wěn)定、噪聲小，電流容量小于1000mA。使用的是LINEAD的LT1117-3.3.5V電壓。為傳感器及相關(guān)外設(shè)提供電源。采用的SOT223封裝的TL1963A。6V電壓。為搖頭舵機(jī)3010提供電源，要求電壓穩(wěn)定，電流容量大于1000mA。采用的TO263封裝的TL1963A。5.5V電源。為轉(zhuǎn)向舵機(jī)SD-5提供電源，要求電壓穩(wěn)定，電流容量大于1000mA。采用的TO263封裝的TL1963A。電源模塊結(jié)構(gòu)圖如圖1-2。電池電池7.2V-8.5V電機(jī)驅(qū)動(dòng)6V穩(wěn)壓5V穩(wěn)壓5．5V穩(wěn)壓電機(jī)搖頭舵機(jī)激光傳感器轉(zhuǎn)向舵機(jī)3．3V穩(wěn)壓MCULT1117穩(wěn)壓電路TL1963A穩(wěn)壓電路3.3主控制器電路 考慮到官方提供的最小系統(tǒng)板體積太大，不夠簡潔，我們自制了印刷電路板，集成度更高，針對(duì)性更強(qiáng)，體積更小。我們使用的事MK60DN512，使用JTAG下載程序。其管腳分配如下表所示：普通I/O口PA14—PA16，PB0—PB7，PB16—PB23，PC14—PC19，PD0—PD5，傳感器接口人機(jī)交互模塊電機(jī)控制PWM模塊PA5，PA6舵機(jī)控制PA8電機(jī)控制PA10激光調(diào)制LPTMR模塊PA7測速模塊UART模塊PA7無線模塊SD卡模塊PA7數(shù)據(jù)存儲(chǔ)3.4激光傳感器電路激光傳感器屬光電傳感器，分兩部分，一部分為發(fā)射部分，一部分為接收。我們使用大功率半導(dǎo)體激光二極管，功率為100MW。驅(qū)動(dòng)電路使用74HC138配合PNP三極管9015，分時(shí)點(diǎn)亮。接收為一對(duì)多，這樣減少了接收單元，還可以避免發(fā)射管之間的干擾。具體電路如下圖。圖3.3激光發(fā)射接收電路3.5電機(jī)驅(qū)動(dòng)電路電機(jī)是整個(gè)智能車中能耗最大的地方，其最大電流能達(dá)到10A以上，為了提高智能車的加速和制動(dòng)性能，一個(gè)性能優(yōu)秀的驅(qū)動(dòng)電路是必不可少。常用的電機(jī)驅(qū)動(dòng)芯片MC33886使用簡單方便、功能強(qiáng)大。但其不足在于最大電流只能達(dá)到5A，即使采用多片并聯(lián)，其壓降還是比較大，不利于智能車加速和制動(dòng)性能的提升。我們采用由功率MOSFET管搭建H橋電路作為電機(jī)驅(qū)動(dòng)，橋電路采用門電路控制，使得電機(jī)驅(qū)動(dòng)使用更加方便、安全。由于門電路的控制，不會(huì)由于程序控制錯(cuò)誤的原因?qū)е聵螂娐范搪?，而且由于門電路的沿時(shí)，使得驅(qū)動(dòng)不會(huì)因?yàn)镸OSFET管關(guān)斷延時(shí)導(dǎo)致短路，使MOSFET管發(fā)熱嚴(yán)重。不同MOSFET管功率和內(nèi)阻不一樣，我們采用N溝道MOSFET管SI4405和P溝道IRF7832構(gòu)成橋電路。圖3.5H橋電路電機(jī)驅(qū)動(dòng)電路的電源可以直接使用蓄電池兩端電壓。模型車在啟動(dòng)過程中往往會(huì)產(chǎn)生很大的沖擊電流，會(huì)對(duì)其他電路造成電磁干擾，為了減小干擾，一方面我們在單片機(jī)和電機(jī)驅(qū)動(dòng)中間采用光耦隔離，另一方面，我們在進(jìn)行PCB設(shè)計(jì)時(shí)，將電機(jī)驅(qū)動(dòng)單獨(dú)分開設(shè)計(jì)，和主控板分開安裝,并在電源兩端加大電容。3.6人機(jī)交互電路一個(gè)好的系統(tǒng)，人機(jī)交互是必不可小，智能車在調(diào)試過程中要不斷的修改參數(shù)，并且要隨時(shí)觀測智能車的運(yùn)行狀況。由于智能車本身系統(tǒng)比較復(fù)雜。對(duì)單片機(jī)速度要求比較高，所以人機(jī)交互所占的資源越少越好，在方便實(shí)用的前提下，應(yīng)該盡量簡單。圖3-2人機(jī)交互模塊我們采用最為簡單的辦法來實(shí)現(xiàn)人機(jī)交互，采用拔碼開關(guān)進(jìn)行參數(shù)設(shè)置，指示燈用來指示系統(tǒng)工作狀態(tài)。第四章軟件設(shè)計(jì)4.1總體設(shè)計(jì)4.1.1層次設(shè)計(jì)智能車的軟件設(shè)計(jì)，由于和硬件結(jié)合非常緊密，當(dāng)硬件方案有較大改動(dòng)時(shí)，程序修改量會(huì)非常大，所以我們從一開始就注意程序分層和結(jié)構(gòu)化。從圖4-1中可以看出我們的層次設(shè)計(jì)思想。其中HardWare是硬件層；Sensor是硬件驅(qū)動(dòng)層；Bases是算法基礎(chǔ)層；Special和Control是算法層；Data用于存放控制策略數(shù)據(jù)，SCI用于無線串口調(diào)試，單獨(dú)給出。這樣，當(dāng)硬件方案發(fā)生較大修改時(shí)，只需要修改程序相關(guān)部分就可以短時(shí)間內(nèi)進(jìn)行新方案的算法調(diào)試了。4.1.2總體流程智能車要能自動(dòng)的循跡行駛，并且對(duì)不同情況的賽道都能完成功能，這要求程序有較強(qiáng)的適應(yīng)能力。采用激光發(fā)射接收的方式所得到的信息會(huì)不僅會(huì)隨著賽道情況不同有較大的改變，傳感器本身的特性也不穩(wěn)定，而且不同傳感器總存在著差別。為了能適應(yīng)不同賽道，我們采取了對(duì)賽道信息進(jìn)行提前采樣，數(shù)據(jù)歸一化處理的方式。程序設(shè)計(jì)的總體流程如圖4-2所示。圖4-2總體流程圖4.2位置確定由于今年的規(guī)則有所變化，賽道變成雙線，因此我們將激光分射在賽道兩邊，同時(shí)獲取賽道兩邊的信息。后排傳感器總共有18個(gè)發(fā)射單元，6個(gè)接收單元，1對(duì)3。激光點(diǎn)間距2CM，賽道內(nèi)白板上5個(gè)點(diǎn)，賽道外及黑線上4個(gè)點(diǎn)。分布示意圖見圖4—3。賽道位置獲取方案：首先需要得到前排激光模塊接收到的道路一個(gè)18位的數(shù)據(jù)。0代表白色，1代表黑色。通過分階段窮舉算法得到道路偏轉(zhuǎn)方向及偏轉(zhuǎn)角度。具體算法是：分割18位數(shù)據(jù)為2個(gè)9位數(shù)據(jù)，分別代表左側(cè)信息和右側(cè)信息；分割左側(cè)9位數(shù)據(jù)3個(gè)3位數(shù)據(jù)，分別窮舉3位數(shù)據(jù)可能的狀態(tài)。注意3個(gè)數(shù)據(jù)并不是有8種，而是考慮到在道路上的情況，即每個(gè)點(diǎn)依次有1、2、4種窮舉狀態(tài)。因此9位數(shù)據(jù)一共需要7*3=21次窮舉即可列舉盡所有道路信息，列舉過程中賦予相應(yīng)左側(cè)偏差值；左側(cè)9位數(shù)據(jù)列舉過程類似步驟②，并賦予相應(yīng)右側(cè)偏差值；4點(diǎn)5點(diǎn)5點(diǎn)4點(diǎn)。。。。。。圖4-3激光點(diǎn)分布示意圖4.3舵機(jī)控制4.3.1搖頭舵機(jī)變結(jié)構(gòu)控制作為一種特殊的非線性控制，變結(jié)構(gòu)很好的符合搖頭舵機(jī)轉(zhuǎn)動(dòng)所需方案的要求。它迫使系統(tǒng)的狀態(tài)被限制在某一子流形上運(yùn)動(dòng),即“滑動(dòng)模態(tài)”運(yùn)動(dòng)，具有使控制系統(tǒng)結(jié)構(gòu)隨時(shí)間變化的開關(guān)特性。特點(diǎn)在于一，系統(tǒng)的控制目標(biāo)由跟蹤誤差改變?yōu)榛：瘮?shù)，當(dāng)控制使滑模函數(shù)s為0時(shí)，跟蹤誤差也將漸變至0；二，對(duì)于大于二階的高階系統(tǒng)，常規(guī)模糊控制中的輸入是[e,e,e1）滑模面的選擇選定4個(gè)模糊面?；Ｃ嬉唬脕硖幚淼缆忿D(zhuǎn)折角小于10°的道路?；Ｃ娑?，處理向右轉(zhuǎn)折角大于10度小于60°的道路?；Ｃ嫒?，處理向左轉(zhuǎn)折角大于10度小于60°的道路?；Ｃ嫠?，處理十字交叉的空白部分。2）確定模糊滑?？刂埔?guī)律根據(jù)獲取的賽道信息數(shù)據(jù)分析可以得到滑模面一存在的條件是左右信息均非白；滑模面二存在的條件是右側(cè)信息全白，左側(cè)不為全白；滑模面三存在的條件是左側(cè)信息全白，右側(cè)不為全白；滑模面四存在的條件是左右信息全白。在每個(gè)滑模面采用模糊控制規(guī)律，在切換面附近采用模糊化算法減弱抖振現(xiàn)象。模糊變量選擇如下:1.滑模面一：偏差E為QUOTE偏差||Br-Bl|,偏差變化率ΔE為|Br-Bl|-|PreBr-PreBl|QUOTE|Br-Bl|；2.滑模面二：偏差E為Bl，偏差變化率ΔE為Bl-PreBlQUOTEBl；3.滑模面三：偏差E為Br，偏差變化率ΔE為Br-PreBrQUOTEBr；4.滑模面四：偏差E為為前10次E的平均值，偏差變化率ΔE為前10次ΔE的平均值。模糊面一算法如下：1.定義模糊集PB（正大）PM（正中）PS（正?。㈱O（零）NS（負(fù)小） NM（負(fù)中） NB（負(fù)大）2.選定論域E={NB，NM，NS，ZO，PS，PM，PB}論域?yàn)椋篍={-3，-2，-1，0，1，2，3}3.確定模糊滑模控制器的模糊控制規(guī)則基本的模糊控制規(guī)則是：If|Br-Bl|isAand|Br-Bl|isB,then△表4-1模糊滑?？刂埔?guī)則表NBNMNSZOPSPMPBPBZOPSPMPBPBPBPBPMNSZOPSPMPBPBPBPSNMNSZOPSPMPBPBZONBNMNSZOPSPMPBNSNBNBNMNSZOPSPMNMNBNBNBNMNSZOPSNBNBNBNBNBNMNSZO4.解模糊化 模糊表中的值直接作為舵機(jī)控制值，這樣可以可以省去解模糊化這一步，計(jì)算量小，可控化效果好。4.3.2轉(zhuǎn)向改進(jìn)在搖頭舵機(jī)在轉(zhuǎn)動(dòng)過程中當(dāng)系統(tǒng)處于與二三滑模面交接處時(shí)會(huì)出現(xiàn)一定的快速動(dòng)作，如果不加處理，轉(zhuǎn)換到轉(zhuǎn)向舵機(jī)控制值上，也會(huì)有一定抖動(dòng)，使車身滑動(dòng)，影響穩(wěn)定性和視覺效果。具體改進(jìn)步驟有三：1.選擇以車身速度及道路偏角作為變量，模糊化轉(zhuǎn)向過程，模糊表設(shè)置疏密取決于消除車輪抖動(dòng)及車體側(cè)滑現(xiàn)象的效果；2．給模糊面二三模糊表中各個(gè)給定值以適當(dāng)?shù)脑隽?，減小步驟1模糊控制量幅度，加快車輪調(diào)整，減小突然轉(zhuǎn)動(dòng)的角度；3．減小前輪外擴(kuò)角度，使轉(zhuǎn)向突變現(xiàn)象不明顯。4.4速度控制4.4.1電機(jī)控制舵機(jī)控制采用PID算法。再提一下PID。P來控制當(dāng)前，誤差值和一個(gè)比例常數(shù)相乘，然后和預(yù)定的值相加。P只是在控制器的輸出和系統(tǒng)的誤差成比例的時(shí)候成立。I來控制過去，誤差值是過去一段時(shí)間的誤差和，然后乘以一個(gè)積分常數(shù)，然后和預(yù)定值相加。I從過去的平均誤差值來找到系統(tǒng)的輸出結(jié)果和預(yù)定值的平均誤差。D來控制將來，計(jì)算誤差的一階導(dǎo)，并和一個(gè)微分常數(shù)相乘，最后和預(yù)定值相加。這個(gè)導(dǎo)數(shù)的控制會(huì)對(duì)系統(tǒng)的改變作出反應(yīng)。導(dǎo)數(shù)的結(jié)果越大，那么控制系統(tǒng)就對(duì)輸出結(jié)果作出更快速的反應(yīng)。這個(gè)參數(shù)也是PID被稱為可預(yù)測的控制器的原因。該參數(shù)對(duì)減少控制器短期的改變很有幫助。一些實(shí)際中的速度緩慢的系統(tǒng)可以不需要該參數(shù)。在此，I部分可取為2次以往誤差的和。根據(jù)需要的速度用公式算出來PID控制參數(shù)，用這個(gè)參數(shù)的正負(fù)判斷速度是快了還是慢了。當(dāng)速度快了，而且是快了很多時(shí)，需要控制電機(jī)反轉(zhuǎn)制動(dòng)。為了使電機(jī)反應(yīng)更靈敏一些，可以分快了多少的程度，分別控制電機(jī)PWM的值。例如：當(dāng)快的部分超過了電機(jī)最大PWM值時(shí)，可令當(dāng)前的電機(jī)PWM值為電機(jī)PWM最大值；當(dāng)快的部分超過一定值，當(dāng)前電機(jī)PWM為一個(gè)值；當(dāng)快的部分超過另外一個(gè)值，當(dāng)前電機(jī)PWM為另外一個(gè)值。而當(dāng)速度雖然快了，但是快的不是很多時(shí)，不做調(diào)節(jié)。這樣過度有點(diǎn)大，能再細(xì)一點(diǎn)也許會(huì)更好。除此之外，電機(jī)是正轉(zhuǎn)的。當(dāng)需要的電機(jī)轉(zhuǎn)速超過電機(jī)最大轉(zhuǎn)速時(shí)，要限制。如果能再細(xì)分，效果會(huì)更好吧。注意最后把電機(jī)需要的PWM值賦值給PWM通道23的占空比寄存器。這里需要的速度需要分情況設(shè)定，因?yàn)橹钡缽澋浪俣炔灰粯印Ｒ罁?jù)是以往激光PWM值與中值差的平均值的大小，當(dāng)很大時(shí)一定需要急轉(zhuǎn)彎，速度要??；當(dāng)小的時(shí)候，速度可以適當(dāng)提高。也就是以往激光PWM值與中值差的平均值越小，速度設(shè)定可以越快。所以有必要對(duì)速度在程序中進(jìn)行預(yù)先設(shè)定，調(diào)試時(shí)通過電路板上的按鍵或者撥碼開關(guān)選定。在此，有的程序?qū)⒏鞣N情況下的速度也分檔次。有高速檔，中高速檔，中速擋，中低速檔，低速檔和勻速檔等。4.4.2速度給定策略一般來說速度控制并不是最困難的，智能車要在位置賽道最短時(shí)間內(nèi)完成全程，車處在不同賽道段的速度給定才是最有難度的。我們首先考慮如下：直道應(yīng)當(dāng)盡量快彎道也要盡量快直道進(jìn)彎要減速彎道出直道可以適當(dāng)加速為此我們編寫了較為復(fù)雜的賽道情況確定函數(shù)，最終發(fā)現(xiàn)效果不是很好，分析原因如下：前瞻性不夠，滯后性嚴(yán)重臨界情況導(dǎo)致速度給定反復(fù)變化，造成速度控制不穩(wěn)定最終我們采取了一種只根據(jù)當(dāng)前舵機(jī)偏轉(zhuǎn)角度輸出值確定速度的方法，這種方法的直道進(jìn)彎減速效果不那么明顯，但穩(wěn)定性好，實(shí)際能達(dá)到的平均速度優(yōu)于之前的速度給定方法。我們確定速度給定公式如下：速度設(shè)定值=直線速度-舵機(jī)轉(zhuǎn)角的平方*(直線速度-彎道速度)/(舵機(jī)最大轉(zhuǎn)角的平方)作出圖形如圖4-8所示：圖4-8速度給定曲線4.5特殊情況處理4.5.1十字彎由于今年的規(guī)則變化，十字彎成為光電的一大重難點(diǎn)。十字彎的判斷：由于我們激光分布得寬，其他情況不會(huì)出現(xiàn)激光點(diǎn)全灑在白板上，所以當(dāng)接收管全能接收到信號(hào)時(shí)，十字彎標(biāo)志位置位。十字彎策略：根據(jù)進(jìn)彎前的速度和搖頭偏差各一個(gè)合適的回轉(zhuǎn)角度。4.5.2坡道由于坡道處于直道，所以坡道的速度給定不需特殊處理，轉(zhuǎn)向方面當(dāng)坡道比較陡的時(shí)候，由于前排架設(shè)角度比較大，可能會(huì)出現(xiàn)前排無法獲取有效數(shù)據(jù)的現(xiàn)象，這時(shí)候可以采用只用后排轉(zhuǎn)向。上坡判斷：小車上坡時(shí)，數(shù)據(jù)會(huì)全部丟失，以此我們設(shè)置坡道標(biāo)志位。當(dāng)出現(xiàn)一定次數(shù)的錯(cuò)誤數(shù)據(jù)時(shí)就默認(rèn)為坡道，這時(shí)將切換到后排激光。坡道策略：根據(jù)后排的信息，用窮舉法得到偏差，給轉(zhuǎn)向舵機(jī)相應(yīng)的轉(zhuǎn)角。下坡判斷：小車下坡，前排傳感器會(huì)接收到正確信號(hào)，當(dāng)正確信號(hào)連續(xù)出現(xiàn)一定次數(shù)時(shí)，我們判定小車下坡4.5.3起跑線由于前排激光的抖動(dòng)很大，在判斷起跑線時(shí)很不穩(wěn)定，所以我們利用下排激光進(jìn)行判斷。主要采取方案是跳變法，當(dāng)下排左右兩側(cè)同時(shí)出現(xiàn)1次以上的跳變（由白到黑或由黑到白）時(shí)，將起跑線標(biāo)志位置位。 左1次跳變右1次跳變5.2調(diào)試5.2.1系統(tǒng)硬件調(diào)試對(duì)智能小車系統(tǒng)硬件平臺(tái)的調(diào)試工作在設(shè)計(jì)硬件控制電路板和組裝智能小車模型的時(shí)候就應(yīng)該大體上完成，在日后的調(diào)試工作中再根據(jù)需要和新的思路和方案進(jìn)行修改。值得注意的是，在設(shè)計(jì)控制電路PCB板并交付工廠加工制作之前一定要進(jìn)行縝密的規(guī)劃論證和仔細(xì)的檢查，確保當(dāng)前方案的電路沒有任何錯(cuò)誤，否則如果等PCB制作好了之后發(fā)現(xiàn)元器件封裝或者導(dǎo)線連接錯(cuò)誤再進(jìn)行修改將會(huì)非常麻煩，甚至還要浪費(fèi)時(shí)間和金錢重新制作PCB。此外，在設(shè)計(jì)電路板時(shí)，最好能夠?yàn)閷淼墓δ軘U(kuò)展和方案更新留下一定的修改余地，以便在調(diào)試過程中可以在當(dāng)前的PCB上暫時(shí)焊接飛線，進(jìn)行修改，等方案成熟之后再制作下一塊電路板，這樣做，既節(jié)省了時(shí)間和資金，又使設(shè)計(jì)方案更加可靠。控制電路PCB布線圖如圖5-2所示。圖5-2控制電路PCB圖5.2.2目前光電組普遍使用的傳感器有普通激光管和激光管二種，普通激光管價(jià)格低廉，硬件電路設(shè)計(jì)簡單，穩(wěn)定性好，但其檢測精度與前贍性稍有點(diǎn)遜色。智能小車在控制轉(zhuǎn)向方面對(duì)道路信息要求不是那么苛刻，所以只要保證傳感器的個(gè)數(shù)就沒有問題。激光管的穩(wěn)定性和精確度都遠(yuǎn)高于激光管且能做到很大的前贍性，但其對(duì)硬件電路的依賴性很高，對(duì)溫度也敏感且價(jià)格昂貴。綜合考慮，我們選擇了普通模擬激光管做路面檢測傳感器。激光管調(diào)試起來相對(duì)簡單，調(diào)試首先要做的是選型，發(fā)射管選擇主要從以下二個(gè)參數(shù)考慮：一個(gè)是發(fā)射管的發(fā)射功率（功率大可提升檢測的前贍性），二個(gè)是它的發(fā)射角度（角度大可使單個(gè)發(fā)射管照射面積增大）；接收管選擇主要考慮的就是其線性度和接收范圍。線性度好，可以很精確的識(shí)別黑線離接收管距離，接收范圍大可有使無信號(hào)盲區(qū)減小但也易受干擾。因此在選型的時(shí)候要針對(duì)這幾個(gè)參數(shù)做分析。選好型后就做具體的架設(shè)調(diào)試，傳感器主要是用來確定路面情況的，不論是從速度方面還是從轉(zhuǎn)向方面都是要求傳感是照得越遠(yuǎn)且得到的信息越多是越好的。但看得遠(yuǎn)了在彎道行駛的時(shí)候道路丟失信息會(huì)很大，這樣就不太有利對(duì)路面做具體判斷，可能會(huì)導(dǎo)致很多時(shí)候是誤判，嚴(yán)重時(shí)會(huì)導(dǎo)致小車沖出跑道，結(jié)合考慮傳感的架設(shè)的前贍性應(yīng)有一個(gè)較優(yōu)的位置。這樣既能保證道路判定的準(zhǔn)確性，又能有較好的前贍性。5.2.3軟件系統(tǒng)調(diào)試本文之前已經(jīng)詳細(xì)的討論了智能小車的硬件結(jié)構(gòu)和位置處理算法、控制算法等，但是這些算法都需要用軟件即使用程序語言來實(shí)現(xiàn)。我們使用IARSystem出品的編譯器IAR以及硬件程序下載器——JTAG來開發(fā)智能小車的軟件系統(tǒng)。圖5-3IAR集成開發(fā)環(huán)境第六章賽車具體參數(shù)賽車具體參數(shù)如下表6-1所示。表6-1賽車具體參數(shù)表項(xiàng)目參數(shù)路徑檢測方法（賽題組）光電車模幾何尺寸（長、寬、高）（毫米）320/250/120車模平均電流（勻速行駛）(毫安)1950電路電容總量（微法）1880傳感器種類及個(gè)數(shù)激光接收管10個(gè)激光發(fā)射管34個(gè)編碼器1個(gè)新增加伺服電機(jī)個(gè)數(shù)1賽道信息檢測空間精度（毫米）2CM賽道信息檢測周期（毫秒/次）3主要集成電路種類/數(shù)量LT1963A1個(gè)TL1963A2個(gè)TL11171個(gè)74HC04、00、08各1片，IRF7832、SI4405各8片TLP251-41片車模重量（帶有電池）（千克）1.20第七章總結(jié)與展望7.1總結(jié)要實(shí)現(xiàn)對(duì)高速行駛汽車的自主智能控制并不是一個(gè)簡單的自動(dòng)控制問題，它涵蓋了控制、模式識(shí)別、力學(xué)、光學(xué)、電磁學(xué)、傳感技術(shù)、電子、電氣、計(jì)算機(jī)、機(jī)械及車輛工程等多個(gè)學(xué)科。本文采用的控制核心是一款飛思卡爾半導(dǎo)體公司生產(chǎn)的32位微控制器——MK60DN512，利用了微控制器的強(qiáng)大功能實(shí)現(xiàn)了智能車對(duì)路徑的自主尋跡，以及在未知環(huán)境下，結(jié)合一定的算法，實(shí)現(xiàn)了對(duì)智能車的高速運(yùn)動(dòng)控制，從最終測試結(jié)果來看，本系統(tǒng)具有較好的控制性能與對(duì)未知環(huán)境的適應(yīng)能力。對(duì)智能車系統(tǒng)的自主控制，可以分為三大部分：首先是對(duì)路徑信息的提取與識(shí)別，即智能小車的尋跡；然后是對(duì)前輪舵機(jī)方向的控制，即智能車的轉(zhuǎn)向控制；最后是對(duì)后輪車速的控制，即智能小車的驅(qū)動(dòng)控制。智能小車的轉(zhuǎn)向控制，主要就是對(duì)智能車的舵機(jī)系統(tǒng)進(jìn)行控制，智能車的舵機(jī)轉(zhuǎn)向控制是一個(gè)復(fù)雜的隨動(dòng)控制問題，很難求得其準(zhǔn)確的模型，此時(shí)簡單PID的控制效果將難于達(dá)到預(yù)期的目標(biāo)。因此我們根據(jù)人駕駛車輛的經(jīng)驗(yàn)，再PD控制的基礎(chǔ)上增加了一些模糊規(guī)則。。在搭建好智能車的軟硬件平臺(tái)后，我們進(jìn)行了一系列的仿真和調(diào)試，實(shí)驗(yàn)證明，我們的尋跡方案與轉(zhuǎn)向控制策略是穩(wěn)定、有效的。完成了對(duì)智能車的自動(dòng)控制，并在制作的賽道上實(shí)現(xiàn)了智能車的高速、自主行駛。在調(diào)試過程中，我們也發(fā)現(xiàn)了一些問題，由于對(duì)汽車工程理論與機(jī)械結(jié)構(gòu)設(shè)計(jì)的不熟悉，使我們對(duì)智能車結(jié)構(gòu)的改造和對(duì)激光傳感器支架的設(shè)計(jì)存在著缺陷和不足，這就約束了智能車的提速，例如，支架過重使智能車重心偏高，導(dǎo)致在高速行駛轉(zhuǎn)彎時(shí)會(huì)發(fā)生翻車的情況。此外，我們傳感器信息的處理算法和轉(zhuǎn)向控制算法也有改進(jìn)和提升的空間，例如，可以嘗試在轉(zhuǎn)向控制中使用專家系統(tǒng)等先進(jìn)的算法。這些都是我們今后努力的目標(biāo)。7.2展望智能車系統(tǒng)的研究十分復(fù)雜，需要解決的問題很多，任務(wù)非常艱巨，不是一蹴而就的，必須經(jīng)過長期的理論研究和實(shí)踐探索才能夠取得突破和進(jìn)展。展望未來，對(duì)于我們所研究的這類智能小車，我們認(rèn)為今后還可在以下幾個(gè)方面做進(jìn)一步的研究和提升。1）、智能車車機(jī)械性能方面。需要學(xué)習(xí)一些汽車工程方面的知識(shí)，以便對(duì)車體改造有理論指導(dǎo)和依據(jù)。此外可以在考慮外增加制動(dòng)裝置在入彎時(shí)進(jìn)行減速。這樣直道速度可以提高，平均速度更快。但如何選擇合適的制動(dòng)裝置并對(duì)其進(jìn)行控制，還需要進(jìn)一步深入研究。2）、硬件傳感器方面?？梢試L試采用激光或者其它先進(jìn)光電傳感器。傳感器的數(shù)量和最有布局也有待進(jìn)一步研究。特別是驅(qū)動(dòng)電路，在時(shí)序和器件選擇上應(yīng)該下大工夫。4）、在舵機(jī)控制策略上，可以考慮使用更先進(jìn)的控制算法。參考文獻(xiàn)[1]教育部高等學(xué)校自動(dòng)化教學(xué)指導(dǎo)分委員會(huì),關(guān)于舉辦第五屆杯全國大學(xué)生“飛思卡爾”杯智能汽車競賽的通知,2008.11.1[2]卓晴，黃開勝，邵貝貝.學(xué)做智能車[M].北京：北京航空航天大學(xué)出版社，2007[3]謝良之何信華馮義.中南大學(xué)黑影狂飆3代技術(shù)報(bào)告.2008[4]王宜懷吳瑾蔣銀珍.嵌入式系統(tǒng)原理與實(shí)踐.電子工業(yè)出版社.2012[5]阮毅陳維鈞.運(yùn)動(dòng)控制系統(tǒng).北京.清華大學(xué)出版社，2006.09致謝參與智能車競賽是興趣驅(qū)動(dòng)，但若沒有學(xué)校、老師的大力支持，沒有同學(xué)們的相互交流，我們不可能完成全部智能車研究工作。感謝學(xué)院領(lǐng)導(dǎo)和老師的關(guān)心和支持，并提供優(yōu)越的調(diào)試環(huán)境。特別感謝王擊老師，在我們遇到困難的過程中幫助解決疑點(diǎn)，并始終鼓勵(lì)我們繼續(xù)前行。同時(shí)感謝一起參與研究的所有中南大學(xué)智能車競賽團(tuán)隊(duì)的同學(xué)，沒有大家的幫助，我們的進(jìn)展將十分緩慢。附錄：程序代碼/*******************搖頭舵機(jī)控制******************/voidLaser_Servor(void){floatp=0;floatd=0;floatpd=0;intPD=0;if(!Light){if(Br!=0&&Bl!=0){Wcount=0;zhuan1=0;zhuan2=0;if(Br>Bl){p=Laser_Kpn[Br-Bl]*(Br-Bl);d=Laser_Kd[abs(Br-preBr)]*(Br-preBr);pd=p+d;}else{p=Laser_Kpn[Bl-Br]*(Br-Bl);d=Laser_Kd[abs(preBl-Bl)]*(preBl-Bl);pd=p+d;}PD=(int)pd;CurrentLaserServorPWM=PreLaserServorPWM+PD;}elseif(Br==0&&Bl!=0){Wcount=0;zhuan1=0;zhuan2=0;p=Laser_Kpn[Bl+1]*(0-Bl);d=Laser_Kd[abs(preBl-Bl)]*(preBl-Bl);pd=p+d;PD=(int)pd;CurrentLaserServorPWM=PreLaserServorPWM+PD;}elseif(Bl==0&&Br!=0){Wcount=0;zhuan1=0;zhuan2=0;p=Laser_Kpn[Br+1]*Br;d=Laser_Kd[abs(Br-preBr)]*(Br-preBr);pd=p+d;PD=(int)pd;CurrentLaserServorPWM=PreLaserServorPWM+PD;}elseif(Br==0&&Bl==0){if(Speed<=205)zhuan1=1;if(Speed>205)zhuan2=1;Wcount++;if((Wcount>3&&zhuan1==1)||(Wcount>2&&zhuan2==1)){if(abs(Sum_Laserdegrees<=100))huizhuan=0.992;elseif(abs(Sum_Laserdegrees)<=150&&100<abs(Sum_Laserdegrees))huizhuan=0.987;elseif(abs(Sum_Laserdegrees)<=200&&150<abs(Sum_Laserdegrees))huizhuan=0.987;elseif(abs(Sum_Laserdegrees)<=250&&200<abs(Sum_Laserdegrees))huizhuan=0.987;elseif(abs(Sum_Laserdegrees)<=300&&250<abs(Sum_Laserdegrees))huizhuan=0.987;elsehuizhuan=0.987;CurrentLaserServorPWM=LASERSERVOR_MID+(int)((PreLaserServorPWM-LASERSERVOR_MID)*huizhuan);}elseCurrentLaserServorPWM=PreLaserServorPWM;}elseCurrentLaserServorPWM=PreLaserServorPWM;if(CurrentLaserServorPWM>LASERSERVOR_MOSTLEFT)CurrentLaserServorPWM=LASERSERVOR_MOSTLEFT;if(CurrentLaserServorPWM<LASERSERVOR_MOSTRIGHT)CurrentLaserServorPWM=LASERSERVOR_MOSTRIGHT;FTM0_C2V=CurrentLaserServorPWM;preBl=Bl;preBr=Br;PreLaserServorPWM=CurrentLaserServorPWM;LaserPWMData[0]=CurrentLaserServorPWM;}elseCurrentLaserServorPWM=PreLaserServorPWM;}/**************轉(zhuǎn)向舵機(jī)算法*********************/voidRevolve_Servor1(void){inti=0;Sum_Laserdegrees=0;if(!Light){for(i=0;i<30;i++){Sum_Laserdegrees+=((long)LaserPWMData[i]-LASERSERVOR_MID);}Sum_Laserdegrees=Sum_Laserdegrees/30;//uart_send1((int)(Sum_Laserdegrees/10));if(Br!=0&&Bl!=0){if(Br>Bl)LdisPWM=CurrentLaserServorPWM-LASERSERVOR_MID;elseLdisPWM=CurrentLaserServorPWM-LASERSERVOR_MID;}elseif(Br==0&&Bl!=0)LdisPWM=CurrentLaserServorPWM-LASERSERVOR_MID;elseif(Bl==0&&Br!=0)LdisPWM=CurrentLaserServorPWM-LASERSERVOR_MID;elseLdisPWM=CurrentLaserServorPWM-LASERSERVOR_MID;//uart_send1(LdisPWM/10);if(abs(Sum_Laserdegrees)<=60)FInServor=0.40;//直道elseif(abs(Sum_Laserdegrees)<=140&&abs(Sum_Laserdegrees)>60)FInServor=0.52;//小Selseif(abs(Sum_Laserdegrees)<=260&&abs(Sum_Laserdegrees)>140)FInServor=0.58;//小Selseif(abs(Sum_Laserdegrees)<=300&&abs(Sum_Laserdegrees)>260)FInServor=0.62;//小Selseif(abs(Sum_Laserdegrees)<=350&&abs(Sum_Laserdegrees)>300)FInServor=0.70;//大彎elseif(abs(Sum_Laserdegrees)<=520&&abs(Sum_Laserdegrees)>350)FInServor=0.78;//500大彎elseif(abs(Sum_Laserdegrees)<=700&&abs(Sum_Laserdegrees)>520)FInServor=0.85;//640大彎（十字）elseFInServor=1;ServorPWM=(int)(FInServor*LdisPWM);CurrentServorPWM=SERVOR_MID-ServorPWM;if(CurrentServorPWM<SERVOR_MOSTLEFT)CurrentServorPWM=SERVOR_MOSTLEFT;if(CurrentServorPWM>SERVOR_MOSTRIGHT)CurrentServorPWM=SERVOR_MOSTRIGHT;FTM0_C3V=CurrentServorPWM;Wl=0;Wr=0;}else{LdisPWM=100*(Fr-Fl);if(Speed<115)KSpeed=1;elseif(Speed>=115&&Speed<175)KSpeed=1;elseif(Speed>=175&&Speed<185)KSpeed=1;elseif(Speed>=185&&Speed<195)KSpeed=1;//7elseif(Speed>=195&&Speed<205)KSpeed=1;//8elseif(Speed>=205&&Speed<215)KSpeed=1;//9elseif(Speed>=215&&Speed<225)KSpeed=1;//10elseif(Speed>=225&&Speed<235)KSpeed=1;//11elseif(Speed>=235&&Speed<245)KSpeed=1;//12elseif(Speed>=245&&Speed<255)KSpeed=1;//13elseif(Speed>=255&&Speed<265)KSpeed=1;//14elseif(Speed>=265&&Speed<275)KSpeed=1;//15elseif(Speed>=275)KSpeed=1;ServorPWM=(int)(KSpeed*LdisPWM);CurrentServorPWM=SERVOR_MID-ServorPWM;if(CurrentServorPWM<PODAO_MOSTLEFT)CurrentServorPWM=SERVOR_MOSTLEFT;if(CurrentServorPWM>PODAO_MOSTRIGHT)CurrentServorPWM=SERVOR_MOSTRIGHT;FTM0_C3V=CurrentServorPWM;}}/*****************速度電機(jī)PID控制*******************/voidSpeed_Control(void){floatMP=0;floatMI=0;floatMD=0;GPIOC_PDOR&=0XFFFFFFEF;if(!ting){SpeedErr[2]=SpeedErr[1];SpeedErr[1]=SpeedErr[0];SpeedErr[0]=needspeed-Speed;MP=Mkp*(SpeedErr[0]-SpeedErr[1]);MI=Mki*SpeedErr[0];MD=Mkd*(SpeedErr[0]-2*SpeedErr[1]+SpeedErr[2]);if(MI>I_max)MI=I_max;if(MI<I_min)MI=I_min;M_PID=MP+MI+MD;if((Motor_PWM[1])+M_PID>8000)Motor_PWM[0]=8000;elseif((Motor_PWM[1]+M_PID)<0)Motor_PWM[0]=0;elseMotor_PWM[0]=Motor_PWM[1]+M_PID;Motor_PWM[1]=Motor_PWM[0];FTM2_C0V=(int)(10000-Motor_PWM[0]);}}/*****************************檔位調(diào)節(jié)******************************/voidMode_Set(void){if(((GPIOB_PDIR&0X00000001)!=0)&&((GPIOB_PDIR&0X00000002)!=0)&&((GPIOB_PDIR&0X00000004)!=0)&&((GPIOB_PDIR&0X00000008)!=0)){SpeedSet1=100;SpeedSet2=100;SpeedSet3=100;SpeedSet4=100;}if(((GPIOB_PDIR&0X00000001)!=0X00000001)&&((GPIOB_PDIR&0X00000002)!=0)&&((GPIOB_PDIR&0X00000004)!=0)&&((GPIOB_PDIR&0X00000008)!=0)){SpeedSet1=130;SpeedSet2=130;SpeedSet3=130;SpeedSet4=130;}if(((GPIOB_PDIR&0X00000001)!=0)&&((GPIOB_PDIR&0X00000002)!=0X00000002)&&((GPIOB_PDIR&0X00000004)!=0)&&((GPIOB_PDIR&0X00000008)!=0)){SpeedSet1=140;SpeedSet2=140;SpeedSet3=140;SpeedSet4=140;}if(((GPIOB_PDIR&0X00000001)!=0X00000001)&&((GPIOB_PDIR&0X00000002)!=0X00000002)&&((GPIOB_PDIR&0X00000004)!=0)&&((GPIOB_PDIR&0X00000008)!=0)){SpeedSet1=150;SpeedSet2=150;SpeedSet3=150;SpeedSet4=150;}if(((GPIOB_PDIR&0X00000001)!=0)&&((GPIOB_PDIR&0X00000002)!=0)&&((GPIOB_PDIR&0X00000004)!=0X00000004)&&((GPIOB_PDIR&0X00000008)!=0)){SpeedSet1=160;SpeedSet2=160;SpeedSet3=160;SpeedSet4=160;}if(((GPIOB_PDIR&0X00000001)!=0X00000001)&&((GPIOB_PDIR&0X00000002)!=0)&&((GPIOB_PDIR&0X00000004)!=0X00000004)&&((GPIOB_PDIR&0X00000008)!=0)){SpeedSet1=170;SpeedSet2=170;SpeedSet3=170;SpeedSet4=170;}