第六屆全國大學生智能汽車邀請賽PAGEXVIIIPAGEXIX第六屆“飛思卡爾”杯全國大學生智能汽車競賽技術報告 學校：山東大學威海分校隊伍名稱：憤怒的小車參賽隊員：杜濤閆揚閻曉寧帶隊教師：王小利鄭亞民

關于技術報告和研究論文使用授權的說明 本人完全了解第一屆“飛思卡爾”杯全國大學生智能汽車邀請賽關保留、使用技術報告和研究論文的規定，即：參賽作品著作權歸參賽者本人，比賽組委會和飛思卡爾半導體公司可以在相關主頁上收錄并公開參賽作品的設計方案、技術報告以及參賽模型車的視頻、圖像資料，并將相關內容編纂收錄在組委會出版論文集中。參賽隊員簽名：帶隊教師簽名：日期：第六屆全國大學生智能汽車邀請賽目錄TOC\o"1-1"\h\z\u引言 2第一章緒論 31.1智能車競賽背景 31.2智能車比賽意義 31.3本文的主要內容及安排 4第二章整體方案設計 52.1系統總體方案的選定 52.2設計構思 6第三章智能車機械結構調整 93.1車體重心的調整 93.2智能車前輪參數調整 103.3舵機安裝 133.4齒輪傳送機構的調整 13第四章智能車硬件部分 154.1路經檢測傳感器設計 154.2速度檢測傳感器設計 174.3起始線檢測模塊設計 194.4MCU控制模塊 204.5驅動模塊設計 224.6電源模塊設計 23第五章智能車軟件部分 245.1MC9S12XS128片內資源簡介 255.2智能車控制系統總體方案 265.3整體程序流程圖 275.4路徑識別模塊軟件設計 285.5智能車舵機控制算法 285.6智能車電機控制算法 29第六章系統調試 326.1開發工具及系統調試 336.2在線調試 33第七章賽車主要技術參數 35第八章總結 36參考文獻 36附錄 I

引言該智能尋跡小車以MC9S12單片機最小系統為核心，輔以電源模塊、傳感器模塊、舵機控制模塊、電機控制模塊、速度控制模塊和運行調試模塊，通過感應由賽道中心電線產生的交變磁場進行路經檢測。采用Q值高、靈敏度好、易于購買的工字電感感應電磁場信號，通過信號調理電路濾波放大后輸入單片機AD口，單片機通過轉向控制策略與增量式PID算法驅動電機速度與舵機轉角，實現路徑的檢測與識別。同時通過調整小車的重心、前輪機械結構參數，使小車的制動性、穩定性與行進速度達到最優。系統屬于以能量轉換為主，由直流電機輸入能量與電機，通過小車不同的運動輸出能量，是典型的機電一體化系統。這份技術報告中，我們小組通過對整體方案、硬件電路、軟件算法、機械結構、調試參數等方面進行介紹，詳盡地闡述了我們的思想和創意，具體表現了我們在電路的創新設計，以及算法方面的獨特想法。這份報告凝聚著我們的心血和智慧，是我們共同努力后的成果。關鍵詞：智能汽車MC9S12電磁

第一章緒論1.1智能車競賽背景汽車作為現代的交通工具，為社會的發展和進步做出了很大的貢獻。現階段，汽車工業的發展水平和汽車的持有量已經成為衡量一個國家工業發達程度的標志。二十一世紀，汽車研究的主要方向是智能化汽車。專家們普遍認為，新一輪汽車產業競爭的焦點，將是基于信息技術、微電子技術、計算機技術、智能自動化技術、人工智能技術、網絡技術、通信技術等的智能汽車的研究設計開發。大學生智能模型車競賽是在飛思卡爾半導體公司資助下舉辦的以單片機為核心的大學生課外科技競賽。大賽要求使用組委會統一提供的車模，采用飛思卡爾16位單片機MC9S12DG128作為核心控制單元，參賽隊伍要制作一個能夠自主識別路線的智能車，在專門設計的跑道上自動識別道路行駛，跑完整個賽道用時最短，而且技術報告評分較高的參賽隊就是獲勝者。2006年，我國舉辦了第一屆智能車競賽。首屆比賽采用MC9S12DG128作為主控芯片，相比于MC9S12DP256有256K的程序存儲空間，MC9S12DG128只有128K程序存儲空間。賽車模型、舵機和驅動電機與韓國2005年漢陽大學比賽時幾乎相同。首屆智能車競賽共有來自全國57所高校的112支參賽隊參加。隨著賽事的逐年開展，競賽由原先前四屆比賽分為攝像頭組和光電組外，第五屆比賽新增了電磁組，擴大創意組規模；賽道變窄，提高小車控制難度。1.2智能車比賽意義全國智能車大賽也就是初步模擬了一個人駕駛汽車的思維和控制過程，智能車比賽就是要求參賽人員能夠去進行這樣的研究，專業知識涉及控制、模式識別、傳感技術、汽車電子、電氣、計算機、機械等多個學科，以實現讓車模能夠自動尋跡并自動控制運動，最終實現小車智能化。該競賽綜合性很強，在提高參賽大學生的動手能力和創新能力的同時，也在一定程度上推動了智能汽車產業的發展與進步。1.3本文的主要內容及安排本文首先介紹了研究背景、比賽規則和設計構思。闡述了控制系統的資源配置、資源需求與分配和核心處理器的寄存器——MC9S12單片機寄存器資源。相比于其它類型的單片機，16位的MC9S12的功能更加強大，引腳較多，能夠很好地滿足智能車控制系統的需要。通過對智能模型車尋跡方案分析，我們確定使用電磁傳感器作為識別路線的手段進行線路尋跡。單片機內部的A/D轉換器把采集到的傳感器數據轉換為離散的數字量，用PID算法控制舵機、電機輸出，在保證穩定性的前提下，盡量提高模型車行駛的速度。速度檢測模塊實現速度的閉環控制，提高模型車運行的穩定性。本文的第一章為緒論。第二章是介紹了整體方案的設計，簡要介紹了針對特定路線的控制思想和策略。第三章是智能車機械結構的調整，包括舵機的安裝調整、車體重心的調整、前輪定位的調整、齒輪傳動機構的調整、后輪差速機構的調整。第四章是智能車系統的硬件設計，包括路徑檢測傳感器的選用、速度檢測傳感器的設計、MCU控制模塊的設計、驅動模塊的設計、電源模塊的設計、顯示模塊的設計。第五章分析了軟件系統的設計，包括傳感器采集、歸一化和線性化擬合和整體路徑識別策略分析及控制算法的介紹。第六章是對系統調試的介紹。第七章是總結。

第二章整體方案設計本章簡要地介紹了智能車系統總體方案的選定和總體設計思路，在后面的章節中將整個系統分為機械結構、控制模塊、控制算法三部分對智能車控制系統進行深入的介紹分析。2.1系統總體方案的選定本設計是一個典型的機電一體化系統。機電一體化又稱機械電子學，字面上表示機械學與電子學兩學科的綜合。其特征是給“機械”增添了頭腦（計算機信息處理與控制），因此是一個要求傳感器技術、控制用接口元件、機械結構、控制軟件水平較高的系統。機電一體化系統由機械系統(機構)、電子信息處理系統(計算機)、動力系統(動力源)、傳感檢測系統(傳感器)、執行元件系統五個子系統組成。圖2.SEQ圖2.\*ARABIC1機電一體化結構圖智能模型車是典型的機電一體化結構，單片機通過傳感器采集路面信息，通過算法控制電機、舵機運行，以達到尋跡的目的。系統屬于以能量轉換為主，輸入能量和信息，輸出不同形式能量的系統。其中由直流電機輸入能量，輸出的能量是小車的運動。2.2設計構思制作智能車，需要參賽隊伍學習和應用嵌入式軟件開發工具軟件和在線開發手段，以飛思卡爾16位單片機MC9S12XS128作為核心控制單元，自行設計和制作可以自動識別路徑的方案、電機的驅動電路、模型車的車速傳感電路、模型車轉向伺服電機的驅動以及微控制器控制軟件的編程等。系統結構簡圖見圖2.2，圖中箭頭方向表示信號傳遞方向。圖2.SEQ圖2.\*ARABIC2系統結構簡圖2.2.1單片機控制模塊系統核心為單片機最小系統，是數據采集和輸出控制的最重要的部分。單片機最小系統以MC9S12XS128單片機為核心，主要包括時鐘、旁路電容、電源接口、燒錄和調試接口、I/O接口等。通過豐富的I/O接口和內部強大的數據處理能力，實現信號采集和系統控制。2.2.2電源模塊整個設計中，電源采用7.2V電壓、2000mAh電池容量、Ni-cd鎳鎘蓄電池作為系統能源。電源模塊是系統的能量來源，它分別向電機、舵機、傳感器模塊和單片機模塊提供獨立的電源，防止相互之間的電源干擾。第六屆全國大學生智能汽車邀請賽第三章智能車機械結構調整第二章整體方案選定2.2.3傳感器模塊傳感器模塊包括路徑傳感器、速度傳感器和起點檢測傳感器。路徑傳感器以電磁傳感器陣列為核心，用安裝在智能車車頭支架上的電磁感應線圈，檢測賽道黑線下通有20kHz交流電流的導線所產生磁場的磁感應強度和方向，通過信號選頻放大檢波后，送入單片機進行AD轉換。單片機對采集的信息進行處理，通過算法優化和PID控制運行調試。速度檢測傳感器是電機閉環控制中的重要部分，在傳感器選型上，充分考慮了傳感器的精度、控制延時、體積和重量等方面。在滿足系統要求的基礎上，以簡化結構為主要條件，選擇合適的速度檢測傳感器，并設計了相應的硬件電路。起點傳感器為一組干簧管組成的陣列，利用干簧管在磁場中通電導通的性質，通過檢測安放在起點處的強力磁鐵的磁性而判斷當前車輛所跑過的圈數，同時判斷比賽是否完成。2.2.4舵機控制模塊舵機是模型車轉向的控制對象，舵機控制模塊利用轉向控制策略，判斷車體與漆包線的位置關系，改變輸出轉角實現模型車車輪的轉向，以控制智能車前進方向。2.2.5電機控制模塊電機是模型車的動力源。直流電機驅動模塊接收速度控制信號，驅動芯片采用由MOS管搭建的H橋作為驅動，利用速度控制策略，控制電機正反轉，以控制智能車前進與后退。2.2.6速度測量模塊速度測量模塊包括無線串口模塊、LED顯示等，為模型車參數確定提供試驗條件。速度檢測模塊通過光電編碼器實時測量智能車車速，用于系統車速的閉環控制，以精確控制車速。2.2.7運行調試模塊運行調試模塊采用Metroworks公司的CodeWarriorIDE軟件集成運行環境作為開發工具對飛思卡爾的MC9S12XS微控制器進行軟件開發。并且通過CodeWarriorIDE的在線調試工具，對傳感器不同排布的特性進行詳細的了解，并輔助制定控制策略。經過多次試跑可以得到小車運行參數，包括運行速率、傳感器探測值、輸出轉向值、輸出速度值等，然后將運行參數發送至電腦，在電腦上運用matlab作圖分析。第六屆全國大學生智能汽車邀請賽第三章智能車機械結構調整第三章智能車機械結構調整第三章智能車機械結構調整在智能車比賽中，最主要的比賽內容是速度和穩定性，而模型車的機械結構無疑是影響速度和穩定性的關鍵因素之一。因此我們對模型車的機械結構做了大量的調整，使其達到理想的效果。車模主要機械參數見表3.1。.圖3.SEQ圖3.\*ARABIC1B型車模項目參數車模幾何尺寸（長、寬、高）（毫米）650mm250mm150mm車模軸距/輪距（毫米）軸距200mm輪距1電機型號540伺服器型號S-A6車模平均電流（勻速行駛）(毫安)700mA車模重量（帶有電池）（千克）1.8kg表3.SEQ表3.\*ARABIC1車模主要機械參數3.1車體重心的調整汽車重心是指汽車重力的作用點，汽車重心的位置會影響汽車的動力性、制動性、操縱穩定性、平順性、通過性和舒適性等重要特性。通過物理知識可知，為了保證智能汽車在高速入彎時不發生側翻，重心越低越好。所以，我們在安放主控電路板的時候盡可能的低。且為減輕車體重量，繪制PCB板以小巧、簡潔為重要指標。同時，使用輕便的碳素桿固定前置傳感器裝置杠桿，使用亞克力板（有機玻璃）制作固定裝置，在滿足剛性要求的同時，減輕車體重量。關于重心前后位置的調整，根據汽車理論，車體重心前移，會增加轉向，對模型車制動性和操縱穩定性有益，但是降低轉向的靈敏度，同時降低后輪的抓地力；重心后移，會減少轉向，但是增大轉向靈敏度，后輪抓地力也會增加，對模型車動力性能有益。通過大量實驗，我們發現：將車體重心調整到中間偏前的位置，智能車的各項性能均較好。3.2智能車前輪參數調整在小車過彎時，轉向舵機的負載會因為車輪轉向角度的增大而增大。為了盡可能降低轉向舵機的負載，我們對前輪的安裝角度，即前輪定位進行了調整。前輪定位的作用是保障汽車直線行駛的穩定性，轉向輕便和減少輪胎的磨損。前輪參數主要定位參數包括主銷后傾角、主銷內傾角、前輪外傾角和前輪前束四個內容。3.2.1主銷后傾角當站在車身左側，觀察車的左前輪，我們會發現主銷是向后傾倒的。這樣做的主要目的是為了讓主銷的延長線與地面的交點在車輪觸地點的前面。這種設計是為了使車輪在滾動的過程中保持穩定，不致左右搖擺。主銷后傾角越大，車速越高，車輪偏轉后自動回正力也就越強；但同時回正力矩過大，將會引起前輪回正過猛，加速前輪擺振，并使轉向沉重。通常后傾角為1°-3°，如圖3-2所示。圖3.SEQ圖3.\*ARABIC2主銷后傾3.2.2主銷內傾角主銷軸線與地面垂直線在汽車橫向斷面內的夾角成為主銷內傾角，即從車前后方向看輪胎時，主銷軸向車身內側傾斜的角度。舉一個生活中的例子：就像高速運動的滑冰運動員，當在拐彎的時候會向所轉向的方向傾斜，學過物理的人都知道，這是為了加大向心力，減小側滑。但主銷內傾角過大時，會使汽車轉向時輪胎與地面產生較大滑動，使轉向沉重，還將加速輪胎磨損。通常主銷內傾角不大于8°。圖3.SEQ圖3.\*ARABIC3主銷內傾角3.2.3車輪外傾角除了上述的主銷后傾和內傾兩個角度以保證汽車穩定直線行駛外，車輪中心平面也不是垂直于地面的，而是向外傾斜一個角度，稱為車輪外傾角。當車輪上下跳動時，車輪平面沒有傾斜，發生側滑的可能性較大。調試過程中主要調節了前輪的外傾角，通過車輪中心的車輪橫向平面與車輪平面的交線和地面垂線之間的夾角稱為前輪外傾角。從前后方向看車輪時，輪胎并非垂直，而是稍微傾倒呈現“八”字形張開，稱為負外傾，而反向張開呈“V”字形時稱為正外傾。前輪外傾角一方面可以在汽車重載時減小或消除主銷與襯套、軸承等處理的裝配間隙，使車輪接近垂直路面滾動而滑動，同時減小轉向阻力，使汽車轉向輕便。圖3.SEQ圖3.\*ARABIC4前輪外傾角3.2.4車輪前束車輪有了外傾以后，在滾動時就會導致兩側車輪向外滾開。由于轉向橫拉桿和車橋的約束使車輪不可能向外滾開，于是車輪在無法按照自己的預想軌跡滾動的情況下，勢必產生橫向滑動，從而加重了輪胎的磨損。為了消除這種不良影響，在安裝車輪時，使汽車兩前輪并不平行，俯視車輪，會發現兩前輪就象人的內八字腳一樣。這稱為車輪前束。圖3.SEQ圖3.\*ARABIC5車輪前束3.3舵機安裝舵機轉向是整個控制系統中延遲較大的一個環節，為了減小此時間常數，通過改變舵機的安裝位置可以提高舵機的響應速度。經過實驗分析，我們將舵機立起來安裝在車的中軸線上，舵機連接桿與轉向搖臂在水平方向連接，從而使舵機大部分力臂用于轉向；同時將舵機前置，使其重心基本保持在前輪軸距上，增加了車頭重量，為轉向提供了足夠大的摩擦力。并且我們加長了舵機的連接桿，利用增大力臂長度，可提高線速度的原理，使舵機輸出力臂增加，即讓舵機轉動一個小角度即可使前輪轉動大角度，可更好的滿足舵機靈敏度，利于小車轉向。圖3.SEQ圖3.\*ARABIC6舵機安裝3.4齒輪傳送機構的調整車模后輪采用RS385S電機驅動，由競賽主辦方提供。齒輪傳動機構對車模的驅動能力有很大的影響。調整的原則是：兩傳動齒輪軸保持平行，齒輪之間的配合間隙要合適，過松容易打壞齒輪，過緊又會增加傳動阻力，浪費動力；傳動部分要輕松，順暢，容易轉動，不能有卡住等現象。判斷齒輪傳動是否調整好的依據是，聽一下電機帶動后輪空轉時的聲音。聲音刺耳響亮，說明齒輪間的配合間隙過大，傳動中有撞齒現象；聲音悶而且有遲滯，則說明齒輪間的配合間隙過小，或者兩齒輪軸不平行，電機負載加大。調整好的齒輪傳動噪音小，并且不會有碰撞類的雜音。第四章智能車硬件部分第四章智能車硬件部分智能車系統主要分為以下幾個部分：中央處理器單元、路經檢測單元、速度檢測單元、直流電機驅動單元、舵機驅動單元、調試電路單元、電源單元等。4.1路經檢測傳感器設計路徑檢測傳感器是針對于特種路線識別的傳感器，工業上應用與自動導引小車（AutomaticGuidedVehicles）的定線尋跡。電磁車要檢測的賽道環境是由通有20kHz、100mA交變電流的導線所產生的電磁場。經過前述理論分析可知，采用線圈作為傳感器有很強的適應能力，能夠滿足電磁場檢測的要求，并且容易實現，比較適合檢測變化的電磁場。根據傳感器總體結構圖，設計傳感器電路圖包括LC串聯諧振電路感應電磁場信號，經過后級放大電路放大弱小信號，通過半橋檢波和低通濾波器后輸出直流信號。4.1.1探頭部分電感使用市場上比較方便購買的工字形電感，這類電感體積小，Q值高，具有開放的磁芯，可以感應周圍交變的磁場。如圖4.1所示。圖4.SEQ圖4.\*ARABIC1LC電路使用LC串并聯電路實現選頻，中心頻率為： （公式4.1）從公式中可以看出，在頻率一定的情況下電感與電容成反比，減小電容容量可以增加諧振電感的容量，從而使分布電容相對于諧振頻率影響較小。但是這樣會降低諧振電路的Q值，減小系統的靈敏度。折中之后，最終選定參數：L=10mH，C=6.3nF（故電容最終選擇市面上最接近的6.8nF）。整車效果圖如圖4.2所示：圖4.SEQ圖4.\*ARABIC2整車效果圖4.1.2放大調理電路本方案使用三個10mH電感置于車模頭部作為小車位置的傳感器。使用下圖（圖4.3）模擬電路對電感得到的電動勢信號進行采集、模擬、放大。該電路采用電壓并聯負反饋電路，采用電感和電容諧振放大感應電動勢。使用三極管進行放大后，利用肖特基二極管檢波后送入MCU進行AD轉換。圖4.SEQ圖4.\*ARABIC3放大調理電路4.2速度檢測傳感器設計在智能車競速系統中，速度控制是至關重要的一個步驟，無論是直道加速還是彎道減速，都涉及到對電機速度的檢測，而對電機轉速的檢測則有多種方法。4.2.1速度檢測傳感器選擇與安裝速度傳感器常見的有以下三種方案：方案一：霍爾傳感器?；魻栭_關集成電路中的信號放大器將霍爾元件產生的幅值隨磁場強度變化的霍爾電壓經過信號變換器整形、放大后輸出幅值相等、頻率變化的方波信號。方案二：光電編碼盤。在后輪齒輪傳動上粘貼一個黑白相間的光碼盤，通過固定在附近的反射式紅外傳感器讀取光碼盤轉動的脈沖。方案三：測速發電機。輸出電動勢與轉速成比例的微特電機。改變旋轉方向時輸出電動勢的極性即相應改變。在被測機構與測速發電機同軸聯接時，只要檢測出輸出電動勢，就能獲得被測機構的轉速。通過比較以上三種，我們測速裝置采用一個小型的歐姆龍E6A2-CWZ3C旋轉編碼器。電機轉動時帶動傳動盤轉動，測速裝置就產生一系列脈沖，把這一系列的高低電平送到單片機的ECT接口，單片機捕捉到這一系列高低電平的上升沿和下降沿。通過累積到一定時間內的脈沖個數，可以計數出相應的速度值。編碼器示波器輸出波形如圖4.4，安裝方式如圖4.5。圖4.SEQ圖4.\*ARABIC4編碼器示波器輸出波形圖4.SEQ圖4.\*ARABIC5編碼器安裝4.2.2速度檢測傳感器電路設計測速傳感器通過與主驅動電機的嚙合得到當前電機的轉速，然后通過內置的光電編碼盤輸出占空比可調的TTL電平。圖4.6中的三端精密可調電阻用于控制輸出信號中的占空比，在調出最滿意的波形信號之后電路的輸出端就能輸出TTL電平，然后通過MCU的計數器直接計算累加的脈沖數量（單位時間內累計的脈沖數量與電機轉速成正比）。圖4.SEQ圖4.\*ARABIC6速度檢測傳感器設計4.3起始線檢測模塊設計按照比賽規則要求，跑完一圈后賽車需要自動停止在起始線之后三米之內的賽道內。如圖4.7所示，起始線是導引線兩邊的長度10cm的黑色線，起始線中間安裝有永久磁鐵，每一邊各三只。磁鐵參數：直徑7.5-15mm，高度1-3mm，表面磁場強度3000-5000Gs。圖4.SEQ圖4.\*ARABIC7賽道起始線示意圖針對上述要求，利用干簧管設計起始線檢測電路如圖4.8。干簧管是磁機械效應的磁場傳感器，其內部是一個常開觸點開關，在磁場強度超過其閾值時，開關閉合。智能車中使用六個干簧管并聯為“線或”關系，任何一個干簧管檢測到磁鐵，GGAN端都會輸出正脈沖，引發中斷程序使賽車停車。圖4.SEQ圖4.\*ARABIC8干簧管電路圖4.4MCU控制模塊控制模塊主體是單片機MC9S12XS128最小系統，主要包括時鐘、旁路電容、電源接口、燒錄和調試接口、I/O接口等。S12XS產品滿足了用戶對設計靈活性和平臺兼容性的需求，并在一系列汽車平臺上實現了硬件和軟件可重用性。圖4.SEQ圖4.\*ARABIC9S12XS128核心控制板實物圖4.4.1MCU控制模塊電路設計MCU最小系統包括其5V電源輸入電路、時鐘電路、復位電路、RS232電平轉換電路、BDM調試電路等。MCU內部最高總線速度為40MHz本設計在最小系統的基礎上，開發設計了外圍電路，包括傳感器采集電路、驅動電路、調試電路、電源電路、顯示電路等。圖4.SEQ圖4.\*ARABIC103.3V電源無線模塊圖4.SEQ圖4.\*ARABIC11BDM調試接口電路圖4.SEQ圖4.\*ARABIC12單片機最小系統板4.5驅動模塊設計在往屆比賽中，絕大多數隊伍采用的方案是通過并聯多片MC33886驅動芯片提高電機的驅動能力。但是由于設計電路的不合理性使得流過多片MC33886驅動芯片的電流不均勻，經常出現一片過流，而另外的沒有滿載的情況。我們采用由MOS管搭建而成的H橋電路來提高整體電路的驅動能力。通過單片機輸出占空比可調的脈沖信號，控制MOSFET導通時間實現對電機轉速的控制。這樣電路的內阻很小，而且允許通過的電流很大，可以提供很強的驅動能力。圖4.SEQ圖4.\*ARABIC13電機驅動H橋電路圖4.6電源模塊設計電源模塊為系統其他各個模塊提供所需要的電源。設計中，除了需要考慮電壓范圍和電流容量等基本參數外，還要在電源轉換效率、降低噪聲、防止干擾和電路簡單等方面進行優化?？煽康碾娫捶桨甘钦麄€硬件電路穩定可靠運行的基礎。圖4.SEQ圖4.\*ARABIC14電源分配圖4.6.15V供電電源5V電壓用來供給MC9S12單片機、串口、傳感器驅動等，因此采用LM2941的穩壓芯片。LM2941具有大電流、低功耗、電路簡單可靠的優點。圖4.SEQ圖4.\*ARABIC155V數字、模擬供電電路第五章智能車軟件部分第五章智能車軟件部分高效穩定的軟件程序是智能車平穩快速尋線的基礎。本智能車采用工字電感線圈作為尋跡傳感器，傳感器數據處理就成為了整個軟件系統的核心內容。在智能車的轉向和速度控制方面，我們使用的是經典的PID控制算法，配合使用理論計算和實際參數補償的方法，使在尋跡中智能車達到了穩定快速的效果。5.1MC9S12XS128片內資源簡介MC9S12XS微控制單元作為MC9S12系列的16位單片機，包括16位中央處理器、128KB的Flash存儲器、8KB的RAM、2KB的EEPROM、兩個異步串行通信接口、兩個串行外圍接口、一組8通道的輸入捕捉或輸出捕捉的增強型捕捉定時器、兩組8通道10路模數轉換器、一組8通道脈寬調制模塊、一個字節數據鏈路控制器、29路獨立的數字I/O接口、20路帶中斷和喚醒功能的數字I/O接口、3個增強型CAN總線接口。同時，單片機內的鎖相環電路可使能耗和性能適應具體操作的需要。圖5.SEQ圖5.\*ARABIC1MC9S12XS128片內資源在整個系統設計中，用到了6個單片機基本功能模塊：PWM輸出模塊、ECT模塊、AD模塊、串口通信模塊、外部中斷模塊以及普通I/O模塊。根據系統實際需求，對各個模塊進行了初始化配置，通過對相應數據寄存器或狀態寄存器的讀寫，實現相應的功能。5.2智能車控制系統總體方案軟件設計時控制系統的核心，在具有良好的硬件基礎上，好的算法才能充分發揮作用，粗略思想則更進一步決定了比賽的成績。算法上的靈活運用可以用來彌補硬件上的一些不足，所以硬件基礎和策略算法是相輔相成的，最終目的是實現算法和硬件的相協調。磁導航智能車系統的控制算法及軟件設計由以下幾個部分組成：路徑識別檢測、速度檢測、PWM電機控制算法、速度控制策略、控制舵機的轉向控制策略等。道路檢測由電感傳感器完成，其中使用了MC9S12XS128的ATD模塊。電機、舵機驅動和測速部分使用了單片機的PWM模塊、SPI模塊和外部中斷等。小車主程序框圖如圖5.2。圖5.SEQ圖5.\*ARABIC2主程序框圖控制策略的選擇對智能小車的行駛性能也是至關重要的。速度固然越快越好，但也要使小車能夠平穩完成比賽。譬如，直道入彎前速度需要減慢，以免沖出賽道；而從彎入直時則應讓小車加速，在直道上能以較高速度完成。5.3整體程序流程圖圖5.SEQ圖5.\*ARABIC3整體程序流程圖圖5.SEQ圖5.\*ARABIC4中斷程序流程圖5.4路徑識別模塊軟件設計路徑識別是智能小車的核心內容，是決定小車能否順利完成比賽的關鍵部分。路徑識別模塊將智能車前端電感傳感器對20kHz交變電磁場的采樣值進行處理，并將所得出的值控制舵機轉向與電機轉速。路徑識別模塊程序流程圖如圖5.5。圖5.SEQ圖5.\*ARABIC5路徑識別模塊流程圖5.5智能車舵機控制算法在智能車通過路徑識別模塊識別路況后，利用轉向控制算法控制智能車，使之很好的跟隨帶狀引導線前進。由于小車頻繁進入彎直道，故對小車舵機的控制策略是一個連續的控制策略，使小車不停調整方向適應不同路徑。圖5.SEQ圖5.\*ARABIC6智能車舵機控制框圖5.5.1轉向控制策略細述首先通過比較兩路A/D采集傳感器電壓大小，可以得知傳感器與漆包線之間的位置。利用車頭左右兩只傳感器采集處理后的電動勢數值進行做差。當漆包線在車體正中間時，左右側傳感器做差后數值為0，判定前方為直線軌跡則控制舵機轉向正中，同時控制驅動電機加大速度；當車體偏向漆包線左方時，右側傳感器靠近漆包線數值增加，左側傳感器遠離漆包線數值下降，左右側傳感器數值相減出現負值，判定前方為右彎軌跡，則控制舵機使前輪向右偏轉相應角度，同時根據車速檢測單元檢測的當前速度，控制電機使速度下降，以免小車沖出賽道；當車體偏向漆包線右方時類似處理。以此根據做差后的數值可以判斷車體與漆包線的位置關系，從而控制舵機轉向，修正車體位置。圖5.SEQ圖5.\*ARABIC7小車相對黑線典型位置5.6智能車電機控制算法一般情況下小車的實際速度與期望速度之間會有一定的差異，而且該差異會隨著路徑曲率的變化而發生變換。為了使得小車能夠以最快速度跟蹤期望速度，調速過程中采用了增量PID控制。實際使用速度控制策略，構成閉環速度控制系統，利用PWM控制電機轉速，使得小車能夠在穩定運行的前提下盡可能提高響應速度，從而縮短調速過程。5.6.1PWM控制原理脈寬寬度調制（PWM）技術控制的原理就是通過直流斬波，利用大功率晶體管的開關特性來調制固定電壓的直流電源。按照一個固定的頻率來接通和斷開，并根據需要改變的一個周期內的“接通”和“斷開”時間的長短，通過改變直流伺服電機電樞上的電壓的“占空比”來改變平均電壓的大小，從而控制電動機的轉速。5.6.2智能車速度控制策略速度控制及執行系統硬件部分由H橋電機驅動系統、電機、傳動齒輪、后輪系統組成。系統的控制對象為模型汽車、系統執行器為電機、系統測量環節為速度檢測。系統框圖如下：圖5.SEQ圖5.\*ARABIC8閉環速度控制系統框圖5.6.3PID控制算法比例積分微分控制是過程控制中應用最廣泛的一種控制規律。實際經驗及理論分析充分證明這種控制規律用于多數被控對象能夠獲得較滿意的控制成果。(1)比例(P)控制比例控制是一種最簡單的控制方式。其控制器的輸出與輸入誤差信號成比例關系。當僅有比例控制時，在比例控制器中系統輸出存在穩態誤差?？刂破鞯妮敵鲂盘朥o與輸入信號ΔUi(即偏差)的大小成正比，即 （公式5.1）(2)積分(I)控制在積分控制器中，控制器的輸出信號Uo與輸入信號ΔUi對時間的積分成正比，即 （公式5.2）積分控制中，只要輸入信號ΔUi存在，控制器的輸出信號Uo就不斷積累，系統的輸出量逐漸趨向期望值，直至輸入偏差信號ΔUi=0，系統進入穩態為止。無靜差控制是積分控制的最大優點。為了消除穩態誤差，在控制器中必須引入“積分項”。積分項對誤差取決于時間的積分，隨著時間的增加，積分項會增大。因此，比例+積分的控制器，可以使系統在進入穩態后無穩態誤差。(3)微分(D)控制微分控制器針對被調量的變化率進行調節，可以調節被調量的變化趨勢，及時避免出現大的偏差。在微分控制中，控制器的輸出與輸入誤差信號的微分(即誤差的變化率)成正比關系，即： （公式5.3）這樣，具有比例+微分的控制器，就能夠提前抑制誤差的控制作用等于零，從而避免了被控量的嚴重超調。所以對有較大慣性或滯后的被控對象，比例+積分微分控制器能改善系統在調節過程中的動態特性。5.6.4增量式PID控制電機電機控制的期望速度來源于速度規劃環節。expspeed=Vmax–SPKp*fabs(Pe) 其中Vmax為設定的電機最大速度，它減去位置偏差Pe的倍數作為期望速度。也就是說，偏差越小，期望速度越大；偏差越大，期望速度越小。速度傳感器獲取電機實際速度；期望速度與實際速度的偏差用于增量式PID計算，得出控制電機的PWM波占空比；電機驅動將PWM信號功率放大，驅動電機，控制賽車速度等于期望速度。根據上述公式設計電機控制程序核心語句如下：Vdu=VKp*(Ve-Ve1)+VKi*Ve+VKd*(Ve-2*Ve1+Ve2);Vu=Vu+Vdu;其中Ve、Ve1、Ve2是偏差值及其歷史值，Vu是向電機驅動對應的PWM通道占空比寄存器值，Vdu是Vu的增量。第六章系統調試6.1開發工具及系統調試系統的開發調試用到了Metroworks公司的CodeWarriorIDE軟件集成運行環境作為開發工具對飛思卡爾的MC9S12XS微控制器進行軟件開發，開發界面如圖6.1。圖6.SEQ圖6.\*ARABIC1CodeWarrior4.7開發界面CodeWarrior的功能非常強大，可用于絕大部分單片機、嵌入式系統的開發。用戶可在新建工程時將芯片的類庫添加到集成環境開發環境中，工程文件一旦生成就是一個最小系統，用戶無需再進行繁瑣的初始化操作，就能直接在工程中添加所需的程序代碼。利用BDM和CodeWarrior4.1自帶的hiwave.exe用戶可以進行一系列的調試工作，如監視寄存器狀態、修改PC指針、設置斷點等，這樣能快速地幫助我們找到軟件或硬件的問題。6.2在線調試通過CodeWarriorIDE的在線調試工具，對傳感器不同排布的特性進行詳細的了解，并輔助制定控制策略。同時，通過按鍵調整模塊進行策略的選擇和調試的試驗。在控制系統中，環境參數的確定很重要，故本設計采用環境變量采集策略，對于傳感器采集的數據，在歸一化時需要標準量進行轉化，所以我們在比賽前對賽道進行一次參數矯正，手動調整模型車的位置，使其所有傳感器采集環境變化量，作為歸一化的標準。采集完之后調整撥碼開關，進入比賽程序。這樣就在最大程度上適應了環境的變化。圖6.SEQ圖6.\*ARABIC2在線調試方法第七章賽車主要技術參數改造后智能車的主要技術參數如表所示：項目參數路徑檢測方法（賽題組）電磁組車模幾何尺寸（長、寬、高）（毫米）650250150車模軸距/輪距（毫米）軸距200輪距135車模平均電流（勻速行駛）(毫安)700電路電容總量（微法）350.9傳感器種類及個數10mH電感3個干簧管6個100碼編碼器1個新增加伺服電機個數0賽道信息檢測空間精度（毫米）300賽道信息檢測頻率（次/秒）100主要集成電路種類/數量mc9s12x128最小系統板1塊電源模塊1塊電機等驅動模塊1塊比較電路模塊1塊運算放大電路模塊1塊車模重量（帶有電池）（千克）1.8第八章總結這份技術報告中，我們小組通過對整體方案、硬件電路、軟件算法、機械結構、調試參數等方面進行介紹，詳盡地闡述了我們的思想和創意，具體表現在電路的創新設計，以及算法方面的獨特想法。這份報告凝聚著我們的心血和智慧，是我們共同努力后的成果。本文設計了基于電磁感應檢測技術的智能模型車。首先對設計方案進行分析，選擇感應線圈檢測的方案。檢測電路方面，我們嘗試使用了高放大倍數的三極管和運算放大器，最終選擇了較為簡單的三極管放大電路。本文接著分別對模型車的設計構思和策略進行介紹。最后是測試方案和試驗結果。模型車還有需要改進的地方，如算法上可以采用記憶算法，更進一步提高智能化，提高運行速度。在整個的設計和調試過程中，我們不斷發現新問題，解決問題，爭取做到最好，但很多地方還有待提高。在準備比賽的過程中，我們小組成員涉獵該競賽涵蓋了控制、模式識別、傳感技術、電子、電氣、計算機、機械等多個學科知識，這次磨練對我們的知識融合和實踐動手能力的培養有極大的推動作用。在此要感謝清華大學將這項很有意義的科技競賽引入中國；感謝飛思卡爾公司和主辦方為我們提供了一個展示自我的舞臺；我們的成果離不開學校的大力支持及指導老師悉心的教導；還要感謝的是和我們一起協作的隊員們，協助、互促、共勉使我們能夠走到今天。最后，向審閱此報告的各位專家教授表示深深的敬意。參考文獻[1]卓晴,黃開勝,邵貝貝.學做智能車：挑戰“飛思卡爾”杯[M].北京:北京航空航天大學出版社,2007.3-5.[2]林凌.微型感應線圈車輛傳感器[J].天津大學精密儀器與光電子學院.2006[3]李仕伯,馬旭,卓晴.基于磁場檢測的尋線小車傳感器布局研究[J],電子產品世界，2009-12-10.[4]張昊飏,馬旭,卓晴.基于電磁場檢測的尋線智能車設計[J].電子產品世界，2009-11-10.[5]邵貝貝.單片機嵌入式應用的在線開發方法.清華大學出版社.2004[6]孫同景，陳桂友.Freescale9S12十六位單片機原理及嵌入式開發技術[M].北京：機械工業出版社，2008.8[6]徐士良.常用算法程序集:C語言描述.清華大學出版社.2004[7]張濤.李家啟.基于參數自整定模糊PID控制器的設計與仿真.交通與計算機.2001年第19期[8]王毅敏.馬麗英等.一種改進的數字PID控制算法及其在勵磁系統中的應用電網技術[J].1998[9]KwangSooChang.AUTOMATEDHIGHWAYSYSTEMEXPERIMENTSINTHEPATHPROGRAM.DepartmentofElectricalEngineeringandComputer,SciencesUniversity[10]MC9S12XS128DeviceUserGuideV02.09.技術手冊附錄A程序源代碼附錄A部分源程序代碼#include<hidef.h>/*commondefinesandmacros*/#include"derivative.h"/*derivative-specificdefinitions*/#include"Variable.h"#defineKduoji100////connectedwithUartvoidSendByte(unsignedcharch){ while(!(SCI0SR1&0x80)); //keepwaitingwhennotempty SCI0DRL=ch;}voidSCI_Init(){ SCI0CR1=0x00; //一個停止位，8個數據位，無校驗SCI0BDH=0x02;SCI0BDL=0x71; //SCI0BDL=busclk/(16*SCI0BDL)13位 //busclk8MHz,9600bps,SCI0BD=0x34 //busclk16MHz,9600bps,SCI0BD=0x68 //busclk24MHz,9600bps,SCI0BD=0x9C //busclk32MHz,9600bps,SCI0BD=0xD0 //busclk40MHz,9600bps,SCI0BD=0x106SCI0CR2=0x2C;}//將uchar分解為3個char型’0‘－’9‘的字符voidSCI0Tx255(UINTch){uchara,b,c;ch=ch%1000;a=ch/100;b=ch%100;b=b/10;c=ch%10;a+='0';b+='0';c+='0';SendByte(a);SendByte(b);SendByte(c);SendByte('');}voiddelay(){UINTi=100;UINTj=6000;for(;i>0;i--){//_FEED_COP();/*feedsthedog*/for(;j>0;j--);}//_FEED_COP();/*feedsthedog*/}/***************AD_Init()***************///000KKKKKKvoidATD_Init(void){ATD0CTL1=0X00;ATD0CTL2=0XC0; //11000000正常工作，快速清除，禁止外部觸發，禁止中斷ATD0CTL3=0X80; //1000000轉換序列長度為8，非先進先出模式ATD0CTL4=0x8A; //100010108為精度，轉換時鐘周期為32M/,共需2+2+8==12個轉換時鐘周期ATD0CTL5=0XB0;//10110000右對齊，無符號，連續轉換，使用多通道從通道0開始}voidSET_PLL(){CLKSEL=0x00; //disable pll; /busperiod=12Mhz*(SYNR+1)/(REFDV+1)=24MHzPLLCTL=0xe1;//CLOCKMONITOR=ON,PLL=ON,AUTOSELECT,是默認設置SYNR=5;REFDV=2;//busclock=25M,如果采用25M晶振，要獲得25M總線則SYNR=1;REFDV=1;PLLCTL=0x60;//自動鎖定//busclock32MasmNOP;asmNOP;asmNOP;while(0==(CRGFLG&0x08));CLKSEL=0x80; //pllstart}/********Servo_Init()**********///OOKKKKKKKKKKvoidServo_Init(void) //舵機16位PWM控制{PWME_PWME1=0; //disablePWM1servo//PWMPRCLK|=0x03;//CLKA:8分頻PWMCLK_PCLK1=0;//這就是默認值//通道1用clockA時鐘源//24MHZ/8=3MPWMPOL_PPOL1=1;//先高電平PWMCAE=0X00; //對齊方式默認左對齊PWMCTL_CON01=1; //通道1:16BIT連接PWMPER01=60000; //60000對應20ms //1ms脈沖到2ms脈沖 //1.5ms脈沖對應0度PWMDTY01=ANGLE_MID; //4500:1.5ms; //控制3500左極限5500右極限4500正中央PWMCNT01=0; //寫計數寄存器,會使PWMDTYx,PWMPERx進入鎖存器,同時使能通道時會從此值PWME_PWME1=1; //PWM通道1輸出}//***************RTIinitiation***************/*voidRTI_Init(void){RTICTL=0x7f; //總分頻16*2^16，所以實際RTI中斷頻率為4M/2^20=4HzCRGINT_RTIE=1;//使能RTI中斷}*///***************RTIinitiation***************//neendtobechangedvoidInit_IRQ(void){//IRQCR_IRQMOD=0; //0:邊沿方式;1:電平方式IRQCR_IRQE=0; //0:邊沿方式;1:電平方式IRQCR_IRQEN=1; //1：中斷引腳使能；0：普通IO//IRQSC_IRQEDG=0; //下降沿或低電平信號有效}voidRTI_Init(void){/*setupoftheRTIinterruptfrequency*//*adjustedtoget1millisecond(1.024ms)with16MHzoscillator*/RTICTL=0x3f; //01001111//5*2^16:48.8hz//0x1f//setRTIprescaler::晶振16384分頻;(低四位+1)*(2^(高三位+9))CRGINT=0x80; //enableRTIinterrupts;//低四位:1-7,不可為零,否則分頻器不工作}//***************Forward_Init()***************/voidForward_Init(void) //假定前進方向，初始化函數{PWME_PWME3=0;PWME_PWME5=0;DDRT_DDRT6=1;DDRT_DDRT7=1;PTT_PTT6=1;PTT_PTT7=0;PWMPRCLK=0X33; //時鐘8分頻：ClockA=clockB=24/8=3MHZPWMSCLA=0X01;//時鐘SA為3m/2ClockSAis3mHz/2=1.5MHzPWMSCLB=0X01;//時鐘SB為3m/2ClockSBis3mHz/2=1.5MHzPWMCAE=0x00;//左對齊輸出模式outputleftalignwaveformPWMPOL=0Xff; //通道輸出波形開始極性為1outputwaveformwhichhighfirstthenlowwhenthedutycounterisreachedPWMCLK_PCLK3=1;//PWM時鐘源SA:01SB:23SA:45PWMCLK_PCLK5=1;PWMCTL=0X60;//2345級聯//電機控制PWMPER23=750;//2khzPWMDTY23=0;//占空比初始：Dutyis50%,andPWMwaveform'sfrequentis16PWMPER45=750;PWMDTY45=0;PWME_PWME3=1;PWME_PWME5=1;}/***************ECT初始化***************/////////////////187.5KHZ//////needtochangefrequencybasedonspeedvoidECT_Init(void)//ECT初始化，使用輸入捕捉功能{TSCR1_TFFCA=1;//automaticallyclearflagTSCR2=0x87;//TimerOverflowinterruptopen,TCNTprescaler:24MHZ/128=187.5kHz計數頻率TIOS_IOS2=0;//Thecorrespondingchannelactsasaninputcapture//ONCH0PT0;TCTL4_EDG2A=1;//2通道上升沿捕捉TCTL4_EDG2B=0;TIE_C2I=1;//使能2通道中斷TSCR1_TEN=1;//使能定時器/*TIOS=0x00;//輸入捕捉TCTL3=0x02;//通道4下降沿觸發TSCR2=0X07;//分頻,用來給TCNT作時間參考,這里沒有意義TSCR1=0x80;//TimerEnableTIE=0X10;//初始化定時器開啟TIE_C2I=1;//使能2通道中斷*/}uintAbs(inta){if(a>=0)returna;elsereturn-a;}voidBrake_Car(intPACE){//Speed_Control(0);/*if(!BrakeFlag){PTT_PTT6=1;PTT_PTT7=0;PWME_PWME3=0;PWME_PWME5=0;PWMDTY45=0;PWMDTY23=0;PWME_PWME3=1;PWME_PWME5=1;}*///delay();PTT_PTT6=0;PTT_PTT7=1;PWME_PWME3=0;PWME_PWME5=0;PWMDTY45=0;PWMDTY23=PACE;PWME_PWME3=1;PWME_PWME5=1;}voidSpeed_Control(intPACE){//_FEED_COP();/*feedsthedog*//*if(BrakeFlag){BrakeFlag=0;PTT_PTT6=1;PTT_PTT7=0;PWME_PWME3=0;PWME_PWME5=0;PWMDTY45=0;PWMDTY23=0;PWME_PWME3=1;PWME_PWME5=1;}*/if(PACE>=0){PTT_PTT6=1;PTT_PTT7=0;PWME_PWME3=0;PWME_PWME5=0;PWMDTY45=PACE;PWMDTY23=0;PWME_PWME3=1;PWME_PWME5=1;}elseBrake_Car(Abs(PACE));}voidAngle_Control(uintANGLE){PWME_PWME1=0;PWMDTY01=ANGLE;PWME_PWME1=1;//delay();}voidStopCar(){//StopFlag=1;PORTB=0XFE;delay();Speed_Control(0);DisableInterrupts;}voidStartCar(){StopFlag1=0;EnableInterrupts;}voidLossRoad(void){staticcnt=0;//if(Angle==ANGLE_MID)//else//Speed_Control(600);/*if(Speed>550){Reverse(50);delay();delay();//delay();}*///Speed=SPEEDLOW;5//Speed-=10;//delay();//if(Speed<100)Speed=100;//Speed_Control(Speed);//if((Angle!=(ANGLE_RIGHT+70))&&(Angle!=(ANGLE_LEFT-70)))/*if(cnt++==10){cnt=0;Speed=0;}*///else//PORTB=0x0f;//PORTB=0X00;Speed=400;{/*if(Angle>ANGLE_MID)Angle=ANGLE_LEFT;elseif(Angle<ANGLE_MID)Angle=ANGLE_RIGHT;*//*else{if(SamLeft>SamRight)Angle=(ANGLE_LEFT);elseif(SamLeft<SamRight)Angle=(ANGLE_RIGHT);}*/}if(LossCnt<10){Object_Speed=MAX_SPEED+150;Speed_Control(500);}else{Object_Speed=MAX_SPEED-50;Speed_Control(700);//if(Angle==ANGLE_LEFT+20)//Angle-=100;//else//Angle+=100;}Angle_Control(Angle);}voidAD_Convert(void){inti;staticintcnt;UCHARmax1,max2;//PreValue[0]=Value[0]; //前左線圈電壓//PreValue[1]=Value[1]; //前右線圈電壓////**不帶檢波的信號采集**///////for(i=0;i<5;i++)/*for(m=0;m<4;m++){for(n=0;n<8;n++){Value[m*8]=Value[(m+1)*8];}}while(!ATD0STAT0_SCF);for(i=0;i<40;i++)sum+=Value[i];return((char)(sum/40));*//////***帶檢波電路的信號采集***///////////DisableInterrupts;SamLeft=0;SamRight=0;SamMid=0;max1=0;max2=0;for(i=0;i<8;i++){//ATD0CTL5=0XB0;while(!ATD0STAT0_SCF);ValueLeft[0]=ATD0DR0L;//左線圈電壓ValueRight[0]=ATD0DR6L;//右線圈電壓ValueMid[0]=ATD0DR4L;//SamLeft+=ValueLeft[0];SamRight+=ValueRight[0];SamMid+=ValueMid[0];}SamLeft>>=3;SamRight>>=3;SamMid>>=3;if(cnt++==7){cnt=0;PpreSamLeft=PreSamLeft;PreSamLeft=SamLeft;PpreSamRight=PreSamRight;PreSamRight=SamRight;}range1=0;range2=0;range3=0;range4=0;PianZuoCnt=0;PianYouCnt=0;for(i=0;i<199;i++){ MemLeft[i]=MemLeft[i+1]; MemRight[i]=MemRight[i+1];MemMid[i]=MemMid[i+1];}MemMid[i]=SamMid;MemLeft[i]=SamLeft;MemRight[i]=SamRight;for(i=0;i<20;i++){if(MemMid[i]>140)range1++;}for(i=180;i<199;i++){if((MemMid[i]<10)&&(MemRight[i]<50)&&(MemLeft[i]<50))range2++;}for(i=160;i<199;i++){if((MemMid[i]>100))range3++;}for(i=160;i<199;i++) //20{if(MemMid[i]>140&&i<190)PianZuoCnt++;elseif(MemMid[i]<30&&i>=190)PianYouCnt++;}/*if(WaitWanFlag&&!WaitNextFlag)if((MemMid[199]<MemMid[197])&&(MemMid[197]<MemMid[195])&&MemMid[199]<50){WaitWanFlag=0;WaitNextFlag=1;if(SamLeft>SamRight)PreWanStyle=1;//leftelsePreWanStyle=2;//right}if(WaitNextFlag){JianGe++;if((MemMid[199]<MemMid[197])&&(MemMid[197]<MemMid[195])&&MemMid[199]<50){if(SamLeft>SamRight)NextWanStyle=1;//leftelseNextWanStyle=2;//rightif(NextWanStyle!=PreWanStyle){if(JianGe<=75){SmallSFlag=1;SmallSCnt=25;}WaitNextFlag=0;JianGe=0;}}}if(SmallSFlag&&SmallSCnt){SmallSCnt--;PORTB=0X00;}else{SmallSFlag=0;PORTB=0Xff;}*/}voidPID_Init(){pid.KP=2;pid.KI=2;pid.KD=2;pid.PosLimit=750; //速度最高機極限pid.NegLimit=-750; //速度最低限pid.SUM=0;pid.PreError=0;}intFloatPIDCal(intPresent_Speed,intObject_Speed)//onlyuseKP{interror,derror,i;pid.SUM=0;error=Object_Speed-Present_Speed;for(i=0;i<4;i++)speedmem[i]=speedmem[i+1];speedmem[i]==error;//pid.SUM+=error;for(i=0;i<4;i++)pid.SUM+=speedmem[i];derror=error-pid.PreError;pid.PreError=error;error=1*error+derror*10+pid.SUM; //+derror*10;*7/5/*if(error>pid.PosLimit)error=pid.PosLimit;if(error<pid.NegLimit)error=pid.NegLimit;*/returnerror;}voidsetPa(void){ucharpatemp;uinti=6000,j=100;ucharPa;DDRA=0x00;Pa=PORTA;patemp=Pa|0x0f;switch(patemp){case0x7f:MAX_SPEED=280;break;case0xbf:MAX_SPEED=300;break;case