基于磁場導(dǎo)航智能車控制器的設(shè)計

1、基于磁場導(dǎo)航智能車控制器的設(shè)計學(xué) 校: 專 業(yè)：電氣工程及其自動化帶隊教師: 參賽隊員: 目錄 第一章 前言3第二章方案論證4第三章整體設(shè)計思路51）、磁場檢測原理2)、系統(tǒng)整體結(jié)構(gòu)3)、定磁場放大電路4）、交變磁場放大電路第四章單元電路101)、單片機(jī)最小統(tǒng)2)、速度傳感器模塊3)、磁場檢測模塊4)、電機(jī)驅(qū)動模塊5)、舵機(jī)驅(qū)動模塊6)、LCD顯示模塊第五章 軟件設(shè)計18第七章 結(jié)論21附頁前言在智能導(dǎo)航系統(tǒng)中，如無人駕駛飛機(jī)、無人駕駛汽車，目前較為常用的導(dǎo)航方式為GPS導(dǎo)航。而地球磁場的大小和方向是任何人改變不了的，因此如何利用磁場導(dǎo)航具有很高的研究價值。磁導(dǎo)航智能車根據(jù)多給交變的磁場信息或

2、是根據(jù)無法人為改變的地球磁場來判斷方向及大小，自行達(dá)到預(yù)期的目標(biāo)并完成導(dǎo)航任務(wù)。當(dāng)今機(jī)器人技術(shù)發(fā)展如火如荼，其應(yīng)用已涉及包括國防等眾多領(lǐng)域，工業(yè)自動化，神五、神六升天，無人探月飛船無不得益于機(jī)器人技術(shù)的飛速發(fā)展。智能小車應(yīng)該說是最基本的機(jī)器人雛形，智能小車控制系統(tǒng)的研制將有助于推動智能機(jī)器人等智能控制系統(tǒng)的發(fā)展。實時采集傳感器信號，智能分析外部環(huán)境、路徑信息，自動實現(xiàn)方向控制及速度調(diào)節(jié)，是智能小車控制的主要特點(diǎn)，其設(shè)計內(nèi)容涵蓋機(jī)械、汽車、電子、自動控制、計算機(jī)、傳感器技術(shù)等多個學(xué)科的知識領(lǐng)域。作為一門新興的綜合技術(shù)，可廣泛應(yīng)用于工廠自動料車、固定場地搬運(yùn)車等技術(shù)領(lǐng)域，具有良好的應(yīng)用前景。本文設(shè)

3、計一智能車,能夠檢測有磁引導(dǎo)的軌跡識別,及自行檢測當(dāng)前地磁場.在有磁導(dǎo)航線路時,可跟蹤線路磁場,自動尋找軌跡,并迅速或按照規(guī)定速度前進(jìn).在水平面內(nèi)確定方位角,當(dāng)接受到位置坐標(biāo)和速度等命令后,智能車可再不受人干預(yù)的情況下,自行到達(dá)目標(biāo)位置.實現(xiàn)對特定磁導(dǎo)航的智能循跡和地磁場的方位角的測定,以實現(xiàn)水平面內(nèi)的定位,用以實現(xiàn)導(dǎo)航.第二章 方案論證該項目的研究內(nèi)容為首先對磁場導(dǎo)航相關(guān)的理論進(jìn)行分析，根據(jù)所分析的理論設(shè)計導(dǎo)航模型。利用單片機(jī)對磁場強(qiáng)度和方向進(jìn)行測量。在磁場測量方面主要分為恒定磁場和變化的磁場，對于恒定磁場的測量，可以采用霍爾元件進(jìn)行測量；對于變化的磁場可以利用電磁感應(yīng)原理進(jìn)行測量。不管是恒

4、定的還是變化的磁場，其產(chǎn)生的電信號都是很微弱的，因此要采取相應(yīng)的措施進(jìn)行放大，如利用放大電路進(jìn)行放大。放大之后的信號經(jīng)過單片機(jī)采集分析，最終確定磁場的大小，然后可以通過磁場的大小來判斷磁場的方向，進(jìn)而進(jìn)行導(dǎo)航。（1） 控制器選擇（A） At89s52是以款普及性很強(qiáng)的控制器，控制簡單，入手容易，易于操作，但此單片機(jī)結(jié)構(gòu)較為簡單，功能較少，速度相對也慢，很難完成本項任務(wù)（B） Stc單片機(jī)繼承了51的諸多優(yōu)點(diǎn)，同樣含有多種功能模塊運(yùn)算速度較快，能順利完成CPU的數(shù)據(jù)處理。（C） 飛思卡爾mc9s12xs128單片機(jī)是是基于速度更快的CPU12內(nèi)核的單片機(jī)系列，自身具有多個功能模塊，其運(yùn)行速度快，

5、功能模塊性能強(qiáng)，對完成本課題設(shè)計可以說可以完全勝任，只是價格較高，需要購買專門下載器和編程軟件，在此性價比不高。綜上所述，我們選擇方案C（2） 交變磁場信號放大電路（A） 集成電路組成的交流放大器（B） 分立式元器件組成放大電路綜上所述，我們選擇方案B（3） 定磁場檢測第一級放大電路（A） 采用差動放大電路，運(yùn)用三個分立運(yùn)放LM324組成的差動放大電路組成高輸入阻抗完成微弱電信號的（B） 單片集成芯片AD620，電路結(jié)構(gòu)簡單：一個AD620，一個增益設(shè)置電阻Rg，外加工作電源就可以使電路工作，設(shè)計周期短，電路可靠性強(qiáng)。綜上所述，我們選擇方案B第三章 整體設(shè)計思路1）、磁場檢測原理 3.1.1：

6、交變磁場檢測原理本設(shè)計我們設(shè)計電磁車要檢測的賽道環(huán)境是由通有 20kHz、100mA 左右交變電流的導(dǎo)線所產(chǎn)生的電磁場。電磁場檢測是課題要解決的第一個關(guān)鍵技術(shù)，需要首先對電磁場的特性進(jìn)行分析，然后根據(jù)分析的結(jié)果選擇合適的檢測原理，而后再選擇相應(yīng)的電磁傳感器。通有 20kHz、100mA 左右交變電流的導(dǎo)線，其周圍的電磁場如下圖：導(dǎo)線周圍磁場強(qiáng)度與距離的關(guān)系由上圖，很容易看到導(dǎo)線周圍磁場的強(qiáng)度分布圖。上圖磁場強(qiáng)度分布圖為垂直方向上距導(dǎo)線5cm高度的強(qiáng)度分布，圓上的磁場強(qiáng)度大小相同，并隨著距離導(dǎo)線的半徑r 增加成反比下降。此時也是兩者之間較線性的高度值。因此，我們的傳感器就安裝在距導(dǎo)線垂直高度5-

7、10cm處。導(dǎo)線周圍的磁場強(qiáng)度一般為10-11Gs左右，我們選用工字線圈來做磁場強(qiáng)度檢測元件，其檢測的范圍可以達(dá)到10-11Gs的數(shù)量級，同時它還具有原理簡單、價格便宜、體積相對較小、頻率響應(yīng)快等優(yōu)點(diǎn)。實際選用電感值為10mH、磁芯為鎳鋅材料的工字線圈作為電磁傳感器，其Q值較高，具有開放的磁芯，輸出信號幅值大。3.1.2：地磁場檢測原理地磁場的檢測，我們在這認(rèn)為其強(qiáng)度的恒定的，即方向和大小都不變。根據(jù)這樣的特性，我們采用磁阻傳感器，其原理如下：上圖左側(cè)長條就是一個磁阻傳感器的測量原理，右圖為整體的測量原理。2）、系統(tǒng)整體結(jié)構(gòu)定系統(tǒng)組成框圖中，我們能夠看到系統(tǒng)的硬件單元主要有CPU、磁檢測電路、

8、人機(jī)界面、舵機(jī)控制、電機(jī)驅(qū)動、測速單元等組成。本設(shè)計主要涉及磁傳感器模塊的設(shè)計及制作，其他模塊較常見，只做簡單理論描述，詳細(xì)模塊在下文都將涉及到。3）、磁場放大電路3.3.1一級放大電路定磁場一級放大本級放大電路將AD620微弱的差動信號進(jìn)行放大，使幅值在+2V-2V之間，在1腳和8腳之間的電阻決定了精密放大器的放大倍數(shù)，放大大小為49.4/R+1，當(dāng)前電路的放大倍數(shù)為100倍3.3.2二級放大電路二級電位拉升電路本電路的主要功能為上級電信號的的電位提升，使檢測信號又正負(fù)信號變成正電源信號，利于下一級模數(shù)轉(zhuǎn)換。4）、交變磁場放大電路此上電路為交變信號放大電路以及后面的整流濾波電路，它決定了所需

9、放大信號的大小，左側(cè)的電感電容并聯(lián)決定了放大信號的通過頻率，即帶通濾波電路根據(jù)電感值，電容值的大小即可決定當(dāng)前所要檢測磁場的頻率大小，對有用磁場進(jìn)行選擇。電感線圈外形第四章 單元電路1）、單片機(jī)最小系統(tǒng)2）、速度傳感器模塊 MC9S12XS128最小系統(tǒng)原理圖單片機(jī)最小系統(tǒng)板2）、速度傳感器我們用的是3000線編碼器，驅(qū)動電路電路由比較強(qiáng)完成放大、整形，然后將處理后的脈沖直接送人單片機(jī)模數(shù)轉(zhuǎn)換口。3）、磁場檢測模塊4.3.1首先是定磁場檢測：所選器件為Kmz52如下圖:圖是KMZ52的內(nèi)部結(jié)構(gòu)框圖和引腳排列。圖中,Z1和Z4為翻轉(zhuǎn)線圈,和為補(bǔ)償線圈。由于環(huán)境溫度可能會影響系統(tǒng)精度,因此,在高精

10、度系統(tǒng)中,可以通過補(bǔ)償線圈對其進(jìn)行補(bǔ)償。KMZ52內(nèi)部有兩個正交的磁場傳感器 分別對應(yīng)二維平面的軸和軸。磁場傳感器的原理是利用磁阻（）組成磁式結(jié)構(gòu),這樣可改變電磁物質(zhì)在外部磁場中的電阻系數(shù)。以便在磁場傳感器的翻轉(zhuǎn)線圈Z1和Z4上加載翻轉(zhuǎn)電信號后使之能夠產(chǎn)生變化的磁場。由于該變化磁場會造成磁阻變化（R）并將其轉(zhuǎn)化成變化的差動電壓輸出,這樣,就能根據(jù)磁場大小正比于輸出差動電壓的原理,分別讀取對應(yīng)的兩軸信號,然后再進(jìn)行處理計算即可得到偏轉(zhuǎn)角度。 復(fù)位/置位電路使用置位/復(fù)位電流帶需要施加置位/復(fù)位脈沖，簡稱S/R脈沖。需要注意的是，S脈沖與R脈沖對傳感器的影響相同，唯一不同的是傳感器輸出信號的改變。

11、這是因為磁阻傳感器有兩種工作方式。其中，工作方式1輸出電壓與磁場強(qiáng)度成正比，而工作方式2輸出電壓與磁場強(qiáng)度成反比。對于置位/復(fù)位電流帶輸入正向的脈沖電流磁阻傳感器為工作方式1；反之則是工作方式2。產(chǎn)生S/R脈沖電路稱為置位/復(fù)位脈沖電路，是采用磁阻傳感器作為磁場傳感器所特有的。A）第一級放大電路:差動放大電路ad620ad620的接線圖中表明了各個引腳的功能，及引腳排布B）第二級放大電路: 在第二章中已經(jīng)介紹了第二級放大電路的原理圖以及功能。 在這里說明一下程序?qū)Σ傻降腁D信號做的處理：由于轉(zhuǎn)換后的信號理論上是0到 5V之間，傳感器輸出的是正負(fù)電壓，所以還要先還原到正負(fù)信號，然后根據(jù)下圖中的方

12、法確定偏轉(zhuǎn)角度。 由于干擾磁場的影響，傳感器輸出具有固定偏差，設(shè)地磁場磁感應(yīng)強(qiáng)度為G，傳感器輸出為Vn，則傳感器工作在方式1時，V1=G+A；工作在方式2時，V2=-G+A。由此可得：G=(V1-V2)2；C）角度處理:在上圖中可分析出其判斷水平面內(nèi)360角的大小。值得一提的是，接收到的檢測信號是05V之間的電壓值，在程序中我們都要對每次接收到數(shù)據(jù)做減2.5V的預(yù)處理，完成于上圖的配合。4.3.2交變磁場檢測1）交變磁場的檢測電路較容易，在此主要注意幾個問題：A）本電路所選三極管為C1815，其放大倍數(shù)為80-700，帶寬為80MH，完全可以擔(dān)任放大任務(wù)。三極管的性能直接決定了放大后的效果，放

13、大倍數(shù)太小直接影響輸出電壓的復(fù)制；B）在設(shè)置基極偏置電壓時，對基極電阻的調(diào)節(jié)和選三極管有恨大關(guān)系，調(diào)節(jié)此電阻使集電極靜態(tài)工作點(diǎn)在電源電壓一半稍高即可，防止波形嚴(yán)重失真，導(dǎo)致無有效電壓值；C）在最后整形濾波出，采用二極管1819，其壓降僅為0.2V可盡量減小電壓的損失；D）電阻于電容的選擇關(guān)系到采集電路信息的時效性，準(zhǔn)確性。2）電感線圈的排布分析：通電導(dǎo)線周圍磁場的分布如圖3.1.1導(dǎo)線周圍磁場強(qiáng)度與距離的關(guān)系，根據(jù)通電導(dǎo)線周圍的磁場分布可知，當(dāng)線圈偏離導(dǎo)線較遠(yuǎn)時，感應(yīng)電壓值很小，傳感器感應(yīng)的電勢太小，不利于信號的處理。所以距車頭同一距離處設(shè)置兩對電感線圈。為加大前沿性，我們在最前設(shè)置一對傳感器

14、，這樣共兩排傳感器，能夠較好地完成尋跡。4）、電機(jī)驅(qū)動模塊 用場效應(yīng)管搭建H橋來驅(qū)動電機(jī)。場效應(yīng)管具有內(nèi)阻極小、開關(guān)速度快等諸多優(yōu)點(diǎn)。并且方便加散熱片。場效應(yīng)管是電 壓驅(qū)動器件，只要柵極電壓稍高一點(diǎn)就能使管子導(dǎo)通，單片機(jī)直接輸出的電壓不太夠，所以還要增加?xùn)艠O驅(qū)動電路，可以用cmos與非門CD4011，場效應(yīng)管P管用IRF4905，N管用IRF3205，受到P管電流限制，最大電流為74A。實際電路我們想用兩管子并聯(lián)，提高本驅(qū)動的驅(qū)動電流，最終為提高使用時間和壽命，我們四片并聯(lián)實現(xiàn)電機(jī)驅(qū)動。5）、舵機(jī)驅(qū)動模塊所選舵機(jī)外形：舵機(jī)工作原理：原理介紹：舵機(jī)的控制信號為周期是20ms的脈寬調(diào)制（PWM）信

15、號，其中脈沖寬度從0.5ms-2.5ms，相對應(yīng)舵盤的位置為0180度，呈線性變化。也就是說，給它提供一定的脈寬，它的輸出軸就會保持在一個相對應(yīng)的角度上，無論外界轉(zhuǎn)矩怎樣改變，直到給它提供一個另外寬度的脈沖信號，它才會改變輸出角度到新的對應(yīng)的位置上。舵機(jī)內(nèi)部有一個基準(zhǔn)電路，產(chǎn)生周期20ms，寬度1.5ms的基準(zhǔn)信號，有一個比較器，將外加信號與基準(zhǔn)信號相比較，判斷出方向和大小，從而產(chǎn)生電機(jī)的轉(zhuǎn)動信號。由此可見，舵機(jī)是一種位置伺服的驅(qū)動器，轉(zhuǎn)動范圍不能超過180度，適用于那些需要角度不斷變化并可以保持的驅(qū)動當(dāng)中。6）、LCD顯示模塊1602原理圖液晶的使用方便調(diào)試中需要知道的數(shù)據(jù)的顯示。在調(diào)試中，

16、可以顯示每個磁傳感器的當(dāng)前的AD值，速度反饋值，當(dāng)前驅(qū)動電機(jī)及舵機(jī)的PWM信號的頻率等信息，很直觀地顯示當(dāng)前狀態(tài)。這是比較簡單使用的人機(jī)界面。第五章 軟件設(shè)計1）概述賽車控制系統(tǒng)的設(shè)計主要由賽道信息的采集與數(shù)據(jù)分析、方向控制、速度控制著三個部分組成?？驁D如下：賽道信息的采集與數(shù)據(jù)分析HC9S12XS128轉(zhuǎn)向控制速度控制圖 5.1 系統(tǒng)框圖由上圖，我們就可以將系統(tǒng)細(xì)分為：AD采集賽道信息、數(shù)據(jù)分析、舵機(jī)PWM控制、定時測速、電機(jī)PWM控制、起跑線檢測這六個部分。將系統(tǒng)細(xì)分可以讓軟件與底層硬件模塊接口的配合更緊密，同時也是把軟件系統(tǒng)的實現(xiàn)從具體的硬件中盡量抽象出來，使控制更方便，程序更易讀，也是

17、把團(tuán)隊分工整合起來的有效途徑。2） 賽道信息的采集與數(shù)據(jù)分析賽道信息的采集分為路況信息的采集和起跑線的采集這兩個部分。路況信息的采集主要是由安裝在賽車前面的多排感應(yīng)線圈來完成。感應(yīng)線圈通過檢測賽道上方的磁場，采集到的信號經(jīng)過諧振、放大、檢波，然后直接送到單片機(jī)的AD口。單片機(jī)通過AD采樣模塊直接就可以得到感應(yīng)線圈感應(yīng)到磁場信號的大小。另外，賽車安裝完成后，感應(yīng)線圈之間的距離以及對地的高度都是恒定的，因此通過多次對比采集得到的感應(yīng)線圈信號的大小即可得到賽道中心相對于感應(yīng)線圈的位置，為賽車的轉(zhuǎn)向控制提供依據(jù)。起跑線的采集主要是由安裝在車頭上的一排干簧管來完成。由于干簧管是連接在單片機(jī)的T3口上的，

18、當(dāng)采集到起跑線時觸發(fā)單片機(jī)的中斷，然后使賽車停止。數(shù)據(jù)處理方面要分別對采集的兩部分信息進(jìn)行處理。賽道信息的處理，主要是對AD采樣得到的數(shù)據(jù)進(jìn)行歸一化，以消除不同的感應(yīng)線圈之間的差異。而起跑線信號則主要是去抖動，防止因抖動而誤動作。數(shù)據(jù)有錯？Y程序開始AD采樣N歸一化結(jié)束初始化賽道信息的采集與數(shù)據(jù)分析的程序流程如上圖所示：3） 轉(zhuǎn)向控制賽車的轉(zhuǎn)向主要是由舵機(jī)來完成的。賽車的轉(zhuǎn)向控制就是對舵機(jī)的控制，并考慮速度對于轉(zhuǎn)向的影響。舵機(jī)的角度分配一般有兩種方式：查表方式、PID方式。分析比賽的要求，不難看出精確的轉(zhuǎn)向控制是完成比賽的關(guān)鍵，而我們采樣得到的電壓值也是連續(xù)變化的，如果采用查表方式的話，還需要

19、對數(shù)據(jù)進(jìn)行模糊化的處理，很難完成精控制的要求。實踐也發(fā)現(xiàn)，采用查表方式時，舵機(jī)的轉(zhuǎn)角會出現(xiàn)不連續(xù)的狀況，影響賽車的穩(wěn)定性。而使用PID調(diào)節(jié)方式對舵機(jī)進(jìn)行控制，該方式在不論是反應(yīng)速度，還是舵機(jī)轉(zhuǎn)向連續(xù)以及轉(zhuǎn)角預(yù)測上都優(yōu)于查表方式，因此，在實際過程中，我們使用的是PID方式。 方向的控制主要是根據(jù)預(yù)瞄區(qū)賽道中心的位置，初步確定舵機(jī)轉(zhuǎn)角，并根據(jù)賽道變化的趨勢對舵機(jī)轉(zhuǎn)角進(jìn)行修正。簡單的說，轉(zhuǎn)向控制就是以偏離糾正的思想為原則。在實際的系統(tǒng)中，數(shù)據(jù)進(jìn)行歸一化的處理，一方面消除了傳感器差異的影響；另一方面也為獲得賽車與跑道的偏離度提供方便。下圖為賽車與賽道偏離的示意圖，ABCD表示賽車，Y軸是賽車的中軸線，

20、EF為賽道的趨勢線。在實際的轉(zhuǎn)向過程中，賽車的轉(zhuǎn)向是靠前輪來完成的。結(jié)合下圖，傳感器之間的距離以及安裝的高度都是確定的，我們就可以將賽車的偏離度簡單的轉(zhuǎn)化成車前方左右兩邊傳感器電壓的差值。并以電壓差為輸入量，用增量式的PID控制算法直接計算，獲得舵機(jī)的動作增量。計算公式如下： 賽車與賽道偏離示意圖4）速度控制在正確檢測跑道的前提下，速度是本設(shè)計的重要特性。由于賽道的情況是未知的，速度過高很可能會使賽車不穩(wěn)定或者來不及轉(zhuǎn)向而沖出賽道。在隨動控制系統(tǒng)中，如果速度控制采用開環(huán)控制其魯棒性很差，極易受到擾動量的干擾使使系統(tǒng)不穩(wěn)定。賽車對穩(wěn)定性和速度的要求都很高，因此要求系統(tǒng)的抗干擾性能很強(qiáng)。所以開環(huán)控

21、制不適合賽車的控制系統(tǒng)，必須使用閉環(huán)控制。一般而言在采集到實時的速度以后速度控制方式是對動力進(jìn)行控制，當(dāng)速度高于一定值的時候減小賽車的動力，而速度過小的時候需要增加賽車的動力。這種方法有很大的缺點(diǎn)，它不能快速降低速度，可能會使賽車在直道進(jìn)入彎道的時候因速度過高而轉(zhuǎn)向不及時。另外電磁前瞻距離很小，對賽道類型的判斷也不能保證很準(zhǔn)確，為了穩(wěn)中求勝，我們在速度控制時只對速度進(jìn)行了控制。此處的程序原理于方向的控制相似，利用PID控制，快速、準(zhǔn)確地控制速度值。具體程序見附頁。第六章 結(jié)論本設(shè)計自參賽以來經(jīng)過查找資料，設(shè)計機(jī)構(gòu)，組裝車模,分析問題，編寫程序階段工作，直到系統(tǒng)成型，一共經(jīng)過了五個月的時間，其間

22、嘗試了兩路電感控制，四路電感做差控制；六點(diǎn)感“二”字排法等算法,最后我們選擇了六路傳感器作為最終的方案，能車控制系統(tǒng)的設(shè)計中所完成的工作如下： (1)研究了傳感器的排布對控制策略的影響，提出了合適的傳感器排布方式，提高了尋線的準(zhǔn)確性。(2)并在實驗的基礎(chǔ)上證明了在本系統(tǒng)中電機(jī)可以近似為一階系統(tǒng)，并由此完成了電機(jī)PID算法。(3)在機(jī)構(gòu)結(jié)構(gòu)上進(jìn)行了大量的理論分析及改進(jìn)，并在此基礎(chǔ)上完成了剎車制動算法。由于水平有限，并且時間不足，有許多技術(shù)與算法都沒有深入的研究，需要在以后繼續(xù)研究中不斷的完善，總結(jié)如下：(1) 電機(jī)的PID并不能將電機(jī)的加速性能很好的表現(xiàn)出來。(2) 電路的可靠性，抗高頻電磁干擾

23、的性能有待提高。(3) 對路況信息的處理還有待提高，特別是有用信息的提取。附頁：#include /* common defines and macros */#include derivative.h /* derivative-specific definitions */#include #include #include #include #pragma LINK_INFO DERIVATIVE MC9S12XS128#define Led5 PORTE_PE5#define Led6 PORTE_PE6#define KEY PORTB_PB3#define Led_left POR

24、TB_PB5#define Led_right PORTB_PB7 uchar n,r=0,z,flag,Flag=0;uchar count=0;static unsigned int waittime = 0;uchar FB=90,SH=45,SH1=45,V1=90,V2=90, V3=85,V4=150,V5=90,V6=90; unsigned int AD_1; /AD 轉(zhuǎn)換結(jié)果unsigned int AD_2; /AD 轉(zhuǎn)換結(jié)果unsigned int AD_3; /AD 轉(zhuǎn)換結(jié)果unsigned int AD_4; /AD 轉(zhuǎn)換結(jié)果unsigned int AD_5; /A

25、D 轉(zhuǎn)換結(jié)果unsigned int AD_6; /AD 轉(zhuǎn)換結(jié)果signed int Speed,x1,x2,y,x11,x22; /速度轉(zhuǎn)換值unsigned int time,numm,m;unsigned int numm_last0,numm_last1,numm_last2, num_last0,num_last1,num_last2; /-延時子程序-/void delay(unsigned int xms) int a,b,c; for(c=0;cxms;c+) for(a=0;a=50;a+) for(b=0;b90) sp=90; if(spFB); PWME_PWME0=

26、0; PWME_PWME1=1; flag=0; Led_right=1; Led_left=1; / /按鍵檢測/ / void KEY_SCAN(void) if(KEY=0) delay(10); if(KEY=0) while(!KEY); z+; if(z=1) FB=85; SH=45; SH1=45; V1=80; V2=80; V3=80; V4=150; V5=90; V6=80; Led_left=0; Led_right=1; delay(500); Led_left=1; if(z=2) FB=90; SH=45; SH1=45; V1=85; V2=85; V3=80

27、; V4=150; V5=90; V6=80; Led_right=0; Led_left=1; delay(500); Led_right=1; if(z=3) FB=100; SH=45; SH1=45; V1=95; V2=95; V3=90; V4=160; V5=100; V6=100; Led_right=0; Led_left=0; delay(500); Led_right=1; Led_left=1; if(z=4) z=0; FB=100; SH=45; SH1=45; V1=95; V2=95; V3=90; V4=170; V5=100; V6=100; Led_rig

28、ht=1; Led_left=0; delay(500); Led_left=1; /速度控制/ void Speed_con(void) while(!ATD0STAT0_SCF); /等待當(dāng)前隊列轉(zhuǎn)換完成*/ AD_1=ATD0DR0L; AD_2=ATD0DR1L; AD_3=ATD0DR2L; AD_4=ATD0DR3L; AD_5=ATD0DR4L; AD_6=ATD0DR5L; / /斜坡檢測/ / /if(AD_190 & AD_490 | AD_150 & AD_43 | AD_43) x1=AD_2-82; /中間左 x2=AD_3-82; /中間右 if(AD_1AD_4)

29、 x1=-x1; if(AD_4650) y=650; if(y40 | AD_440) /彎道 if(flag=1 & FeedBackFB) if(AD_1AD_4) Led_left=0; PWMDTY23=numm; shacheV(SH); Sudu(V1); else Led_right=0; PWMDTY23=numm; shacheV(SH1); Sudu(V2); else Sudu(V3); / /保存numm值/ / if(5AD_215 | 5AD_350 & AD_350 & AD_540 & AD_640) /斜坡檢測/ if(AD_195 & AD_495 | A

30、D_130 & AD_430) PWMDTY23=1570; Sudu(num_last2); else PWMDTY23=1570; flag=1; Sudu(V4); num_last0=FeedBack; else Sudu(V5); numm_last1=numm_last0; numm_last2=numm_last1; else PWMDTY23=numm_last2; Sudu(V6); num_last1=num_last0; num_last2=num_last1; if(num_last2=0) num_last2=100; /- 時鐘初始化程序 -/void SetBus

31、CLK_64M(void) CLKSEL=0X00; /disengage PLL to system PLLCTL_PLLON=1; /turn on PLL SYNR =0xc0 | 0x07; REFDV=0x80 | 0x01; POSTDIV=0x00; /pllclock=2*osc*(1+SYNR)/(1+REFDV)=128MHz; _asm(nop); /BUS CLOCK=64M _asm(nop); while(!(CRGFLG_LOCK=1); /when pll is steady ,then use it; CLKSEL_PLLSEL =1; /engage PLL

32、 to system;/-PWM初始化程序-/void PWM_Init() PWME=0; PWMPRCLK=0X53; /時鐘預(yù)分頻設(shè)置 A=4MHz B=2MHz PWMCLK=0x03; /時鐘源選擇SA,SA,B PWMSCLA=0X02; /分頻寄存器，時鐘設(shè)置SA=A/2PWMSCLA=1MHz PWMPOL=0x0f; /極性設(shè)置，開始輸出高電平 PWMCAE=0x0f; /對齊方式選擇1為中心對齊（通道關(guān)閉后才能對其設(shè)置) PWMCTL_CON23=1; /通道 2,3 級聯(lián)成一個16位的PWM通道 PWMDTY0=0x32; /占空比設(shè)置 PWMDTY1=0x32; /占空

33、比設(shè)置 PWMDTY23=0; /占空比設(shè)置 PWMPER0=0x64; /周期寄存器設(shè)置Frequency=SA/10=10KHz PWMPER1=0x64; /周期寄存器設(shè)置Frequency=SA/10=10KHz PWMPER23=0x2710; /周期寄存器設(shè)置Frequency=B/10000=100Hz PWMCNT0=0X00; /通道0計數(shù)寄存器 PWMCNT1=0X00; /通道1計數(shù)寄存器 PWME=0x0f; /-電機(jī)驅(qū)動初始化-/void TheMotor_Init() PWME=0x00; PWMDTY0=0x00; /(5/50)*100 PWMPER0=0x64

34、; PWMDTY1=0x00; PWMPER1=0x64; PWMDTY23=0; /1.5ms PWMPER23=0x2710; PWME=0x0f; /-ATD模塊-/void ATD_Init() ATD0CTL1=0x00; ATD0CTL2=0X40; /使能AD，清除標(biāo)志 */ ATD0CTL3=0Xb0; /隊列長度為6，依次存入結(jié)果寄存器，繼續(xù)轉(zhuǎn)換 */ ATD0CTL4=0X05; /8位精度，總線頻率32分頻 */ ATD0CTL5=0X30; /多通道轉(zhuǎn)換 0開始轉(zhuǎn)換*/ ATD0DIEN=0x0f; /禁止數(shù)字輸入 /-定時器初始化程序-/ /* void ECT_In

35、it(void) /ECT初始化，使用輸入捕捉功能 TIOS =0x00; /設(shè)為輸入捕捉 TSCR1=0x80; /定時器使能 TSCR2=0x07; TIE=0b00000001; /開中斷 TCTL4=0b00000010; /0 ,1觸發(fā)電平: TC0=888; */*J口外部中斷檢測起跑線* void PJ7_Init(void) DDRJ = 0X00; / PJ0判斷行同步脈沖到達(dá) PPSJ = 0x80; / 上升沿 /PPSJ = 0x00; / 下降沿 PIEJ = 0X80; PERJ = 0xff; /-PIT初始化-/void Pit0_Init(void) PITC

36、FLMT_PITE=0; /disable PIT PITCE_PCE0=1; /enable timer channel 0 PITMUX_PMUX0=0; /ch0 connected to micro timer 0 PITMTLD0=0X0a; /micro time base 0 equals 255 clock cycles PITLD0=0Xff00; /time-out period = (PITMTLD + 1) * (PITLD + 1) / /時間計算 /For example, for a 16 MHz bus clock, the maximum time /1 * 6400 * /64000000 = 0.0001s. PITINTE_PINTE0=1; /enable interupt channel 0 PITCFLMT_PITE=1; /enable PIT /*void Pit1_Init(void) PITCFLMT_P