山東大學F1技術報告

第三屆全國大學生智能汽車大賽第三屆“飛思卡爾”杯全國大學生智能汽車邀請賽技術報告 學校：山東大學隊伍名稱：F1參賽隊員：田崇翼田濱孫慶軒帶隊教師：楊西峽關于技術報告和研究論文使用授權的說明 本人完全了解第二屆全國大學生“飛思卡爾”杯智能汽車競賽關保留、使用技術報告和研究論文的規(guī)定，即：參賽作品著作權歸參賽者本人，比賽組委會和飛思卡爾半導體公司可以在相關主頁上收錄并公開參賽作品的設計方案、技術報告以及參賽模型車的視頻、圖像資料，并將相關內(nèi)容編纂收錄在組委會出版論文集中。參賽隊員簽名： 帶隊教師簽名： 日期：

目錄TOC\o"1-3"\h\z第一章引言 4第二章智能車整體設計思路和方案 5第三章智能車機械部分安裝和調(diào)試 6第四章智能車的電路設計與實現(xiàn) 64.1電源部分 64.2電機驅(qū)動電路 74.3測速電路 74.4紅外對管檢測電路 84.5撥碼開關電路 9第五章智能車的控制策略和算法 105.1路徑搜索算法 105.2舵機、電機的控制 11第六章運行調(diào)試部分 136.1Codewarrior開發(fā)環(huán)境 136.2軟件仿真 136.2實際調(diào)試 14第七章技術參數(shù)統(tǒng)計 15參考文獻 16附件程序 17第三屆全國大學生智能汽車大賽PAGE28

第一章引言“飛思卡爾”杯智能車大賽是教育部為了加強大學生實踐、創(chuàng)新能力和團隊精神的培養(yǎng)而舉辦的面向全國大學生的智能汽車比賽?！帮w思卡爾”杯智能車大賽從2006年開始舉辦，今年是第三屆。比賽所涉及的專業(yè)知識包括控制、模式識別、傳感技術、汽車電子、電氣、計算機、機械等多個學科，對大學生的知識融合和實踐動手能力的培養(yǎng)，對汽車電子技術的發(fā)展，具有良好的長期的推動作用。大賽要求使用組委會統(tǒng)一提供的車模，采用飛思卡爾16位單片機MC9S12DG128作為核心控制單元，自主構思控制方案及系統(tǒng)設計，包括傳感器信號采集處理、控制算法及執(zhí)行、動力電機驅(qū)動、轉向舵機控制等。系統(tǒng)按照功能劃分為：電源模塊、單片機模塊、運行調(diào)試模塊、路徑識別模塊、直流電機驅(qū)動模塊、轉向伺服模塊、速度測量模塊等。本文將先從整體上介紹模型車的制作思路和方案，再從車子的機械、傳感器、電路等硬件方面到控制的策略、算法的實現(xiàn)和代碼等方面具體介紹模型車的制作以及調(diào)試過程，最后介紹模型車的主要技術參數(shù)。第二章智能車整體設計思路和方案由于今年組委會光電管和攝像頭分開比賽。所以傳感器部分我們選擇了光電管，比賽以小車的速度記成績，為了讓小車更快更穩(wěn)得跑完全程，傳感器的探測距離必須要遠，既要有大的前瞻，普通的紅外對管由于功率較小，探測距離增大時，干擾嚴重，所以我們自制了大功率對管，同時采用了程序控制脈沖發(fā)光的辦法，有效的降低了發(fā)熱，提高了系統(tǒng)的穩(wěn)定性。系統(tǒng)采用采用了7.2V2000mAhNi-Cd蓄電池作為系統(tǒng)能源，并且通過穩(wěn)壓電路分出6伏，5伏已分別給舵機和單片機供電。直流電機驅(qū)動模塊接收速度控制信號控制驅(qū)動電機運行，達到控制車速目的。轉向伺服模塊控制舵機轉向，進而控制智能車轉彎。速度測量模塊實時測量智能車車速，用于系統(tǒng)的車速閉環(huán)控制，以精確控制車速。系統(tǒng)充分使用了MC9S12DG128單片機的外圍模塊，具體使用到的模塊包括：ADC模擬數(shù)字轉換模塊、定時器模塊、PWM脈沖寬度調(diào)制模塊、中斷模塊、I/O端口和實時時鐘模塊等。系統(tǒng)調(diào)試過程中，使用了組委會提供的代碼調(diào)試環(huán)境CodeWarriorIDE，同時使用了清華的Plastid2軟件進行了仿真試驗。圖1.1系統(tǒng)結構框圖第三章智能車機械部分安裝和調(diào)試3.1舵機部分為了使轉彎更加靈活，對舵機相關部分作了部分改動。首先，我們將舵機力臂加長85mm。這樣，對于同樣的轉彎角度值，只需更小的舵機轉角，減小了舵機轉彎時慣性帶來的弊端。其次，我們將舵機反裝，使舵機連桿水平，因為此時舵機提供的力全部用在轉彎上。3.2前輪部分為了增加前輪轉彎時的穩(wěn)定性，對前輪相關部分進行了部分改動。首先，更改前后墊片的數(shù)量，使前輪主銷后傾，這樣，車輪具有更好的自動回正功能。其次，更改連桿的長度，使車輪外傾，車輪轉彎時，前半部分重心上移，促使賽車轉彎更加穩(wěn)定。再次，我們通過更改舵機連桿的長度，增加前輪前束，同樣增加了前輪的穩(wěn)定性。3.3底盤部分為了提高賽車運行時的穩(wěn)定性，對地盤相關部分作了部分改動。首先，前輪相關位置加墊片，降低了前輪重心。其次，更改后輪車軸處的調(diào)節(jié)塊，使后輪重心升高，這樣，車身前傾，一定程度上，增加了車的穩(wěn)定性。3.4后輪部分首先，更換后輪輪距調(diào)節(jié)塊，使后輪兩輪之間間距加大。這樣，車在轉彎時不容易產(chǎn)生側滑。其次，調(diào)節(jié)后輪差速，使賽車轉彎更加靈活。第四章智能車的電路設計與實現(xiàn)4.1電源部分為了能使智能車系統(tǒng)能正常工作，就需要對電池電壓調(diào)節(jié)。其中，單片機系統(tǒng)、車速傳感器電路需要5V電壓，路徑識別的光電傳感器和接收器電路電壓工作為5V、伺服電機工作電壓范圍4.8V到6V(或直接由電池提供)，直流電機可以使用7.2V2000mAhNi-cd蓄電池直接供電。考慮到由于驅(qū)動電機引起的電壓瞬間下降的現(xiàn)象，因此采用低壓降的三端穩(wěn)壓器成為必然。我們在采用lm7805，和lm7806作為穩(wěn)牙芯片。經(jīng)試驗電壓紋波小，完全可以滿足要求。電池（7.2v）2000mAhNi-cd電池（7.2v）2000mAhNi-cd穩(wěn)壓電路 穩(wěn)壓電路5V5V6V7.2V6V7.2V電機單片機舵機測速板Encoder電機單片機舵機測速板Encoder對管圖4.1系統(tǒng)電壓調(diào)節(jié)圖 圖4.1系統(tǒng)電壓調(diào)節(jié)圖圖4.27805電路圖圖4.3電源模塊示意圖4.2電機驅(qū)動電路電機驅(qū)動使用飛思卡爾專用電機驅(qū)動芯片MC33886。驅(qū)動電路如圖4.4所示。為了增大驅(qū)動能力，減少單片發(fā)熱量，電路采用兩片MC33886并聯(lián)的方案。系統(tǒng)使用PWM控制電機轉速，充分利用單片機的PWM模塊資源。電機PWM頻率設定為8KHz。MC33886芯片的工作電壓為5-40V，導通電阻為140毫歐姆，PWM頻率小于10KHz，具有短路保護、欠壓保護、過溫保護等功能。電機驅(qū)動芯片安裝在制作的電機驅(qū)動PCB板上，在PCB板設計時，考慮到芯片散熱問題，在芯片腹部設計了方型的通孔，實際運行效果表明芯片散熱均勻，設計合理。為了防止電動機突然停止時產(chǎn)生的電磁干擾，在電動機的兩端焊接了一個0.1μF濾波電容。圖4.4兩片MC33886并聯(lián)使用圖4.5兩片MC33886并聯(lián)使用的實物圖在圖中可以看到，我們使用PWM23和PWM45作為電機驅(qū)動PWM信號，兩個PWM通道級聯(lián)可以使其輸出更加精確。在程序中，我們把PWM值直接轉換成了以米/秒為單位的絕對速度，這樣使智能車的速度更加直觀切易于調(diào)試。4.3測速電路由于考慮到成本需要，我們采用了紅外對管和黑白碼盤作為測速模塊的硬件構成。其中碼盤為32格的黑白相間圓盤，如下圖所示：圖4.5碼盤紅外傳感器安裝在正對碼盤的前方，雖然這樣做精度比編碼器要低很多，但是成本低廉制作容易，如果智能車速度較快，可以考慮再減少碼盤上黑白色條的數(shù)量即可。當圓盤隨著齒輪轉動時，光電管接收到的反射光強弱交替變化，由此可以得到一系列高低電脈沖。設置9S12的ECT模塊，同時捕捉光電管輸出的電脈沖的上升沿和下降沿。通過累計一定時間內(nèi)的脈沖數(shù)，或者記錄相鄰脈沖的間隔時間，可以得到和速度等價的參數(shù)值。測速電路使用自行研制的紅外反射式光電測速傳感器。速度測量電路使用紅外反射式光電對管RPR220，自行制作的編碼盤，比較電路等組成。圖4.6測速電路速度測量電路圖2.8所示。紅外反射式光電對管的光敏三極管信號通過比較器處理后輸入單片機的計數(shù)器模塊，利用單片機的輸入捕捉功能，處理智能車速度信息。自制的編碼盤有24道黑色條紋，電機旋轉一周將產(chǎn)生24次輸入捕捉中斷。圖4.6測速電路單片機記錄兩次中斷的時間間隔T。兩次中斷對應于智能車前進的距離S為：16.5/24cm,即0.6875cm，其中16.5cm為智能車后輪實測周長[7]。智能車實時速度V(cm/s)的計算公式如下： 4.4紅外對管檢測電路由于我們采用了大功率對管，所以紅外對管的電路是整個電路中要求最高的，不緊要保證對管正常工作，而且還要考慮整個電路的能耗和發(fā)熱問題。經(jīng)測試我們發(fā)現(xiàn)單個對管在通以100mA到170mA電流時可以。保證抬高20到30厘米的距離。此時每個對管的管壓降為1.2到1.5伏。為了進一步加大發(fā)光量，我們采用了雙發(fā)射管的辦法，即一個接受管對應兩個發(fā)射管。為了降低整體的能耗。我們讓同一對的發(fā)射管串聯(lián)，14對發(fā)射管再并聯(lián)。同時使用了irf540進行開關控制?？刂茖苊}沖發(fā)光。開關頻率為200HZ。這樣既保證了大前瞻探測的需要，又降低了整體的能耗和對電源的沖擊圖4.7先串聯(lián)再并聯(lián)的脈沖發(fā)光對管電路圖。圖4.8對管實物圖反面圖4.9對管實物圖正面4.5撥碼開關電路由于在智能車比賽開始后，不能夠?qū)χ悄苘囉布败浖M行修改，在保證了硬件有效可靠的同時，軟件有可能不能夠適應新場地，所以設計撥碼開關對智能車有關參數(shù)進行設置也是必要的。撥碼開關電路如下圖所示：圖4.9撥碼開關這是一個八段的撥碼開關，我們把它成成上下連個部分，顯然，每個部分都有16種狀態(tài)，前四個來改變舵機參數(shù)，后四個改變直流電機參數(shù)，這樣對于適應新的場地很有好處。第五章智能車的控制策略和算法5.1路徑搜索算法對于本控制系統(tǒng)采用14對光電對管的方案，單排排列在車體頭部10cm處。編號為6、7的光電對管處于正中央位置。利用14對傳感器進行道路識別。傳感器對白色的反射率比黑色的大。單片機ADC讀入值相應也大。在程序中對傳感器信號進行處理，判斷傳感器是否檢測到黑色引導線。將單個傳感器對白色和黑色路面的ADC值之差分為平均的兩段，每次處理實時傳感器信號時，判斷本次采樣的ADC值與黑色路面ADC值之差落在兩段中的哪一段。如果在靠近黑色的一段，則判定該傳感器檢測到黑線，將該傳感器對應的變量置為判定值1；如果在靠近白色的一段，則判定該傳感器檢測到白線，將該傳感器對應的變量置為判定值1；為了增強判斷的準確性，在對ADC值采樣時，采用了中值濾波方法，以去除瞬間的干擾。路徑檢測完后，將測的的路徑值暫時存儲，然后將路徑信息傳遞給舵機和電機控制部分，以選擇給定合適的轉角和速度。5.2舵機、電機的控制智能車的舵機和電機都采用了經(jīng)典的PID控制方法。但是由于舵機和電機性能的不同要求，分別對其進行了不同的修改。PID控制器由比例單元（P）、積分單元（I）和微分單元（D）組成。其輸入e(t)與輸出u(t)的關系為

式中積分的上下限分別是0和t

因此它的傳遞函數(shù)為：G(s)=U(s)/E(s)=kp(1+1/(TI*s)+TD*s)

其中kp為比例系數(shù)；TI為積分時間常數(shù)；TD為微分時間常數(shù)比例KP用來控制當前，誤差值和一個負常數(shù)P（表示比例）相乘，然后和預定的值相加。P只是在控制器的輸出和系統(tǒng)的誤差成比例的時候成立，KP能夠快速的跟隨變化量。及時的產(chǎn)生與之相關的調(diào)節(jié)作用。但是KP是有差調(diào)節(jié)，無法消除靜態(tài)誤差。積分KI來控制過去，誤差值是過去一段時間的誤差和，然后乘以一個負常數(shù)I，然后和預定值相加。I從過去的平均誤差值來找到系統(tǒng)的輸出結果和預定值的平均誤差。一個簡單的比例系統(tǒng)會振蕩，會在預定值的附近來回變化，因為系統(tǒng)無法消除多余的糾正。通過加上一個負的平均誤差比例值，平均的系統(tǒng)誤差值就會總是減少。所以，最終這個PID回路系統(tǒng)會在預定值定下來。微分KD來控制將來,計算誤差的一階導，并和一個負常數(shù)D相乘，最后和預定值相加。這個導數(shù)的控制會對系統(tǒng)的改變作出反應。導數(shù)的結果越大，那么控制系統(tǒng)就對輸出結果作出更快速的反應。這個D參數(shù)也是PID被成為可預測的控制器的原因。D參數(shù)對減少控制器短期的改變很有幫助。一些實際中的速度緩慢的系統(tǒng)可以不需要D參數(shù)。舵機PID由于舵機是一個具有大的延遲的執(zhí)行機構，所以在PID控制中不能加入積分環(huán)節(jié)。否則會導致小車震蕩。所以小車采用PD控制。同時加入一個一階慣性環(huán)節(jié)，構成不完全微分，給小車一個超前的調(diào)節(jié)。實際使用中，為了減少計算時間，將位置式PID轉化為增量式增量式PID公式：電機PID控制小車行使過程中，隨著跑道的不同，需要配合不同的速度值，因此對電機的PID是一個給定值不斷變化的PID。小車的目標速度（Object_Speed）給定規(guī)則：小車在直道上，Object_Speed為最大值200。小車在大彎道上，Object_Speed為160。小車在小彎道或S型彎道上，Object_Speed為120小車沖出跑道，Object_Speed為70。小車由直道進入彎道，Object_Speed逐漸減小。小車由彎道進入直道，Object_Speed逐漸加大。在實驗中發(fā)現(xiàn)，PID的超調(diào)量主要在第一個波形中起作用，也即單速度由很大到很小的時候，或由低速突然加到高速的過程中，會出現(xiàn)很大的超調(diào)。但是這個超調(diào)并不是有害的，因為，當速度要求突變的時候，往往是小車由直道入彎道，或者由彎道入直道的過程，這個過程往往需要很快的大加減速，而由于小車的慣性，一般的PID調(diào)節(jié)難以滿足要求，這時使用大的超調(diào)量可以使小車有一個加速或剎車的過程，使之更好的達到要求速度。第六章運行調(diào)試部分6.1Codewarrior開發(fā)環(huán)境在整個開發(fā)調(diào)試過程中，使用Metrowerks公司為MC9S12系列專門提供的全套開發(fā)工具（FreescaleCodewarriorIDE4.6）。這是一套用C語言進行編程的集成開發(fā)環(huán)境——本文智能車定位系統(tǒng)的軟件設計部分就是在此開發(fā)環(huán)境下完成的。Codewarrior是由Metrowerks公司提供的專門面向Freescale所有MCU與DSP嵌入式應用開發(fā)的軟件工具。其中包括集成開發(fā)環(huán)境IDE、處理器專家、全芯片仿真、可視化參數(shù)顯示工具、項目工程管理、C交叉編譯器、匯編器、鏈接器以及調(diào)試器。CodeWarriorIDE能夠自動地檢查代碼中的明顯錯誤，它通過一個集成的調(diào)試器和編輯器來掃描你的代碼，以找到并減少明顯的錯誤，然后編譯并鏈接程序以便計算機能夠理解并執(zhí)行你的程序。每個應用程序都經(jīng)過了使用象CodeWorrior這樣的開發(fā)工具進行編碼、編譯、編輯、鏈接和調(diào)試的過程。MetrowerksCodewarriorIDE中的mc9s12dg128.h文件對所有寄存器對應的存儲映射地址都進行了宏定義，開發(fā)者在軟件開發(fā)時直接調(diào)用這些宏就可以了。6.2軟件仿真為了更好的定量分析影響小車行駛的各個因素，而且最大限度的節(jié)約時間和成本。我們采用了軟件仿真和實際調(diào)試相結合的辦法。仿真軟件使用了清華的PLAST2。通過仿真，我們發(fā)現(xiàn)：小車傳感器的探測距離對速度有著決定的影響。所以傳感器應該盡量的探測更遠。但是傳感器的探測距離不能超過最小轉彎的半徑。否則會出現(xiàn)盲區(qū)。適度增加舵機的靈敏度，可以使轉彎更加靈活。所以我們在實際調(diào)試中，加長了舵機的力臂。6.2實際調(diào)試實際調(diào)試過程中，我們發(fā)現(xiàn)小車在直道上會出現(xiàn)左右抖動的現(xiàn)象，通過軟件設置死區(qū)或其他處理方法，效果都不是很明顯，最后發(fā)現(xiàn)小車舵機和前輪的間隙是罪魁禍首，通過把前輪設置為內(nèi)八形，完美的解決了這個問題。在調(diào)試過程中，我們加了液晶顯示，還設置了蜂鳴器，這些輔助設備在比賽中為了減輕小車的重量，都是不需要的。但是在調(diào)試過程中，通過這些設備，可以及時的了解小車運行的情況，達到事倍功半的效果。第七章技術參數(shù)統(tǒng)計表7.1模型車技術參數(shù)統(tǒng)計：項目參數(shù)路徑檢測方法（賽題組）光電組車模幾何尺寸（長、寬、高）（毫米）385*220*60車模軸距/輪距（毫米）200/150車模平均電流（勻速行駛）(毫安)200電路電容總量（微法）430傳感器種類及個數(shù)紅外對管15個新增加伺服電機個數(shù)0賽道信息檢測空間精度（毫米）9賽道信息檢測頻率（次/秒）200主要集成電路種類/數(shù)量9s12單片機最小系統(tǒng)/133886電機驅(qū)動電路/4速度檢測電路/1車模重量（帶有電池）（千克）0.9參考文獻[1]黃開勝、金華民、蔣狄南，韓國智能模型車技術方案分析，北京：清華大學汽車安全與節(jié)能國家重點實驗室，2004.3[2]邵貝貝著，單片機嵌入式應用的在線開發(fā)方法，北京：清華大學出版社，2004.2[3]‘LM2940datasheet’,July2000,NationalSemiconductor[4]‘LM7806datasheet’NationalSemiconductor[5]‘RPR220datasheet’,ROHM[6]'SemiconductorTechnicalDataMC33887'Aug2002MotorolaInc[7]大賽車模拼裝手冊[8]CodeWarriorIDE3.1helpdatasheet[9]'MC9S12DG128DeviceUserGuide',October2002MotorolaInc[10]'HCS12COREdatasheet',Augt2000MotorolaInc[11]'S12PWM8B8CV1datasheet',Mar2002MotorolaInc[12]'S12ATD10B8CV2datasheet',Augt2002MotorolaInc[13]'S12ECT16B8V1datasheet',July2002MotorolaInc[14][15][16]附件程序電機PID控制程序typedefunsignedcharBOOL;typedefunsignedcharINT8U;//無符號8位數(shù)typedefsignedcharINT8S;//有符號8位數(shù)typedefunsignedintINT16U;//無符號16位數(shù)typedefsignedintINT16S;//有符號16位數(shù)typedefunsignedlongINT32U;//無符號32位數(shù)typedefsignedlongINT32S;//有符號32位數(shù)typedeffloatFP32;//單精度浮點數(shù)typedefdoubleFP64;//雙精度浮點數(shù)#defineMAX_32(signedlong)0x7fffffffL#defineMIN_32(signedlong)0x80000000L#defineMAX_16(signedint)0x7fff#defineMIN_16(signedint)0x8000typedefstruct{signedintProportionalGain;signedintProportionalGainScale;signedintIntegralGain;signedintIntegralGainScale;signedintDerivativeGain;signedintDerivativeGainScale;signedintPositivePIDLimit;signedintNegativePIDLimit;signedintIntegralPortionK_1;signedintInputErrorK_1;}sCaiXinBoPID;sCaiXinBoPIDSpdPID;externsignedintCaiXinBoPIDController(signedintDesiredValue,signedintMeasuredValue,sCaiXinBoPID*pParams);staticsignedlongL_sub(registersignedlongsrc_dst,registersignedlongsrc2){ return(src2-src_dst);}staticsignedlongL_deposit_l(registersignedintssrc){ return(signedlong)(ssrc);}staticsignedintextract_l(registersignedlonglsrc){ return(signedint)lsrc;}staticsignedlongL_mult(registersignedintsinp1,registersignedintsinp2){ registersignedlonglaccum; laccum=sinp1; laccum*=sinp2; returnlaccum;}staticsignedlongL_add(registersignedlongsrc_dst,registersignedlongsrc2){ return(src_dst+src2);}signedintCaiXinBoPIDController(signedintDesiredValue,signedintMeasuredValue,sCaiXinBoPID*pParams){signedlongProportionalPortion,IntegralPortion,PIDoutput;signedintInputError;/**//*Saturationmodemustbeset*//*InputError=sub(DesiredValue,MeasuredValue);*//*inputerror*//**//*inputerrorcalculation-16bitrange,withandwithoutsaturationmode*/PIDoutput=L_sub(L_deposit_l(DesiredValue),L_deposit_l(MeasuredValue));/*inputerror-32bitrange*/if(PIDoutput>MAX_16)/*inpurerrorisgreaterthan0x00007fff=32767-32bitrange*/InputError=MAX_16;/*inputerror=max.positive16bitsignedvalue*/elseif(PIDoutput<MIN_16)/*inputerrorislessthan0xffff7fff=-32768-32bitrange*/InputError=MIN_16;/*inputerror=min.negative16bitsignedvalue*/elseInputError=extract_l(PIDoutput);/*inputerror-16bitrange*//**//*proportionalportioncalculation*/ProportionalPortion=L_mult((pParams->ProportionalGain),InputError)>>(pParams->ProportionalGainScale+1);/**//*integralportioncalculation*/IntegralPortion=L_mult((pParams->IntegralGain),InputError)>>(pParams->IntegralGainScale+1);/*integralportioninstepk+integralportioninstepk-1*/IntegralPortion=L_add(IntegralPortion,L_deposit_l(pParams->IntegralPortionK_1));/*integralportionlimitation*/if(IntegralPortion>(pParams->PositivePIDLimit))(pParams->IntegralPortionK_1)=(pParams->PositivePIDLimit);elseif(IntegralPortion<pParams->NegativePIDLimit)pParams->IntegralPortionK_1=pParams->NegativePIDLimit;elsepParams->IntegralPortionK_1=extract_l(IntegralPortion);/**//*derivativeportioncalculation*/PIDoutput=L_sub(L_deposit_l(InputError),L_deposit_l(pParams->InputErrorK_1));/*[e(k)-e(k-1)]-32bitrange*/pParams->InputErrorK_1=InputError;/*e(k-1)=e(k)*/if(PIDoutput>MAX_16)/*[e(k)-e(k-1)]isgreaterthan0x00007fff=32767-32bitrange*/InputError=MAX_16;/*[e(k)-e(k-1)]=max.positive16bitsignedvalue-16bitrange*/elseif(PIDoutput<MIN_16)/*[e(k)-e(k-1)]islessthan0xffff7fff=-32768-32bitrange*/InputError=MIN_16;/*[e(k)-e(k-1)]=min.negative16bitsignedvalue-16bitrange*/elseInputError=extract_l(PIDoutput);/*[e(k)-e(k-1)]-16bitrange*//*drivativeportioninstepk-integer*/PIDoutput=L_mult((pParams->DerivativeGain),InputError)>>(pParams->DerivativeGainScale+1);/**//*controlleroutputcalculation*/PIDoutput=L_add(PIDoutput,ProportionalPortion);/*derivativeportion+proportionalportion*/PIDoutput=L_add(PIDoutput,L_deposit_l(pParams->IntegralPortionK_1));/*+integralportion=controlleroutput*//*controlleroutputlimitation*/if(PIDoutput>pParams->PositivePIDLimit)PIDoutput=pParams->PositivePIDLimit;elseif(PIDoutput<pParams->NegativePIDLimit)PIDoutput=pParams->NegativePIDLimit;、初始化程序voidRTI_Init(void){/*setupoftheRTIinterruptfrequency*//*adjustedtoget1millisecond(1.024ms)with16MHzoscillator*/RTICTL=0x1f;//5*2^16:48.8hz//0x1f//setRTIprescaler::晶振16384分頻;(低四位+1)*(2^(高三位+9))CRGINT=0x80;//enableRTIinterrupts;//低四位:1-7,不可為零,否則分頻器不工作}voidSET_PLL(void){CLKSEL=0x00;PLLCTL=0xe1;SYNR=2;REFDV=1;PLLCTL=0x60;asmNOP;asmNOP;asmNOP;while((CRGFLG&0x08)==0);CLKSEL=0x80;}/*鎖相環(huán)程序*/voidATD_Init(void){ATD0CTL2=0xc0;//不開中斷2;////上電，標志位快速清零 ... ATD隊列轉換完成中斷使能ATD0CTL3=0x00;//sequencelength:80; //轉換隊列長度8，非FIFO存儲模式，ATD0CTL4=0x85;//8位精度，AD時鐘為bus_clk/(2*(5+1))ATD0CTL5=0x90;//右對齊，無符號數(shù)，單次轉換隊列模式，ch1begin,6channels;多通道8路，從通道0開始ATD1CTL2=0xc0;//ADPU,fastflagclearATD1CTL3=0x30;//sequencelength:6,notfifomode,ATD1CTL4=0x85;//8bitdecision,ATDclock:bus_clk/(2*(5+1))ATD0CTL5=0x90;//右對齊，無符號數(shù)，單次轉換隊列模式，ch1begin,6channels;多通道8路，從通道0開始}/*ATD初始化程序*/voidServo_Init(void)//舵機16位PWM控制{PWME_PWME1=0;//disablePWM1servoPWMPRCLK|=0x03;//CLKA:8分頻PWMCLK|=0x00;//這就是默認值//通道1用clockA時鐘源//24MHZ/8=3MPWMPOL_PPOL1=1;//先高電平PWMCAE=0x00; //對齊方式默認左對齊PWMCTL_CON01=1;//通道1:16BIT連接PWMPER01=30000;//30000:10ms;;;30000//舵機的周期是10ms 1/3M*x=1/100//1ms脈沖到2ms脈沖//1.5ms脈沖對應0度PWMDTY01=4500;//4500:1.5ms; //控制3500左極限5500右極限4500正中央PWMCNT01=0; //寫計數(shù)寄存器,會使PWMDTYx,PWMPERx進入鎖存器,同時使能通道時會從此值PWME_PWME1=1;//PWM通道1輸出}/*舵機初始化程序*/voidForward_Init(void)//假定前進方向，初始化函數(shù){ //PWM3:IN1//PWM5:IN2 =1//PWM7:EN//DDRP=0xff;//控制輸出PWME_PWME3=0;PWME_PWME5=0;PWMPRCLK|=0x20;//clockb總線頻率的4分頻 //通道3用clockB時鐘源//24MHZ/4=6MPWMPOL_PPOL5=0;//lowelectricallevelfirst,,highelectricallevelfirst,buthighlevelmakesmc33886disabled //對齊方式默認左對齊PWMCAEPWMCTL_CON45=1;//16BIT連接PWMPER45=750;//電機頻率8kHz 1/1.5M*x=1/2KHz PWMDTY45=500; //占空比精度1/750PWMCNT45=0; //啟動PWMPWME_PWME5=1;//PWM通道3輸出}/*電機初始化程序*/voidECT_Init(void)//ECT初始化，使用輸入捕捉功能{TSCR1_TFFCA=1;//automaticallyclearflagTSCR2=0x87;//TimerOverflowinterruptopen,TCNTprescaler:24MHZ/128=187.5kHz計數(shù)頻率TIOS_IOS0=0;//Thecorrespondingchannelactsasaninputcapture//ONCH0PT0;TCTL4_EDG0A=1;//captureonrisingedgeonlyonchannel0TCTL4_EDG0B=0;TIE_C0I=1;//EnableCh1InterruptsTSCR1_TEN=1;//ENABLETIMER}主程序#include<hidef.h>/*commondefinesandmacros*/#include<mc9s12dg128.h>/*derivativeinformation*/#pragmaLINK_INFODERIVATIVE"mc9s12dg128b"#include"pid.h"#include"init.h"#defineAD_LEVEL130#defineMID4785#defineMAXTURN750#defineDC_Limit600#defineKP1(-40)#defineKI10#defineKD1(0)#defineSPEED70//變量定義inttimes,exit,exit1,exit_flag;//停車用變量charzanshi=0;//調(diào)速intSensor=4785;//舵機intave_weight[3]={0,0,0};intcount0=0;intAD_Value[14];inti;intatd_value[14];charboma=0;//撥碼開關intzhidao_speed=0;intwangdao_speed=0;intchongchu_speed=0;//速度設定intcha0,cha1;//不完全微分volatileunsignedintIC0Counter=0; //2.083msoneintvolatileunsignedintIC0Counter_1=0; volatileunsignedintstop_flag=0;volatileintNowSpeed;volatileunsignedintGearWidth[4]={0};volatileunsignedintGearIndex=0;unsignedlongGearWidthSum;unsignedlongAveGearWidth;unsignedlongAveSpeed;//測速unsignedintpulse_counter=0;//計數(shù)intDCDuty;intObject_Speed=SPEED; //100cm/sintstop_stop=0xff,stop_stop1=0;/******************轉彎PID函數(shù)********************/intpid1(inta,intb,intc){intx=0;x=(KP1*(a-b)+KI1*a+KD1*(a-2*b+c));return(x);}/*********************主函數(shù)*********************/voidmain(void){SpdPID.ProportionalGain=30000;//1.520.150.050.94 1.8424903SpdPID.ProportionalGainScale=13;//0.76的系數(shù)(0-1):GainScale=14//(0-2):gainscale=13 //(0-4)0-80-160-320-640-1280-256.. //.0-32768--不能取最后的那個數(shù)，可以有小數(shù)，隨著數(shù)的增大，小數(shù)數(shù)減少，從15位到0位//精度1/327681 SpdPID.IntegralGain=25000;//20480;15000SpdPID.IntegralGainScale=14;//14;//0.625的系數(shù) 1.251.45(pianda)0.925 0.450.150.85SpdPID.DerivativeGain=20000;//0SpdPID.DerivativeGainScale=14;SpdPID.PositivePIDLimit=DC_Limit;SpdPID.NegativePIDLimit=0;SpdPID.IntegralPortionK_1=0;//=0SpdPID.InputErrorK_1=0;//=0RTI_Init();SET_PLL();ATD_Init();Servo_Init();Forward_Init();ECT_Init();DDRT=0x00;boma=PTIT;//讀入撥碼開關；boma>>=4;boma&=0b00001111;zhidao_speed=90+7*boma;wangdao_speed=75+4*boma;chongchu_speed=50;//60+boma;EnableInterrupts;for(;;){/////////////////////調(diào)速/////////////////////if(abs(ave_weight[2])<3){Object_Speed=zhidao_speed;}elseif(abs(ave_weight[2])<14)Object_Speed=wangdao_speed;elseObject_Speed=chongchu_speed;/////////////////////轉彎/////////////////////cha1=pid1(ave_weight[2],ave_weight[1],ave_weight[0]);cha1=(cha1*100+cha0*0)/100;cha0=cha1;Sensor=4785-38*ave_weight[2];//pid1(ave_weight[2],ave_weight[1],ave_weight[0]);if(Sensor>MID+MAXTURN){Sensor=MID+MAXTURN;}if(Sensor<MID-MAXTURN){Sensor=MID-MAXTURN;}PWMDTY01=Sensor;ave_weight[0]=ave_weight[1];ave_weight[1]=ave_weight[2];count0=0;}}#pragmaCODE_SEGNON_BANKED/**********************定時1ms中斷************************/voidinterrupt7RTI_int(){inti,j,k,l,m;staticintpulse_t;CRGFLG|=0x80;pulse_t++;if(pulse_t>5){pulse_t=0;DDRA=0xff;PORTA_BIT0=1;for(i=0;i<1500;i++);ATD0CTL5=0x90;//啟動一個新的轉換,ch0beginATD1CTL5=0x90;while(!(ATD0STAT0&0x80));//等待隊列轉換結束AD_Value[0]=ATD0DR0L;//ATD0DR0L;//讀取相應結果寄存器值清相應轉換完成通道標志ATD0STAT1AD_Value[1]=ATD0DR1L;AD_Value[2]=ATD0DR2L;AD_Value[3]=ATD0DR3L; AD_Value[4]=ATD0DR4L;AD_Value[5]=ATD0DR5L;AD_Value[6]=ATD0DR6L;AD_Value[7]=ATD0DR7L;//beginATD1,ch0beginwhile(!(ATD1STAT0&0x80));//等待ATD1隊列轉換結束AD_Value[8]=ATD1DR0L;AD_Value[9]=ATD1DR1L;AD_Value[10]=ATD1DR2L;AD_Value[11]=ATD1DR3L;AD_Value[12]=ATD1DR4L;AD_Value[13]=ATD1DR5L;//ATD1DR5L; PORTA_BIT0=0;for(i=0;i<14;i++){if(AD_Value[i]>AD_LEVEL){atd_value[i]=1;count0++;//檢測到的個數(shù)}elseatd_value[i]=0;}if(count0==0){exit1=3000;ave_weight[2]=ave_weight[1];}elseif(count0>6&&count0<14){}else{exit1=3000;ave_weight[2]=((atd_value[0]*(-13)+atd_value[1]*(-11)+atd_value[2]*(-9)+atd_value[3]*(-7)+atd_value[4]*(-5)+atd_value[5]*(-3)+atd_value[6]*(-1)+atd_value[7]*(1)+atd_value[8]*(3)+atd_value[9]*(5)+atd_value[10]*7+atd_value[11]*9+atd_value[12]*11+atd_value[13]*13)/count0);}}for(i=0;i<3;i++){