1、第三屆“飛思卡爾”杯全國(guó)大學(xué)生智能汽車邀請(qǐng)賽技 術(shù) 報(bào) 告學(xué) 校：中國(guó)地質(zhì)大學(xué)（武漢）隊(duì)伍名稱：地大一對(duì)參賽隊(duì)員：鄒國(guó)雄龐建東何東帶隊(duì)教師：王勇關(guān)于技術(shù)報(bào)告和研究論文使用授權(quán)的說(shuō)明 本人完全了解第三屆“飛思卡爾”杯全國(guó)大學(xué)生智能汽車邀請(qǐng)賽關(guān)保留、使用技術(shù)報(bào)告和研究論文的規(guī)定，即：參賽作品著作權(quán)歸參賽者本人，比賽組委會(huì)和飛思卡爾半導(dǎo)體公司可以在相關(guān)主頁(yè)上收錄并公開(kāi)參賽作品的設(shè)計(jì)方案、技術(shù)報(bào)告以及參賽模型車的視頻、圖像資料，并將相關(guān)內(nèi)容編纂收錄在組委會(huì)出版論文集中。參賽隊(duì)員簽名： 帶隊(duì)教師簽名： 日 期：目錄第一章 引言11.1智能汽車制作概述11.2控制算法概述11.3引用文獻(xiàn)概述11.4報(bào)告的

2、主要內(nèi)容2第二章 模型車機(jī)械部分安裝及改造32.1傳感器的安裝32.2 主控板的安裝32.3光電碼盤(pán)的安裝4第三章 電路設(shè)計(jì)說(shuō)明53.1電源電路53.2 電機(jī)驅(qū)動(dòng)電路53.3 舵機(jī)驅(qū)動(dòng)電路63.3.1 電源電路6信號(hào)連接電路63.4 主控辦接口73.5 傳感器電路7第四章 智能車的控制94.1 主要代碼分析9第五章 車體調(diào)速方法115.1 智能車的質(zhì)量對(duì)性能的影響115.2閉環(huán)調(diào)速125.3變速前進(jìn)12第六章 結(jié)論14參考文獻(xiàn)15附 錄IA：程序流程IB：程序源代碼II第一章 引言1.1智能汽車制作概述 為了不影響智能車的整體性能，在制作過(guò)程中并未對(duì)車體結(jié)構(gòu)作大的改動(dòng)，如：沒(méi)有為安裝電路板而特

3、殊鉆孔，沒(méi)有安裝特殊的支架等。但為了安裝光電傳感頭，我們?cè)黾觾善F片將前端光電傳感器固定。在車體的后部增加了碼盤(pán)以確定速度。車體前端安裝第二排紅外傳感器，用于尋找引導(dǎo)線，通過(guò)對(duì)安裝孔的調(diào)整，直接安裝在車體前端的安裝孔上。智能車的控制板安裝在車體的后部，通過(guò)調(diào)整安裝孔的位置和大小，直接安裝在車體上方的原有的兩個(gè)螺栓上，并通過(guò)車體自帶的支架固定。單片機(jī)最小系統(tǒng)板通過(guò)排針插裝在控制板上。紅外線傳感器的信號(hào)通過(guò)10pin的排線連接到控制板上，舵機(jī)的控制線和電源線通過(guò)3pin的排線連接到控制板的相應(yīng)的接口上。為了能提高智能車的行進(jìn)速度，在設(shè)計(jì)電路板時(shí)盡量減小了PCB板的使用面積，同時(shí)簡(jiǎn)化了電路的設(shè)計(jì)，在

4、不影響電路功能的前提下，減少了元器件的數(shù)量和體積。1.2控制算法概述智能車要在引導(dǎo)線的引導(dǎo)下快速行進(jìn)，傳感器的采樣周期的大小對(duì)車體隨動(dòng)性能將會(huì)有很大的影響。當(dāng)采用周期足夠大時(shí)，數(shù)字離散系統(tǒng)的設(shè)計(jì)可以按照連續(xù)系統(tǒng)的設(shè)計(jì)思想設(shè)計(jì)，而且系統(tǒng)的輸出性能也和連續(xù)系統(tǒng)近似。因此決定采用計(jì)算方便位置PID控制算法，在對(duì)實(shí)際的有智能車、電機(jī)、舵機(jī)、單片機(jī)等部分組成隨動(dòng)系統(tǒng)進(jìn)行實(shí)際調(diào)試后發(fā)現(xiàn)，PID算式的微分部分對(duì)提高的系統(tǒng)的各項(xiàng)性能指標(biāo)沒(méi)有明顯的作用，選取不當(dāng)，還會(huì)使系統(tǒng)不穩(wěn)定，因此只保留了比例部分和積分部分。為了提高快速性和穩(wěn)定性，通過(guò)對(duì)算法的調(diào)整，加入了具有飽和特性、死區(qū)特性等特性的環(huán)節(jié)。1.3引用文獻(xiàn)概

5、述為了能讓智能車有較高的平均行進(jìn)速度，控制方法主要參考了由胡壽松主編的自動(dòng)控制原理和陶永華主編的新型PID控制及其應(yīng)用兩本書(shū)。通過(guò)試驗(yàn)進(jìn)行相應(yīng)的校準(zhǔn)。1.4報(bào)告的主要內(nèi)容本文將對(duì)智能車的制作及其控制方法的C語(yǔ)言實(shí)現(xiàn)進(jìn)行論述。第一章是對(duì)全文的概述；第二章將簡(jiǎn)述模型車機(jī)械部分安裝及改造、傳感器的設(shè)計(jì)安裝、系統(tǒng)電路板的固定及連接等；第三章將簡(jiǎn)述電路設(shè)計(jì)說(shuō)明；第四章中主要介紹智能車的控制方法以及算法的C語(yǔ)言實(shí)現(xiàn)；第五章簡(jiǎn)要論述車體質(zhì)量、速度調(diào)整方法等因素對(duì)智能車性能的影響；第六章是結(jié)論。第二章 模型車機(jī)械部分安裝及改造智能車上的各部分出了光電碼盤(pán)需要在車體的后部鉆兩個(gè)孔作為安裝孔，其余所有電路板都安裝

6、在車體原有的安裝孔和支架上。2.1傳感器的安裝傳感器直接安裝在智能車前端的用于安裝支架的安裝孔上，如圖：圖2.1 傳感器的安裝2.2 主控板的安裝智能車采用單片機(jī)HCS12開(kāi)發(fā)板作為控制板，主控制板是用來(lái)連接單片機(jī)和外圍各個(gè)功能模塊的橋梁。它為各個(gè)模塊提供相應(yīng)的接口。根據(jù)2.1節(jié)中的敘述，將主控制板安裝在智能車的后部，并且利用模型原有的安裝支架，如圖：圖2.2 主板的安裝2.3光電對(duì)管的安裝用光電對(duì)管測(cè)量車速圖2.3 光碼盤(pán)的安裝第三章 電路設(shè)計(jì)說(shuō)明3.1電源電路智能車采用的鎳鉻充電電池供電，電池電壓為7.2V，為了能為各個(gè)用電設(shè)備提供穩(wěn)定的供電電壓，選用低壓差穩(wěn)壓集成芯片mc33886為系統(tǒng)

7、供電。由于傳感器的耗電量較大，所以單獨(dú)為期配備一路電源電路。圖3.1電源電路3.2 電機(jī)驅(qū)動(dòng)電路電機(jī)的驅(qū)動(dòng)電路有很多種，這里使用的是單管驅(qū)動(dòng)電路。這個(gè)電路實(shí)現(xiàn)簡(jiǎn)單，發(fā)熱少，因此不用很大的散熱片，驅(qū)動(dòng)能力很強(qiáng)。圖3.2電機(jī)驅(qū)動(dòng)電路3.3 舵機(jī)驅(qū)動(dòng)電路堆積驅(qū)動(dòng)電路包括兩部分，一部分是電源電路，另一部分是信號(hào)連接電路。3.3.1 電源電路為了實(shí)現(xiàn)方便，使用元器件少，不占用太多的電路板使用面積，同時(shí)舵機(jī)對(duì)電源要求很低，采用二極管降壓的方法為舵機(jī)提供電源。信號(hào)連接電路為了降低電機(jī)對(duì)單片機(jī)的干擾，使用光電耦合器作信號(hào)隔離。3.4 主控辦接口主控制板主要完成各個(gè)功能電路的連接工作，因此需要配備所有的模塊的接

8、口電路，主要有傳感器數(shù)據(jù)輸入接口、電機(jī)驅(qū)動(dòng)信號(hào)輸出接口、舵機(jī)驅(qū)動(dòng)信號(hào)輸出接口、單片機(jī)最小系統(tǒng)板接口。接口用直立插針實(shí)現(xiàn)。圖3.4主板接口電路3.5 傳感器電路智能車需要沿著黑色引導(dǎo)線在白色的跑道上行駛，傳感器的穩(wěn)定性直接決定智能車能否按照正確的軌跡行進(jìn)。可見(jiàn)光傳感器容易受到光照強(qiáng)度的影響，因此必須采用不易收環(huán)境可見(jiàn)光強(qiáng)度影響的紅外線傳感器。這里使用的紅外線傳感器的型號(hào)是JX359，JX359對(duì)黑色和白色的分辨能力很強(qiáng)，用它來(lái)做尋找引導(dǎo)線的傳感器具有很好的穩(wěn)定性。圖3.5傳感器電路第四章 智能車的控制4.1 主要代碼分析PID控制是最早發(fā)展起來(lái)的控制策略之一，由于其算法簡(jiǎn)單、魯棒性好和可靠性高，

9、被廣泛應(yīng)用于工業(yè)控制2。因此采用數(shù)字PID控制算法作為智能車的運(yùn)動(dòng)控制算法。若把智能車看做是一個(gè)關(guān)心環(huán)節(jié)，那么他的時(shí)間常數(shù)將會(huì)相對(duì)較大，因此PID控制算法中的微分環(huán)節(jié)對(duì)改善系統(tǒng)的性能不會(huì)有太大的貢獻(xiàn)，最終，對(duì)智能車的運(yùn)動(dòng)控制采用數(shù)字PI控制。數(shù)字PI算式中，將積分環(huán)節(jié)作了處理，積分時(shí)間并不是從0開(kāi)始，而是從一個(gè)積分周期前到當(dāng)前時(shí)刻，積分時(shí)間常數(shù)取30ms。dst = dst0 - (Kp*e + Ki*(e+eold);其中dst0為控制量為0時(shí)的PI控制器的輸出，Kp為比例系數(shù)，Ki為積分系數(shù)。程序中dst04650，Kp70，Ki3。為了能使智能車的速度能夠在各種情況下都能夠達(dá)到或者接近理

10、想的最高速度，在速度控制上采用變速控制的方法。由于控制前輪轉(zhuǎn)動(dòng)的舵機(jī)的轉(zhuǎn)動(dòng)速度是固定不變的，因此當(dāng)引導(dǎo)線相對(duì)車體的位置變化角速度大于前輪的轉(zhuǎn)動(dòng)速度后，智能車將無(wú)法穩(wěn)定的跟蹤引導(dǎo)線。在引導(dǎo)線的位置變化較大時(shí)，降低車速是十分必要的。速度的控制算法是按照傳感器的輸出的變化率作為減速與否的標(biāo)準(zhǔn)，具體方法如下：首先，單片機(jī)對(duì)從傳感器采集回的位置信號(hào)進(jìn)行采樣，采樣周期為30ms。舵機(jī)的轉(zhuǎn)動(dòng)角速度常數(shù)為：在工作的電壓為4.8V 的情況下旋轉(zhuǎn)60度耗時(shí)0.2秒即300度每秒。與其相應(yīng)的引導(dǎo)線相對(duì)傳感器每移動(dòng)過(guò)一個(gè)傳感器耗時(shí)不應(yīng)超過(guò)6.67ms。當(dāng)采樣周期為30ms時(shí)，每個(gè)周期內(nèi)，傳感器輸出的偏差變化值（程序中

11、定義為deltae）應(yīng)小于4。然后，如果deltae大于或等于4，則減速，否則加速。減速為突然減速，而加速采用逐漸家速度方法以防止電機(jī)因突加給定而電流過(guò)大。相應(yīng)的調(diào)速代碼如下：if (time <= AbsoluteTime)deltae = e - eold;if (deltae < 0) deltae = -deltae;if (deltae < 3)speed+;if (speed > 13)speed = 13;if (deltae >= 3) speed = 2;eold = e;time += 30;第五章 車體調(diào)速方法5.1 智能車的質(zhì)量對(duì)性能的影響

12、參加本次智能車大賽的隊(duì)伍將進(jìn)行一場(chǎng)速度賽，因此提高智能車的速度是本次比賽的關(guān)鍵所在。首先，車體的質(zhì)量將會(huì)影響智能車的行進(jìn)速度。車體質(zhì)量的大小直接決定了智能車在行進(jìn)過(guò)程中受到的滾動(dòng)摩擦力的大小，車體的質(zhì)量越大，滾動(dòng)摩擦阻力將越大。過(guò)大的質(zhì)量所引起的車輪滾動(dòng)摩擦力的增加，不僅會(huì)使智能車的起動(dòng)速度降低，同時(shí)也會(huì)降低智能車的最高前進(jìn)線速度。其次，車體質(zhì)量和重心的位置將影響智能車的轉(zhuǎn)向性能。假設(shè)車體的重心在智能車車體的前端，那么，在智能車轉(zhuǎn)彎時(shí)，前輪將承受較大的摩擦力，同時(shí)，前輪將分擔(dān)較大的向心力為車體提供向心加速度，這將導(dǎo)致智能車轉(zhuǎn)向靈活度降低，降低智能車進(jìn)入彎道的線速度（線速度過(guò)高，會(huì)因轉(zhuǎn)向不靈活而

13、沖出跑道），從而影響比賽的成績(jī)。為了提高智能車的行進(jìn)速度和轉(zhuǎn)向的靈活程度，在智能車的制作上，我們盡量減小了車體的質(zhì)量，同時(shí)將重心先后移動(dòng)。具體實(shí)現(xiàn)方法如下：我們?cè)谠O(shè)計(jì)傳感器的PCB板和主控板時(shí)，特別注意在不影響系統(tǒng)的穩(wěn)定性的基礎(chǔ)上，盡量較小電路板的使用面積。電路設(shè)計(jì)上，為了減小使用面積，在完成電路功能的基礎(chǔ)上，使用了盡量少的元器件。沒(méi)有為主控制板和傳感器的電路板安裝特別的安裝支架，而是將他們直接安裝在車體原有的安裝孔上。圖5.15.2閉環(huán)調(diào)速為了能對(duì)智能車的行進(jìn)速度進(jìn)行準(zhǔn)確的控制，并且能方便的實(shí)現(xiàn)加速和減速動(dòng)作，采用速度閉環(huán)控制是十分必要的。典型的閉環(huán)調(diào)速系統(tǒng)通常采用雙閉環(huán)調(diào)速系統(tǒng)，但是雙閉環(huán)

14、調(diào)速系統(tǒng)在實(shí)現(xiàn)時(shí)相對(duì)比較復(fù)雜，同時(shí)會(huì)增加CPU的工作負(fù)擔(dān)，使控制系統(tǒng)的采樣頻率受到影響。因此采用了容易實(shí)現(xiàn)的單閉環(huán)調(diào)速，即只用速度環(huán)。5.3變速前進(jìn)正常駕駛汽車時(shí)司機(jī)都會(huì)在彎道時(shí)減速，以防止在彎道時(shí)由于車身的慣性，導(dǎo)致側(cè)滑。參加本次比賽所使用的智能車雖然一般不會(huì)因?yàn)閼T性在突然轉(zhuǎn)向時(shí)發(fā)生側(cè)滑，但是如果智能車在行進(jìn)路徑中的直線加速過(guò)程中獲得了很大的動(dòng)能，進(jìn)入彎道的瞬間難免會(huì)因?yàn)檐圀w慣性而沖出跑道。所以智能車的速度不能是一成不變的，應(yīng)該在有彎道的地方減速。那么，減速的依據(jù)是什么、速度應(yīng)該降低多少、彎道內(nèi)維持多大的線速度等主管因素將直接影響智能車的平均行進(jìn)速度。在進(jìn)入彎道后，智能車能否正確的沿著引導(dǎo)線

15、前進(jìn)，主要依賴控制前輪轉(zhuǎn)向的舵機(jī)能否在有限的時(shí)間里轉(zhuǎn)動(dòng)得到指定的位置。舵機(jī)有它的固有響應(yīng)速度參數(shù)：在工作的電壓為4.8V 的情況下旋轉(zhuǎn)60度耗時(shí)0.2秒即300度每秒，那么在速度調(diào)整周期內(nèi)，如果引導(dǎo)線偏離智能車車體前端傳感器中間位置相對(duì)車體的角速度大于300度每秒，舵機(jī)將來(lái)不及對(duì)傳感器信號(hào)做出反應(yīng)，智能車將沖出跑道。因此，影響智能車轉(zhuǎn)向性能的因素不是彎道的存在，而是引導(dǎo)線位置相對(duì)車體變化的角速度。根據(jù)以上的分析，智能車能否沿引導(dǎo)線順利的入彎、出彎，取決于車速的調(diào)整規(guī)律能否保證在引導(dǎo)線出現(xiàn)變化時(shí)，引導(dǎo)性位置變化相對(duì)車身的角速度小于300度每秒。所以在智能車前進(jìn)過(guò)程中，彎道的出現(xiàn)不是減速標(biāo)志，減速

16、與否完全取決于舵機(jī)的響應(yīng)速度。加速減速的具體實(shí)現(xiàn)將在程序分析中具體說(shuō)明。第六章 結(jié)論智能車車身的加工制作過(guò)程中，盡量的減輕了車體的質(zhì)量，并后移了中心，從而提高了智能車的靈活度。經(jīng)過(guò)試驗(yàn)，重心在智能車的后部要比在前部車速有了明顯的提高，穩(wěn)定性也有了一定的提高。智能車的控制方法采用了實(shí)現(xiàn)簡(jiǎn)單的控制方法，在程序?qū)崿F(xiàn)上減小了時(shí)間及空間復(fù)雜度，進(jìn)而提高了傳感器對(duì)道路的采樣頻率。經(jīng)過(guò)調(diào)試，只能車的平均速度可以達(dá)到1/左右，路徑的形式及彎道的多少，對(duì)平均速度的影響很大。智能車使用的是紅外線傳感器來(lái)檢測(cè)引導(dǎo)線的位置。紅外線傳感器的采樣頻率較高，配合有效的轉(zhuǎn)換電路，能夠輸出穩(wěn)定性很高的引導(dǎo)線位置信號(hào)。但是傳感器

17、本身性質(zhì)決定了它不能夠?qū)h(yuǎn)距離的路況信息進(jìn)行解讀，因此也就不能夠進(jìn)行有關(guān)道路信息的預(yù)先判斷。只能從采樣頻率上補(bǔ)償信息量少的弊端。在下一步開(kāi)發(fā)中，我們準(zhǔn)備用線陣作為檢測(cè)引導(dǎo)線的檢測(cè)設(shè)備。線陣可以對(duì)相對(duì)較遠(yuǎn)的道路信息進(jìn)行采集，而且分辨率高?；诰€陣的道路檢測(cè)可以對(duì)引導(dǎo)線的變化進(jìn)行預(yù)先判斷和精確的位置微分，為控制算法的實(shí)現(xiàn)會(huì)提供很大的幫助。參考文獻(xiàn)1 自動(dòng)控制原理主編：胡壽松 科學(xué)出版社出版2 新型PID控制及其應(yīng)用 主編：陶永華 機(jī)械工業(yè)出版社附 錄系統(tǒng)初始化YesNo讀取傳感器位置偏差信號(hào)e位置處理計(jì)算前輪轉(zhuǎn)角量，并輸出給舵機(jī)計(jì)算延時(shí)Deltae>=3？減速30ms加速若達(dá)到速度上限則停止

18、加速A：程序流程B：程序源代碼#include <hidef.h> /* common defines and macros */#include <mc9s12dg128.h> /* derivative information */#include "1602.h" #include "PID.h" #include "Sensend.h" #include "main_asm.h" /* interface to the assembly module */#pragma LINK_I

19、NFO DERIVATIVE "mc9s12dg128b" #define uchar unsigned char #define uint unsigned int#define con0 PORTA_BIT4#define con1 PORTA_BIT7#define KEY1 PORTE_BIT0#define KEY2 PTH_PTH2#define KEY3 PTH_PTH0#define KEY4 PTH_PTH1#define D1 PTP_PTP2#define MCIN1 PTP_PTP3#define MCIN2 PTP_PTP4#define Ctr2

20、 PORTA_BIT6#define left_max 1900/1825/1900 /1700/1800/1950#define right_max 2700/2550/2650#define middle 2290/2210 /2250/2240#define e_turn 38/偏差轉(zhuǎn)折點(diǎn)#define k1 1.5/1.5/舵機(jī)微分常數(shù)#define k2 0.8/直道加速段比率常數(shù)/0.7#define k3 0.3/彎道入直提前加速比率#define stright_limit 15/ 直道忽略閾值#define stright_v 240/直道速度#define v0 150/恒

21、速/#define Kv 0.8/0.8/彎道速降常數(shù)uchar pos_middle= 113;uchar AD_wData10; /全局變量存放 AD的結(jié)果uchar low_value10;/AD低基值uchar high_value10;/AD高基值uchar value_range10;/AD值變化范圍uint AD_cData10; /AD偏差值uint stright_turn10;/直入彎地址uchar position=114;/黑線中心位置int pos8; / 黑線中心緩沖區(qū)uchar Angle8;/轉(zhuǎn)角緩沖區(qū)uchar turn_save3000;/道路信息uchar

22、 road_v3000;/速度分配uchar n=0;/道路信息所分段數(shù)uchar p=0;/ 直入彎點(diǎn)數(shù)uint sum=0;/轉(zhuǎn)角之和int angle_sum=0;/轉(zhuǎn)角偏差之和uint angle ;uchar flag=0;uchar time_flag=0; /AD采樣時(shí)間分段標(biāo)志uchar white_flag=0;uchar stright_flag=0;/直線標(biāo)志uchar memor_flag=0;uchar turn_flag1=0;uchar pos0;/和position等同int coun1=0;/測(cè)速脈沖個(gè)數(shù)uint time_count=0;/測(cè)速高頻脈沖0.1

23、msuint speed=0;/實(shí)時(shí)車速uint time_100us=0;uint time_100ms=0;signed int e2;/位置偏差uchar start_cout=0;/起跑線次數(shù)uchar start_time=0;/一段時(shí)間內(nèi)檢測(cè)到的起跑線次數(shù)uchar start_flag=1;uchar last_maxvalue;uchar last_max=4;uchar PID_flag=1;int num;/第一圈脈沖數(shù)int num2;/第二圈脈沖數(shù)int time1;/第一圈時(shí)間int time2;/第二圈時(shí)間uchar circuit_flag=0;/一圈跑完標(biāo)志uc

24、har Brake_flag=0;/剎車標(biāo)志uchar spee1=0; /第一圈速度調(diào)節(jié)uchar spee2=0; /第二圈速度調(diào)節(jié)uchar project_flag1=0;/方案選擇標(biāo)志1uchar project_flag2=0;/方案選擇標(biāo)志2uchar project_flag3=0;/方案選擇標(biāo)志3uchar stright_flag1=0;/直線標(biāo)志/int stright_con=30000;/uchar turn_flag=0;/uchar longsright_flag=0;/uchar turn_count=0;/uchar stright_count=0;/stru

25、ct road_inf /道路信息分段結(jié)構(gòu)體 char road_flag;/直彎標(biāo)志 uint turn_sum;/積分值 uint start; /起始位置 uint end ; /結(jié)束位置 uchar vv; /入彎/直速度road50;void delay(uint t) uint i; for (i=0;i<t;i+);void delay400ms(void) uint i; for (i=0;i<16;i+) delay(50000);void delay5s() uint i; for (i=0;i<100;i+) delay(50000);void getA

26、D0_value(void) AD_wData1 = ATD0DR0; /將結(jié)果寄存器中的值存放到數(shù)組中 AD_wData3 = ATD0DR1; AD_wData5 = ATD0DR2; AD_wData7 = ATD0DR3; AD_wData9 = ATD0DR4; void getAD1_value(void) AD_wData0 = ATD1DR0; /將結(jié)果寄存器中的值存放到數(shù)組中 AD_wData2 = ATD1DR1; AD_wData4 = ATD1DR2; AD_wData6 = ATD1DR3; AD_wData8 = ATD1DR4; void cpu_init(voi

27、d) SYNR=2; /S 16MHZ超頻 REFDV=1; while(CRGFLG_LOCK=0); CLKSEL=0x80; DDRE=0xfc; DDRM=0xff; DDRB= 0xff;/0xe3; DDRP=0xff; DDRH=0xf8; DDRJ=0x3c;PORTB= 0x0f; DDRA=0xd0; /PORTA=0x20; asm nop static void MDCInit(void) MCCTL = MCCTL&0Xfb; /模數(shù)計(jì)數(shù)器禁止運(yùn)行 MCCTL = 0Xe3; /允許中斷，模數(shù)計(jì)數(shù)方式 /返回時(shí)重新加載所用的常數(shù)，分頻常數(shù)為1 MCCTL =

28、MCCTL|0X04; /模數(shù)計(jì)數(shù)器使能 MCCNT = 0X1000; MCCTL = MCCTL|0X08 ; /把模數(shù)常數(shù)寄存器的值加載到模數(shù)計(jì)數(shù)器 void speed_init() TIOS_IOS0=0; TCTL4_EDG0A=1; TCTL4_EDG0B=0; TIE_C0I=1;void Init_ECT()TIOS_IOS7=1;TCTL1_OM7=0;TCTL1_OL7=0;TIE_C7I = 1; /tc1 開(kāi)中斷TSCR1= 0X80; /enable main timerTSCR2 = 0x0d;TFLG1_C7F=1;TC7=75; /中斷 100微秒 = 75*

29、32/24/1000000void InitMotorPWM(void) PWME_PWME2=0; PWMCLK_PCLK2=1; /CHOOSE SB PWMSCLB=32; PWMDTY2=25; PWMPER2=200; PWMPOL_PPOL2=1; PWMCAE_CAE2=0; void InitServoPWM(void) PWME=0x00; PWMCTL_CON01=1; PWMPER01=12000; /PER=8000 PWMDTY01=2250; /DTY=1500 PWMCLK_PCLK1=0; /SOURCE IS CLOCK A PWMPRCLK=0x44; /P

30、RE=16 PWMPOL_PPOL1=1; /HIGH BEGIN PWME_PWME1=1;void InitSerialPort(void) SCI0BDH=0; SCI0BDL=13; SCI0CR2=0x2c;void Send(unsigned char dat) while(!SCI0SR1_TDRE); SCI0DRL=dat; void run(uchar v1)PWMDTY2=v1;/D1=0;PWME_PWME2=1;MCIN1=0;MCIN2=1;void stop(void)D1=1;MCIN1=0;MCIN2=1;PWME_PWME2=0;void fastrun(v

31、oid)D1=0;MCIN1=0;MCIN2=1;void brake(void)D1=1;MCIN1=1;MCIN2=1;void back(uchar v1)PWMDTY2=v1;MCIN1=1;MCIN2=0;void turn_angle(uint a)PWMDTY0= a/256; PWMDTY1= a%256;PWME_PWME1=1;void AD_Init(void) ATD0CTL3=0x28; ATD0CTL4=0x85; ATD0DIEN=0x00; ATD1CTL3=0x28; ATD1CTL4=0x85; ATD1DIEN=0x00; int abs1(int p)

32、if(p>=0)return p; else return(-p);/*黑線中心位置計(jì)算*/void road_infor() uchar i,max,max1,max2; uchar max_r=9; for(i=0;i<10;i+) if(AD_wDatai>low_valuei) AD_cDatai=AD_wDatai-low_valuei; else AD_cDatai=0; max=0; for(i=1;i<10;i+) if(AD_cDatamax<AD_cDatai) max=i; /Get the MAX_AD if(AD_cDatamax<

33、30) switch(flag) case 0:position=pos0;break; /125 case 1:position=0;white_flag=1;break; case 2:position=225;white_flag=2;break; default: break; else if(max>0&&max<9) if(AD_cDatamax-1>AD_cDatamax+1) max1=max-1;max2=max; else max1=max;max2=max+1; else if(max=0) max1=0,max2=1; else max

34、1=8;max2=9; position=(max1*AD_cDatamax1+max2*AD_cDatamax2)*25/(AD_cDatamax1+AD_cDatamax2); if(pos0>0&&pos0<225)white_flag=0; if(position<75) flag=1; else if(position>175) flag=2; else flag=0; if(abs1(int)position-pos0)>40) /40 position=pos0; pos0=position; /*轉(zhuǎn)角控制*/void turn1()

35、uchar e1,e_max;uint angle1;uint ang,ang_max;float kk;signed int ang1;e2=pos0-pos7;if(stright_flag)kk=0;else kk=k1; angle1=100; if(pos0>=pos_middle) e1=pos0-pos_middle; ang_max=right_max-middle; e_max=225-pos_middle; if(e1<e_turn)ang=angle1*e1/e_turn; else ang=(ang_max-angle1)*(e1-e_turn)/(e_ma

36、x-e_turn)+angle1; ang1=middle+ang+e2*kk; if(ang1>right_max)ang1=right_max; /turn_angle(ang1); else e1=pos_middle-pos0; ang_max=middle-left_max; e_max=pos_middle; if(e1<e_turn)ang=angle1*e1/e_turn; else ang=(ang_max-angle1)*(e1-e_turn)/(e_max-e_turn)+angle1; ang1=middle-ang+e2*kk; if(ang1<le

37、ft_max)ang1=left_max; turn_angle(ang1);angle=ang1; void stright_test(void) uchar i; uchar middle_flag=1; for(i=0;i<8;i+) if(posi<(pos_middle-45)|posi>(pos_middle+44)middle_flag=0; if(middle_flag&&abs1(pos0-pos7)<40) stright_flag=1; else stright_flag=0; /*if(stright_flag) if(!long

38、sright_flag) stright_con=coun1;longsright_flag=1; turn_flag=0; else longsright_flag=0; if(coun1-stright_con>15)&&speed>130) turn_flag=1; else stright_con=30000;turn_flag=0; if(pos0>80&&pos0<145)stright_flag1=1; else stright_flag1=0;*/ void speed_pid(uint v) int p; if(PID_

39、flag) PID_flag=0; sPID.vi_Ref = v; sPID.vi_FeedBack=speed; p=v_PIDCalc(&sPID )/6; if(p>200)p=200; else if(p<0)p=0; MCIN1=0;MCIN2=1; PWMDTY2=200-p; /*void v_control()int vv;if(turn_flag) PORTB=0x00;back(50);else if(stright_flag1&&!stright_flag) MCIN1=0;MCIN2=1; PWMDTY2=0; PORTB=0xf0

40、;else PORTB=0xff; MCIN1=0;MCIN2=1;/PWME_PWME2=1; if(pos0>75&&pos0<150) vv=180; else if(pos0>0&&pos0<=75) vv=142+pos0/2; else if(pos0>=150&&pos0<225) vv=255-pos0/2; else vv=180; speed_pid(vv-abs1(e2)*4/10); */int abse(int a1,int a2)if(a1>=a2) if(a1>pos_

41、middle)return (a2-a1); else return (a1-a2);else if(a1>=pos_middle)return (a2-a1); else return (a1-a2); /*void v_control1()int vv,vs; if(stright_flag=1) vv=200; vs=vv+spee1+abse(pos0,pos1)*4; speed_pid(vs); else if(speed>170) back(100);/brake(); else if(pos0>90&&pos0<165) vv=170;/

42、160 else if(pos0<=90&&pos0>0) vv=150+pos0/3; /120/140 else if(pos0>165&&pos0<255) vv=180-(pos0-165)/3; /200 else vv=180; /130 /140 vs=vv+spee1+abse(pos0,pos1)*4; speed_pid(vs); */void v_control1()int vv,vs; if(stright_flag=1) vv=200; vs=vv+spee1+abse(pos0,pos1)*4; speed_p

43、id(vs); else if(speed>170+spee1) back(100);/brake(); else if(pos0>75&&pos0<150) vv=180;/160 else if(pos0<=75&&pos0>0) vv=160+pos0/3; /120/140 else if(pos0>=150&&pos0<225) vv=185-(pos0-150)/3; /200 else vv=180; /130 /140 vs=vv+spee1+abse(pos0,pos1)*4; spee

44、d_pid(vs); /*void v_control1()int vv,vs; if(stright_flag=1) vv=200; speed_pid(vv+spee1-abs1(e2)*5/10); else if(speed>170) back(100);/brake(); else if(pos0>90&&pos0<165) vv=170;/160 speed_pid(vv+spee1-abs1(e2)*5/10); else if(pos0<=90&&pos0>0) vv=150+pos0/3; /120/140 spe

45、ed_pid(vv+spee1-abs1(e2)*5/10); else if(pos0>165&&pos0<255) vv=180-(pos0-165)/3; /200 speed_pid(vv+spee1-abs1(e2)*5/10); else vv=180; /130 /140 speed_pid(vv+spee1-abs1(e2)*5/10); */*道路信息分段處理*/void road_deal(uint numb) uchar flag0=0; uchar flag1=0; uint i; uchar m; /for(i=numb-5;i<=n

46、umb;i+) turn_savei=40; road0.road_flag=0; road0.start=0; for(i=0;i<=numb;i+) if(turn_savei<35) flag0=1; else if(turn_savei>45)flag0=2; else flag0=0; if(flag0=flag1) switch(flag0) case 1:roadn.turn_sum+=(40-turn_savei);break; case 2:roadn.turn_sum+=(turn_savei-40);break; default:break; else

47、n+; roadn-1.end=i; roadn.start=i; switch(flag0) case 1: roadn.road_flag=1; roadn.turn_sum=40-turn_savei;break; case 2: roadn.road_flag=2; roadn.turn_sum=turn_savei-40;break; default:roadn.road_flag=0;break; flag1=flag0; roadn.end=numb; if(n>2) for(i=0;i<=n;i+) if(roadi.end-roadi.start<15) i

48、f(roadi-1.road_flag=roadi+1.road_flag) roadi-1.end=roadi+1.end; roadi-1.turn_sum+=roadi+1.turn_sum; n-=2; for(m=i;m<=n;m+) roadm=roadm+2; i-; else roadi+1.start=roadi.start; roadi=roadi+1; n-; for(m=i+1;m<=n;m+) roadm=roadm+1; i-; /*入彎段標(biāo)記*/void mark(void) uchar i; for(i=0;i<n;i+) if(roadi.r

49、oad_flag=0&&roadi+1.road_flag) stright_turnp=roadi.end;p+; /*速度分配*/void v_deal(void) uchar i; uint m,mid; float Kv; if(project_flag3) Kv=0.8; else Kv=1.5; for(i=0;i<=n;i+) if(!roadi.road_flag) if(i=n)for(m=roadn.start;m<=roadn.end;m+)road_vm=stright_v; else if(roadi.end-roadi.start>stright_limit) mid=(roadi.end-roadi.start)*k2+roadi.start; for(m=roadi.start;m<mid;m+) road_vm=stright_v; roadi.vv=190-(unsigned long)(Kv*roadi+1