第1-2屆飛思杯全國生智能汽車邀賽技術(shù)報告83篇軍交隊

1、第一屆“飛思”杯大學生智能汽車邀請賽技術(shù)學校：軍事交通學院隊伍名稱：軍交 1 隊參賽隊員：帶隊教師：關(guān)于技術(shù)和研究使用的說明本人完全了解第一屆“飛思”杯大學生智能汽車邀請賽關(guān)保留、使用技術(shù) 委會和飛思案、技術(shù)委會和研究的規(guī)定，即：參賽作品著作權(quán)歸參賽者本人，比賽組半導體公司可以在相關(guān)主頁上收錄并公開參賽作品的設(shè)計方以及參賽模型車的集中。、圖像資料，并將相關(guān)內(nèi)容編纂收錄在組參賽隊員簽名：帶隊教師簽名：日期： 8 月 5 號 第一章：引言1設(shè)計制作思路1設(shè)計方案概述2第二章： 硬件部分2車模組裝2傳感器的設(shè)計與安裝2傳感器的選擇及工作原理2傳感器的布置方案2傳感器信號探測及處理電路4系統(tǒng)電路板的設(shè)

2、計、安裝與連接4驅(qū)動電機和舵機5主要車模參數(shù)6第三章：部分7控制算法7理論介紹7程序?qū)崿F(xiàn)7代碼設(shè)計10第四章：結(jié)論10主要性能1改進方向1參考文獻2附錄：程序源代碼2第一章：引言1.1 設(shè)計制作思路為了實現(xiàn)對小車的智能控制，本設(shè)計以 MC9S12DG128 為，將傳感器得到的路況信息進行綜合判別和處理，然后以脈寬調(diào)制（）的方式控制舵機轉(zhuǎn)向和驅(qū)動電機加速或，從而控制小車，使小車能夠快速、準確地識別特定路線并按適當速度行駛。多傳感器并行工作、CPU 的綜合數(shù)據(jù)處理為小車實現(xiàn)自動加速、限速、左轉(zhuǎn)、右轉(zhuǎn)提供了充分的保證。先后經(jīng)過了組裝車模、傳感器的選擇與布置、系統(tǒng)電路板的設(shè)計與安裝、開發(fā)工具 BDM

3、的調(diào)試以及控制算法的不斷改進等過程，完成對智能車粗略控制到精確控制，并使智能車由低速逐漸向高速過渡。在實驗的基礎(chǔ)上不斷發(fā)現(xiàn)問題，并不斷調(diào)試解決問題，使智能車能夠最大限度地沿著軌道快速、準確地行駛。1.2 設(shè)計方案概述本設(shè)計方案主要分硬件、結(jié)論三個方面進行介紹。硬件部分主要闡述了外部傳感器的選擇、安裝、傳感器的探測原理、傳感器的探測電路設(shè)計，系統(tǒng)電路板的電路設(shè)計及安裝，驅(qū)動電機和舵機的控制。 控制算法的理論知識，程序以及代碼的設(shè)計。結(jié)論部分主要部分主要介紹了了小車的性能以及改進的方向。現(xiàn)在對本技術(shù)的主要部分概括介紹如下：傳感器部分：本智能車的傳感器采用了反射式紅外光電傳感器，雙排布置。在智能車的

4、頭部安裝了兩塊電路板，兩塊電路板的布置形式呈工字形。智能車通電后，光電傳感器的發(fā)光管發(fā)出探測光，如果探測到白色部分，接收管能夠接收到反射光；如果探測到黑色部分則探測光被吸收，接收管就接收不到反射光。根據(jù)此原理，把電信號轉(zhuǎn)換成數(shù)字信號，從而把路面信息反映到控制系統(tǒng)，通過CPU 的綜合數(shù)據(jù)處理，對智能車的轉(zhuǎn)向和速度進行控制?？刂葡到y(tǒng)部分：制作了以MC9S12DG128為的系統(tǒng)電路板，它給布置線路帶來了極大的方便，并可以非常方便地安裝在智能車上，使系統(tǒng)得到了簡化。控制算法部分：采用了目前自動控制領(lǐng)域中最常用的控制算法控制算法進行車輛行駛控制。在本方案的設(shè)計中采用了直接計算法。在行駛過程中，由于車輛轉(zhuǎn)

5、向造成車輛中心與黑線有一定的偏差量，就選擇這一偏差量作為輸入的偏差。本設(shè)計方案中以前排電路板的中心為車輛中心線所在位置，在車輛行駛過程中，當某一傳感器感應到黑線位置時，就以該傳感器與電路板中心的距離作為車輛的行駛偏差，以此作為控制器的輸入量。為了改善控制算法中輸入量不連續(xù)，引入車速作為算法輔助輸入量。開發(fā)工具采用了主辦方提供的BDM 調(diào)試器，在中就不再贅述。第二章： 硬件部分車模組裝嚴格按照說明對車模進行組裝。重點是準確地調(diào)整舵機，使舵機和前輪處于最佳的工作位置。傳感器的設(shè)計與安裝2.2.1傳感器的選擇及工作原理根據(jù)要求，有兩個選擇。第一種是采用CCD傳感器，以一定的頻率對路面信息進行。CPU

6、通過圖象處理判斷小車所處相對的位置。使用CCD進行圖象和識別的方法，雖然該方案獲得的信息較精確，但是不適用在小體積系統(tǒng)中使用，并且還涉及圖象、圖象識別等領(lǐng)域，占用系統(tǒng)資源較多，算法較復雜。第二種是采用反射式紅外光電傳感器，以一定的頻率向CPU提供路面標志信號。CPU經(jīng)過比較處理，粗略判斷出小車所處的相對位置。該傳感器體積小，價格便宜，使用方便，而且該方案占用系統(tǒng)資源較少，算法較易實現(xiàn)。故在本設(shè)計中采用了這種傳感器，鑒于車底盤較低，采用近距離（2cm4cm）有效的傳感器。工作原理如圖2.1所示：路面探測距離圖 2.1 傳感器工作原理圖反射式光電傳感器有發(fā)射管和接收管組成，工作時由發(fā)射管發(fā)射紅外線

7、，經(jīng)被探測物體面反射，反射的紅外線由接收管接收，接收管把接收的物理信號轉(zhuǎn)換成電信號。被檢測物體表面的顏色和粗糙程度影響反射光的強弱，反射面越不光滑，顏色越暗，反射光就會越弱。這里就是要運用紅外光電傳感器對顏色敏感的特點，當檢測物體表面的顏色為黑色時，反射光很弱，接收管接收的光線可以忽略，使接收管處于一種狀態(tài)，例如開關(guān)管截止；當檢測物體表面的顏色為白色時，反射光較強烈。使接收管處于另一種狀態(tài)，例如開關(guān)管導通。這兩種相反的狀態(tài)表現(xiàn)在電路中就是高低電平組成的脈沖信號。這個脈沖信號就是控制系統(tǒng)的輸入信號，并且設(shè)的輸入信號為 1 低電平信號輸入為 0。通過編寫控制算法，根據(jù)傳感器搜集的路況信息，調(diào)節(jié)機和

8、驅(qū)動電機，實現(xiàn)小車的智能控制。的占空比，控制舵接收發(fā)射2.2.2 傳感器的布置方案為了減弱外界光線對反射式紅外光電傳感器的影響，且考慮到小車底盤較低，把傳感器放置在設(shè)計的電路板下，垂直探測地面。傳感器的布置方案有單排、雙排、W 字形排列等。由于 W 字形排列的算法比較復雜，了單排布置和雙排布置的方案。只考慮首先選用了單排布置，也就是在車模的頭部安裝一塊電路板，其底部安裝 8 個傳感器。通過對傳感器探測到黑色導航線的情況的分析，用編碼的形式把傳感器探測到的路面信息表示出來。然后把這些路況信息輸入到控制系統(tǒng)通過編寫的控制算法就可以控制舵機和驅(qū)動電機，實現(xiàn)智能車的加速、減速、左轉(zhuǎn)、右轉(zhuǎn)了。通過不斷的

9、實驗，來進一步地調(diào)整各種編碼下，智能車要轉(zhuǎn)的角度的參數(shù)。但是這種控制算法對智能車進行控制時，并沒有考慮速度對其轉(zhuǎn)向的影響，只是簡單地在轉(zhuǎn)彎時，在直道時加速。因此在實驗中發(fā)現(xiàn)智能車在跑道上慢速行駛時還能比較平穩(wěn)的跑完全程，其速度稍微加大時，智能車在拐彎處即使進行了，也會由于慣性常會飛出跑道。改進的方法有二：一是改進算法，采用控制算法，它可以解決車輛行駛速度與轉(zhuǎn)向角度大小的相關(guān)性問題，行駛軌跡的及問題；二是增大傳感器的預描距離，進入彎道時提前。就雙管齊下，增大了傳感器探測電路板伸出智能車的距離。同時為了獲得控制算法比較準確地輸入量，就排的后面又安裝的信息。最終了一塊電路板，并在它的下面安裝了 6

10、個傳感器，采用了雙排布置，即在智能車的頭部安裝了前、后兩塊電路板。其形狀及尺寸如圖 2.2（：毫米）所示：圖 2.2兩塊電路板的布置如圖 2.3 所示：排電路板上任意兩個相鄰的傳感器的中心距離位 2.5cm；在后排電路板上，中間兩個傳感器的中心距離為 5.0cm，其余的相鄰兩傳感器間的距離也為 2.5cm。前后兩塊電路板傳感器分布所在直線間的垂直距離為 10.8cm。圖 2.32.2.3 傳感器信號探測及處理電路傳感器信號探測及處理電路設(shè)計如圖 2.4 所示：圖 2.4D1 為紅外光電傳感器的發(fā)射管，D2 為其接收管。D1 發(fā)射紅外線，D2 接收信號，判斷是否有反射信號。如果有則 D2 輸出，

11、如果沒有則輸出低電平，信號經(jīng)過三極管、集成運放器輸出到 CPU 的PORTA 或PORTB。14 個傳感器獲得的信號并行輸入單片機，從而把路面信息反饋到控制系統(tǒng)，實現(xiàn)對智能車的控制。制作電路板后，要把探測電的元件一一對應地焊接到電路板上。焊接電阻時，要注意防止虛焊；焊接插件時要注意避免兩個焊接點連接在一起造成短路；焊接發(fā)光二極管和接收二極管前要用萬用表測量一下極性，防止將二極管反向安裝。2.3 系統(tǒng)電路板的設(shè)計、安裝與連接沒有采用主辦方提供了S12 的電路板，自行設(shè)計了系統(tǒng)電路板。首先在S12 電路板體積比較大，既不方便在智能車上安裝，也不美觀；其次S12 電路板上找到正確的引腳插線，也顯得比

12、較復雜。設(shè)計的系統(tǒng)電路板就解決了這方面，可以把它很方便地固定在智能車的底盤上，并制作了轉(zhuǎn)用的插槽(如圖 2.2 所示)。制作了以MC9S12DG128為，并附以復位電路、晶體振蕩器及時鐘電路、+5V 的電源、驅(qū)動電機的供電插座、單片機引腳插槽以及其他一些常規(guī)電子元件組成的系統(tǒng)控制電路。系統(tǒng)控制電路原理圖如圖 2.5 所示：圖 2.52.4 驅(qū)動電機和舵機圖 2.6 舵機和電機工作原理原理圖轉(zhuǎn)向舵機MC33886加電機算法波MC9S12DG128傳感器信息驅(qū)動電機和舵機是智能車整個系統(tǒng)的執(zhí)行的控制系統(tǒng)由傳感器獲得路面信息，通過。以 MC9S12DG128為控制算法來調(diào)節(jié)舵機和驅(qū)動的占空比，控制智

13、能車的轉(zhuǎn)向和車速。其工作原理圖 2.6 所示。本電機的次比賽要求不得對驅(qū)動電機和舵機進行改造，本文對其不再贅述。2.5 主要車模參數(shù)車重：1.1kg電路功耗：長：384mm寬：185mm 高：77mm所有電容總?cè)萘浚?900uF傳感器種類：反射式紅外光電傳感器 數(shù)目：14伺服電機個數(shù)：0e)賽道信息檢測精度：6mm頻率：1000Hz第三章：部分3.1 控制算法本智能車輛的控制算法部分主要需解決如下問題：車輛直線行駛的穩(wěn)定性問題，車輛轉(zhuǎn)彎控制問題，車輛行駛速度與轉(zhuǎn)向角度大小的相關(guān)性問題，行駛軌跡的及問題等。為了更好的解決設(shè)計中存在，本方案采用了目前自動控制領(lǐng)域中最常用的控制算法3.1.1 理論介

14、紹控制算法進行車輛行駛控制?？刂扑惴òㄖ苯佑嬎惴ê驮隽克惴ǎ^的增量算法就是相對于標準的相鄰兩次運算之差，得到的結(jié)果是增量。也就是說在上一次的控制量的基礎(chǔ)上需要增加（負值意味著減少）控制量，例如對于智能小車控制算法，就是智能小車相對于上一次轉(zhuǎn)向角度還需要增加或減少的轉(zhuǎn)向角度。在本方案的設(shè)計中采用了 直接計算法。（一）算法中常用概念解釋如下：1、基本偏差 e(t)表示當前測量值與設(shè)定目標間的差，設(shè)定目標是被減數(shù)，結(jié)果可以是正或負，正數(shù)表示還沒有達到，負數(shù)表示已經(jīng)超過了設(shè)定值。這是面向比例數(shù)據(jù)。2、累計偏差 ：的變動調(diào)節(jié)就進行直至無差，積分調(diào)節(jié)停止，積分調(diào)節(jié)輸出一常值。積分作用的強弱取決于積分

15、時間常數(shù) Ti，Ti 越小，積分作用就越強。反之 Ti 越大則積分作用越弱，加入積分調(diào)節(jié)可使系統(tǒng)穩(wěn)定性下降，動態(tài)相應變慢。積分作用常與另兩種調(diào)節(jié)規(guī)律結(jié)合，組成 PI 調(diào)節(jié)器或調(diào)節(jié)器。微分調(diào)節(jié)作用：微分作用反映系統(tǒng)偏差信號的變化率，具有預見性，能預見偏差變化的趨勢，因此能產(chǎn)生超前的控制作用，在偏差還沒有形成之前，以被微分調(diào)節(jié)作用消除。因此，可以改善系統(tǒng)的動態(tài)性能。在微分時間選擇合適情況下，可以減少超調(diào)，減少調(diào)節(jié)時間。微分作用對于噪聲干擾有放大作用，因此過強的加微分調(diào)節(jié)，對系統(tǒng)不利。此外，微分反應的是變化率，而當輸入沒有變化時，微分作用輸出為零。微分作用不能單獨使用，需要與另外兩種調(diào)節(jié)規(guī)律相結(jié)合，

16、組成 PD 或控制器。（三）智能車輛控制算法中參數(shù)的選擇此次參賽的智能車輛主要是尋跡行駛，控制算法所要做的就是控制車輛沿著預先設(shè)計好黑線行駛，保證車輛中心線在黑線上。在行駛過程中，由于車輛轉(zhuǎn)向造成車輛中心與黑線有一定的偏差量，就選擇這一偏差量作為輸入的偏差。本設(shè)計方案中以前排電路板的中心為車輛中心線所在位置，在車輛行駛過程中，當某一傳感器感應到黑線位置時，就以該傳感器與電路板中心的距離作為車輛的行駛偏差，以此做為控制器的輸入量。如圖 3.1 所示：e(t傳感器圖 3.1通過直接算法公式（公式 2）計算出t 時刻智能車的控制轉(zhuǎn)向角，由此達到對智能車轉(zhuǎn)向進行控制的目的。但是由于輸入量取值是由傳感器

17、的位置決定，而傳感器的數(shù)量又是有限的，所以只能獲得有限的精確輸入量，這樣通過控制算法得出的控制轉(zhuǎn)角就不是很理想。為了改善控制算法中輸入量不連續(xù)，引入車速作為算法輔助輸入量。通過車速、車輛上一次轉(zhuǎn)角、車輛長度等值之間的數(shù)學關(guān)系，可以計算出車輛中心與黑線中心在一次傳感器采樣周期內(nèi)的偏差，以此值與傳感器采樣值之間做比較，從而獲得比較理想的偏差值，為控制算法找到較理想的輸入量。車速、車輛上一次轉(zhuǎn)角、車輛長度等數(shù)值之間中心黑線車 輛 中的關(guān)系可由圖 3.2 導出：經(jīng)過t 時間后車輛轉(zhuǎn)過的角度為Kd 的值，然后通過實驗找到合適的三個控制參數(shù)值。3.1.2 程序?qū)崿F(xiàn)數(shù)值分析和變量之間的們通過以上的分析過程已

18、經(jīng)基本了解，要通過程序?qū)⒁陨线^程實現(xiàn)并達到預期的效果，還需要做一些準備工作。首先是將轉(zhuǎn)向舵機和驅(qū)動電機的驅(qū)動程序設(shè)計好，然后測出智能車的最大行駛速度和最大轉(zhuǎn)角，量出車輛軸距以及程序設(shè)計中需要用的一些參數(shù)。完成這些準備工作之后，畫出程序流程圖，如圖 3.3 所示：開始否是傳感器的檢測值=0？否程序是否結(jié)束圖 3.3程序結(jié)束圖 3.3通過程序流程圖，很快就可以確定下來編程方案，按照結(jié)構(gòu)化程序設(shè)計的要求，對程序中的函數(shù)都進行模塊化設(shè)計，保證程序的可讀性和易現(xiàn)預期的設(shè)計目的。3.2 代碼設(shè)計性，實通過以上分析，在編程時需要解決以下問題：調(diào)用轉(zhuǎn)向函數(shù)和調(diào)速函數(shù)，調(diào)整車速和進行數(shù)值編碼調(diào)用函數(shù)輸出控制角度

19、和檢測端口值的變化并到變量中定義變量和函數(shù)并初始化數(shù)據(jù)（1）控制算法的程序?qū)崿F(xiàn)（2）基本計算函數(shù)的編程實現(xiàn)（3）輸入輸出量與算法之間的結(jié)合（4）變量之間數(shù)據(jù)類型的變化根據(jù)以上提出了下述解決方法：（1） 定義計算的結(jié)構(gòu)體如下：typedef structfloat float float float float float floatSetPo;/設(shè)定目標 Desired Value比例常數(shù) Proportional Const積分常數(shù)egral Const 微分常數(shù) Derivative Const Error-1Error-2Proportion; egral; Derivative; La

20、stError; PrevError;SumError;SumsofErrors;（2）定義基本計算函數(shù)floatCalc(floatdError,Error;*pp, floatNextPo)/計算Error = pp-SetPo-NextPo;/偏差積分當前微分pp-SumError += Error; dError = pp-LastError- pp-PrevError;pp-PrevError = pp-LastError; pp-LastError = Error;return+);(pp-ProportionErrorpp-SumErrordError/比例項積分項微分項pp-e

21、gral *pp-Derivativefloat R(floatfloatx)/計算車輛轉(zhuǎn)彎半徑resu=200.0*(cosf(x*3.1415926/180.0)*(cosf(x*3.1415926/180.0)/(sinf (x*3.1415926/180.0);returnresu;float angela(float x)/計算角度 areturn (180.0+x-51.3);float angelb(float x,float y)/計算角度 bfloat rest=(180.0*y*0.025)/(R(x)*3.1415926); return rest;float angel

22、c(float x)/計算角度 cfloat resul=acosf(Rnext(x)*Rnext(x) 180.0/3.1415926;return resul;.0-R(x)*R(x)/(256.0*Rnext(x)*float Rnext(float x)float/計算車輛中心點轉(zhuǎn)彎半徑resaff=R(x)*R(x).0);.0-2.0*R(x)*128.0*cosela(x)*3.1415926/180float reste= sqrtf(resaff); return reste;float clength(float x,float y) /計算部分位移return (2.0*

23、Rnext(x)*sinf(y*3.1415926/180.0);float slength(float x,float y)/計算最終位移return(clength(x,y)*sinf(90.0+y/2-38.66-angelc(x)*3.1415926/180.0);（3）確定輸入輸出量與定義函數(shù)之間的關(guān)系coding ( unsigned charsensors)a=0;floength=0.0,rIn=0.0; switch (sensors)case case case case case case case case case case case case case case1:

24、3:2:6:4:length=12.5;a=1;break;/speed = 50length=25.0;a=1;break;/speed =50;length=37.5;a=1;break;/ speed = 45;length=50.0;a=1;break;/speed length=62.5;a=1;break;/speed=40;35;30;26;= 50;12:length=75.0;a=1;break;/speed 8: length=87.5;a=1;break;/speed16: length=12.5;a=-1;break;/speed48:length=25.0;a=-1;

25、break;/speed = 50;32:96:64:length=37.5;a=-1;break;/speed length=50.0;a=-1;break;/speed length=62.5;a=-1;break;/speed=45;40;35;30;192:length=75.0;a=-1;break;/speed128: length=87.5;a=-1;break;/speed = 26;default:a = 0; speed = 20;break;if(length=templength)rIn=length-slength(tempangle*0.2),angelb(temp

26、angle*0.2),(float)(0.2*(120-tempangle)*125);else rIn=length;tempangle =Calc ( &s,rIn );/Performerationif(tempangle0)tempangle=-1*tempangle; templength=length;a=a*()tempangle;speed=(signed char)(0.2*(120-tempangle)+26; return a;（4）變量之間數(shù)據(jù)類型的變化有以上程序可以知道各個變量之間有著數(shù)據(jù)類型變化，因而考慮在計算具體數(shù)值時采用浮點型數(shù)據(jù)，而最后輸出量采用整型或無符號字

27、符型輸出，便于計算和對單片機端口的寫入。第四章：結(jié)論4.1 主要性能總體上，智能車已經(jīng)實現(xiàn)了加速、黑色導航線比較平穩(wěn)、快速地行駛了。、轉(zhuǎn)向、剎車的基本功能，能夠沿本智能車輛的控制算法部分主要解決如下問題：車輛直線行駛的穩(wěn)定性問題，車輛轉(zhuǎn)彎控制問題，車輛行駛速度與轉(zhuǎn)向角度大小的相關(guān)性問題，行駛軌跡的及問題等?？刂扑惴ň涂梢越鉀Q這些問題。它有諸多優(yōu)點：1）采用狀態(tài)控制。在每個很小的一段時間內(nèi)，認為車輛狀態(tài)不變，因而在這段時間內(nèi)可將車輛作靜態(tài)處理。轉(zhuǎn)向目的就是減小小車所在的中心線與車輛行駛的黑色導航線的靜態(tài)誤差。狀態(tài)控制不考慮時間過程，而注重調(diào)整前后的狀態(tài)變化。實際應用過程中，當時間足夠小時，由于執(zhí)

28、行機構(gòu)的響應能力遠遠低于單片機的運行速度，實際控制操作效果就如同連續(xù)控制一樣，保證了控制的準確性。2）控制輸出不要求最優(yōu)。從實際情況可知，在相同的情況下，同一駕駛員兩次控制的結(jié)果可能不盡相同的，具有一定的隨機性，但結(jié)果都能完成所要求的任務。因此，對于本智能車的控制輸出的轉(zhuǎn)向角，不要求它在當前位置是最優(yōu)的，而只要求它滿足兩個條件，即方向正確，轉(zhuǎn)向角度合理。3）控制算法交易實現(xiàn)。比較其他比較復雜的算法，如最優(yōu)控制必須考慮時間的過程，因而需要了解各執(zhí)行機構(gòu)的動態(tài)響應特性，以獲得準確的傳遞函數(shù)。而本文提供的算法只需要車輛當前的狀態(tài)信息，即小車的當前位置（由傳感器提供）與小車所應處的位置（由算法提供），

29、即 到轉(zhuǎn)向控制角。整個過程不涉及執(zhí)行機構(gòu)的動態(tài)響應函數(shù)，從而降低了難度。4.2 改進方向本控制算法還存在一些：1）由于傳感器的預描距離小（10cm 左右），隨著車速的提高，要求控制的頻率提高。由于系統(tǒng)硬件的限制很難滿足高速情況下的轉(zhuǎn)向控制的要求。2）作為輔助輸入量的速度并不十分準確，所取的速度是算法賦予小車的速度，并非小車的真實速度。不同的路面，不同的轉(zhuǎn)小車的速度都會產(chǎn)生影響，而這些影響是很難量化的。3）算法中參數(shù)的調(diào)整完全依靠實驗人工標定，工作量比較大。這些問題都亟待解決。參考文獻01. 自動控制原理/蔣大名、社，2003.6華主編。：、北方交通大學現(xiàn)代傳感技術(shù)/C 程序設(shè)計：第 2 版/，

30、2001.4著。東學，2001.6著。開發(fā)方法/：著。04. 單片機應用的：，2004.10附錄：程序源代碼#include #include #include stdio.h#include math.h#define start_speed 5;#pragma LINK_INFO DERIVATIVE mc9s12dg128btypedef structfloatSetPo;floatProportion;floategral;floatDerivative;floatLastError;floatPrevError;floatSumError;s;temp;angle;float spe

31、ed;char flag;float templength;float coding( unsigned char sensors);void initial_(void);void initial_atd(void) ;char acc_sensorX (void);char acc_sensorY (void);void steer_(float angle);void drive_(signed char speed);voidInit (*pp);floatCalc(*pps, float NextPo);float R(float x);float angela(float x);f

32、loat angelb(float x,float y);float angelc(float x);float Rnext(float x);float clength(float x,float y);float slength(float x,float y);floatR(float x) return (200*cos(x)*cos(x)/sin(x);float angela(float x)return (180+x-51.34);float angelb(float x,float y)return (180*y*0.025/R(x);float angelc(float x)

33、return (arccos(Rnext(x)*Rnext(x)-R(x)*R(x)/(256*Rnext(x);float Rnext(float x) return (sqr(R(x)*R(x)-2*R(x)*128*cos(angela(x);float clength(float x,float y)return (2*Rnext(x)*sin(y); float slength(float x,float y)return (clength(x,y)*sin(90+y/2-38.66-angelc(x); void TimerOverflow(void) .while (TCNT !

34、= 0 x0000);while (TCNT = 0 x0000);flag = 1;void initial_(void)E = 0X82;CTL = 0X80;CLK = 0X80;PRCLK = 0X24;SCLA = 0X40;SCLB = 0X02;POL = 0X82;CAE = 0X00;PER1 = 0X40;DTY1 = 0X10;PER67 = 0X2710;DTY67 = 0X2D0;void initial_atd(void)ATD0CTL2 = 0 xC0;ATD0CTL3 = 0 x18;ATD0CTL4 = 0 xA7;ATD0CTL5 = 0 xb1;ATD0D

35、IEN = 0 x00;float coding ( unsigned charsensors)float a,length;float rIn,rOut;switch (sensors)case 1: length=12.5;speed = 50;break;case 3: length=25;speed = 50;break;case 2: length=37.5; speed = 45;break;case 6: length=50;speed = 40;break;case 4: length=62.5;speed = 35;break;case 12:length=75;speed

36、= 30;break;case 8: length=87.5;speed = 25;break;case 16: length=-12.5;speed = 50;break;case 48:length=-25;speed = 50;break;case 32: length=-37.5;speed = 45;break;case 96: length=-50;speed = 40;break;case 64: length=-62.5;speed = 35;break;case 192:length=-75;speed = 30;break;case 128: length=-87.5;speed = 25;break;default:a = 0; speed = 20;break;if(length=templength) rIn=length-slength(temp*0.2,angelb(temp*0.2,speed);rIn=length;a =Calc ( &s,rIn );templength=length;retur