第八屆全國大學生智能汽車邀請賽技術報告-1-引言這份技術報告中，通過對小車整體設計思路、電路設計、算法策略及調試參數的介紹，詳盡地闡述了我們的想法，具體表現在機械、電路的獨特設計，以及算法方面的創新想法，而對車模具體參數的調試我們也付出了很大的精力。這份報告凝聚著大家的心血和智慧，在此要感謝學校的大力支持及老師的悉心教導，特別感謝和我們一起協作的隊員們，協助，互促，共勉使我們走到了今天。1.1“飛思卡爾杯”智能車比賽介紹本文以第八屆“飛思卡爾”杯全國大學生智能汽車競賽為背景，該競賽以智能汽車為研究對象的創意性科技競賽，是面向全國大學生的一種具有探索性工程實踐活動，是教育部倡導的大學生科技競賽之一，為加強大學生實踐、創新能力和團隊精神的培養，促進高等教育教學改革。該競賽以“立足培養，重在參與，鼓勵探索，追求卓越”為指導思想，旨在促進高等學校素質教育，培養大學生的綜合知識運用能力、基本工程實踐能力和創新意識，激發大學生從事科學研究與探索的興趣和潛能，倡導理論聯系實際、求真務實的學風和團隊協作的人文精神，為優秀人才的脫穎而出創造條件。智能車系統有著極為廣泛的應用前景。結合傳感器技術和自動駕駛技術可以實現汽車的自適應巡航并把車開得又快又穩、安全可靠；汽車夜間行駛時，如果裝上紅外攝像頭，就能實現夜晚汽車的安全輔助駕駛；它也可以工作在倉庫、碼頭、工廠或危險、有毒、有害的工作環境里，此外他還能擔當起無人值守的巡邏監視、物料的運輸、消防滅火等任務。在普通家庭轎車消費中，智能車的研發也是很有價值的，比如霧天能見度差，人工駕駛經常發生碰撞，如果用上這種設備，激光雷達會自動探測前方的障礙物，電腦會控制車輛自動停下來，撞車就不會發生了。1.2第八屆比賽規則介紹本屆比賽，光電平衡組采用車模直立行走的方式，使用D型車模。車模運行時只允許動力輪著地，車模直立行走。車模運行方向應按照圖1-1所示：圖1-1光平衡組車模運行模式參加光電平衡組的車模可以使用光電傳感器、指定型號的線性CCD傳感器進行道路檢測，禁止使用激光傳感器。1.3技術報告章節安排本技術報告以小車的設計為主，包括小車的機械結構設計、硬件電路設計、軟件及控制算法設計分為四章：第2章智能車系統總體設計，對智能車的機械調整、硬件電路設計、軟件系統的設計進行概要性的介紹。第3章智能車機械調整，主要介紹對小車的機械結構的改裝及車模電路板、線性CCD、編碼器等模塊的安裝方法。第4章智能車硬件系統設計，主要介紹智能車硬件系統的設計思想、設計過程和設計結果。第5章智能車軟件系統設計，主要介紹智能車軟件系統的理論分析、控制器設計過程和調試。第6章結論與展望，總結了在本次大賽當中所取得的成果與不足之處。第二章系統整體介紹第二章系統整體介紹本章從機械、硬件、軟件三個方面上對小車進行介紹，從整體上把握該系統設計思路與框架，通過對這三方面的調整，提高小車性能，使小車平穩快速運行。2.1機械結構介紹智能車系統任何的控制都是在一定的機械結構基礎上實現的，因此在設計整個軟件架構和算法之前一定要對整個車模的機械結構有一個整體的認識，然后建立相應的數學模型。從而再針對具體的設計方案來調整賽車的機械結構，并在實際的調試過程中不斷的改進和提高。車模重心的位置的改造，車模重心的高低對于整車的性能的影響很大，介于直立系統的特殊性，車模的重心越低有利于直立控制，車模的直立效果及速度控制的效果越好，同時重心降低，可以防止車模側翻，所以我們的目標就是要最大限度的降低車模的重心，以此來提高車模高速行駛的穩定度。考慮小車的速度和轉向對靈敏度和靈活度的要求，車模的質量越大，車模的慣性也越大，所以小車應該越輕越好，所以要考慮如何從硬件等方面盡量減輕小車的重量。機械調整部分大致可以概括為以下幾個部分：車模的簡化：買回來的車模是需要經過改裝才會符合要求，而且要根據運行的狀態在不違反規則的前提下進行改變。電池的安放位置：電池的質量占據了整個車模系統很大的比重，所以它的位置將很明顯地影響整個車的重心，電池如何擺放還會影響到車模的力矩，。電路板的安放與布局：我們把全部的電路放在一塊電路板上，為降低車模重心，盡量降低電路板的位置測速光電編碼器的安裝：光電編碼器是用來測輪子的速度的，它的安裝位置是不固定的，光電編碼器類型的選擇也要進行選擇，因為它關系到系統對速度信息獲取。本著讓車模質量輕、重心低、轉動慣量小的目的，我們通過多次修改車模的布局，最終得到一個相對成熟的方案，下圖為改裝后車模的整體圖：圖2-1車模完整圖2.2硬件系統的介紹測速單片機最小系統板線性CCD傾角及角速度傳感器模塊電機驅動模塊測速編碼器液晶及按鍵模塊藍牙模塊電機MOTOR電機MOTOR測速單片機最小系統板線性CCD傾角及角速度傳感器模塊電機驅動模塊測速編碼器液晶及按鍵模塊藍牙模塊電機MOTOR電機MOTOR5V7.2V3.3V電源管理模塊PWM圖2-2硬件系統框圖系統的硬件電路板部分大體可概括為六個部分：單片機最小系統板：我們采用的飛思卡爾公司的系列的16位單片機MC9S12XS128,它是系統的控制核心，具有豐富的資源，功能足以進行小車的控制。(2)電源管理電路模塊：此模塊為整個電路系統提供電源，要求電源的質量高，電壓穩定。我們采用的2940是和AMS1117分別為系統提供5V和3.3V的穩定電壓，電機則直接由電池供電。(3)電機驅動電路模塊：本模塊為單片機與電機之間提供一個接口，讓單片機可以有效地控制電機。要求驅動電路的驅動電流足夠大，內阻小，開關頻率高等。我們采用的是BTS7960半橋驅動芯片，它的驅動能力強，滿足D型車模260電機的需求。(4)為直立控制設計的傳感器模塊：要讓車模站立起來，我們采用的是組委會規定使用的村田公司的ENC03系列陀螺儀和飛思卡爾公司的MMA7361加速度計。這兩種傳感器各有特點，相互補充就可以實現車模傾角和角速度的測量，為直立控制奠定基礎。(5)循跡傳感器模塊：車模識別路徑使用了組委會指定的TSL1401系列的線性CCD。(6)其它附加模塊：這包括藍牙模塊和液晶按鍵模塊。藍牙和按鍵液晶模塊設計成可方便插拔式的，在調試過程中起輔助作用。2.3軟件系統的介紹系統軟件部分大體可分為四個部分：(1)車模直立控制器：直立控制器是要通過陀螺儀和加速度計整合后的輸出值來控制電機，使車模實現直立，而難點就在于兩種傳感器的整合，這是車模直立的關鍵所在。如果車模傾斜的角度和角速度不能精確的得到，直立就很難做好。這兩種傳感器都是基于微機械工藝的傳感器，它的特點是成本低、體積小，在低成本的同時，也會帶來精確度的下降，所以這就需要較好的軟件算法來進行傳感器的融合，通過融合得到一個準確的角度和角速度信息，為直立控制打好基礎。(2)車模轉向控制器：這是算法的重點所在，因為比賽要求以盡量快的速度跑完整圈賽道。這就要求能對賽道做出精準的判斷，(3)車模速度控制器：速度控制也是本系統的一個難點，因為這個兩輪倒立模型本來就是一個不穩定的非線性系統，速度控制和直立控制之間存在耦合(4)車模調試系統：這部分是為方便調試和監控車模的運行所做的附加部分，但也是為追求好的控制效果所必須有的。調試系統包括無線數傳部分，它可以把單片機檢測到的數據傳到PC機上，供上位機分析，這樣對車模運行的快速性和穩定性有很大的幫助，第二個部分是人機交互接口，液晶和按鍵模塊，有了這個部分就可以更加方便的調試車模，提高調試的效率。車模以最優路徑穩定快速的行駛車模以最優路徑穩定快速的行駛直立控制器速度控制器轉向控制器車模信息分析與決策賽道信息獲取角度及角速度測量車模速度測量圖2-3軟件控制框圖第三章機械結構3.1車模簡介車模采用廣東東莞博思電子數碼科技有限公司提供的D型車模，按照大賽組委會的規定，對車模的改動是有一定限制的，不能隨意改動車模的電機、齒輪傳動比、輪距、底盤等。D型車模采用的是兩個RN—260型號的直流有刷電機，電機的功率相對去年直立電磁車較大。今年光電組采用兩輪直立行走的方式，所以和去年電磁車相同，車模兩個前輪及其轉向機構是可以完全去除，再去除被動懸架之后，車模就只剩下兩個電機、兩個輪子、齒輪傳動機構、電池和底盤。之后就將在此基礎上逐步完善這個兩輪直立循跡小車系統。圖3-1D型車模3.2車模機械調整3.2.1車模的固定由于直立車是一個比較特殊的系統，車模整體的機械結構對直立控制、速度控制以及轉向控制會有很大的影響，不同的機械結構小車的性能會有很大的不同，直立車模型可以視為一個一級倒立擺，但是由于車模的底盤和電機的連接部分存在的一個很大的縫隙，縫隙導致車模在直立行走時發生震蕩，從而車模的物理模型就有一級倒立擺演變為二級倒立擺，那么此時再用一級倒立擺的程序來維持小車的平衡就很困難了，當小車在快速度跑動的過程中，輕微的震動或者抖動都會對小車造成較大的影響。為此，我們最終用PCB板來固定車底盤和電機部分，并將PCB板切割成固定的形狀，去除其他多余的部分，以確保車模的穩定直立運行。圖3-2車模固定板3.2.2車模電路板設計安裝為了盡量降低車模重心，我們對電路板進行了改造，并改變了電路板的位置，為了盡量車模降低重心，特意重新設計了將主控電路與驅動電路整合的L字形PCB電路，并改變了電路板的位置，將其和電池放在同一面，驅動模塊位于電池下面，有效利用了車模正面的安裝空間，并同時搭載了液晶顯示器件，合理的利用了車身所能提供的空間，L形電路板如下圖所示。第三章機械結構第三章機械結構圖3-3電路板的設計安裝3.2.3車模電池位置的固定電池是整個小車中質量最大的一個部分，它的位置對小車系統的重心影響較大。經過多次嘗試，并盡量降低車模整體重心，最終確定把電池放在車模的背面，并對電池的高低進行了調整，最終使電機幾乎靠近輪子的軸心的位置，這樣電池的位置就幾乎放到了最低位置。圖3-4車模電池位置固定好電池后，車模的總體質量要注意，質量越大，車模的慣性也越大，對于小車系統來說，它對速度和轉向都要求很高的靈敏度和靈活度，所以整車應該越輕越好。在系統需要添加硬件的時候，必須把它引入的質量而加大了車模慣性這一不利因素考慮進來。3.2.4線性CCD的安裝本車采用兩個CCD來循跡，我們考慮了多種CCD安裝方案，最終確定了兩種方案，其中第二種方案是對第一種的改進。第一種方案是將碳素桿裝在車的中線的位置，開始是用熱熔膠將CCD粘在碳素桿上，在最終確定CCD的前瞻后，在碳素桿上打孔，并自制CCD的安裝支架，將CCD用螺絲固定在碳素桿上，經過區賽后，發現這種方法沒有將CCD完全固定住，小車摔倒后CCD的傾角會發生變化，導致后面小車跑時路徑不好，為此我們做出了改進，改碳素桿為較剛的彈簧片，直接將CCD固定在車上，這樣子就不會導致小車摔倒時CCD傾角改變圖3-5CCD支架圖3-6使用碳素桿裝CCD圖3-7使用彈簧片裝CCD為了盡量減輕車模的重量，我們把CCD原用的杜邦線換成了漆包線，用五根細的漆包線并繞成一根線，每個CCD用五根線，將這些線編織在一起，既減輕車模的重量，又使車模顯得簡潔，同時為防止車摔倒時撞倒CCD，我們在車底板裝了支架，用以防止車模倒下時摔壞CCD。圖3-8保護CCD的支架3.2.5電機的調整及編碼器的安裝直接用剛買回來的車模，發現小車在調速度的過程中，出現直道跑不直的情況，后來分析發現是因為左右兩個電機不對稱，對多個電機進行測試，讓電機加速轉，采集電機的速度值，并在MATLAB里面繪制成曲線，比較曲線找到動態特性接近的兩個電機，換掉原來的兩個電機。電機的響應可以分成兩個階段，一為加速階段，二為恒速階段。其中，在加速階段，電機帶動輪子進行加速運動，加速度近似和施加在電機兩端的電壓成正比。在恒速階段時，車輪的速度和加在電機兩端的電壓近似正比關系。裝好電機后按圖3-9我們把編碼器裝好。圖3-9編碼器安裝圖圖3.10在階躍輸入下電機的轉速變化圖第四章硬件電路設計第四章硬件電路設計4.1車模電路布板思路電路板是由幾個獨立的模塊組成的，前期電路均采用各模塊獨立制板分開布局，經過多次測試后發現電機驅動控制PWM信號端采用杜邦線連接，在車模運行過程中會有接觸不良的現象產生，長期插拔更會加重此故障，所以在后期制作電路時我們將分開的角度傳感模塊和電機驅動模塊集合在一塊電路中，以排除跳線接觸故障。驅動電路和供電電路整合在一塊電路中隨之而來的干擾產生了，驅動電路的大電流會干擾主芯片的工作，帶來的干擾我們采用一塊主板兩個地端（一個模擬地，一個實際地）其中兩個地端中間采用10uh電感相連避免驅動電流產生的干擾。各模塊集成為一塊電路后，不僅利于電路安裝還可以避免電路調整時因人為接線錯誤燒毀電路的事故。4.2車模電路原理車模的電路板總共有兩塊，一塊是最小系統板，一塊是電源主板。圖4-1最小系統板圖4-2電源主板4.2.1最小系統板我們采用的是飛思卡爾公司的MC9S12XS128芯片，最小系統板的硬件電路設計主要包括以下幾個部分：時鐘電路、電源電路、BDM接口。其中各個部分的功能如下：1、時鐘電路給單片機提供一個外接的16MHz的石英晶振；2、電源電路主要是給單片機提供5V電源；3、復位電路由復位開關組成當按下復位開關時，芯片復位；4、BDM接口讓用戶可以通過BDM頭向單片機下載和調試程序。使用的資源有：（1）ATD模塊陀螺儀：一路，檢測車模角速度；加速度計：一路，測量車模傾角；線性CCD：2路，采集CCD輸出的模擬電壓值。（2）PWM模塊控制左右兩個電極雙方向運行。由于采用單極性ＰＷＭ驅動，需要四路PWM接口。如果采用雙極性ＰＷＭ驅動，可以使用兩路。（3）TIM模塊對編碼器脈沖數進行計數。（4）SCI串口通訊模塊用于無線通訊設備藍牙。（5）IO接口用于LCD液晶顯示，撥碼盤、編碼器計數脈沖及方向信號輸入口及信號燈設置等。單片機的時鐘電路：通過把一個16MHz的外部晶振接在單片機的外部晶振輸入接口EXTAL和XTAL上，然后利用MC9S12XS128內部的壓控振蕩器和鎖相環（PLL）把這個頻率提高到40MHz，作為單片機工作的內部總線時鐘，外部振蕩電路如下：4.2.2電源主板電源模塊為整個系統提供電源，要求電源質量高，供電電壓穩定，圖4-2為電源主板。尤其是給單片機供電的電源，對電源質量的要求更高，如果電源波動很大，單片機就會出現復位的情況，而且電池的供電能力有限，所以當電機出現堵轉時電池電壓就會被拉得很低，這時就要對單片機的供電進行保護。所以我們在芯片和主板的電源供電上采用雙穩壓芯片并聯穩壓，給芯片和主板分別獨立供電，防止電機堵轉對芯片電壓的干擾，除此以外我們還采用了大電容給單片機儲存電能，在電池電壓拉低的短時間內給單片機提供電源，防止由于電壓過低導致單片機復位。并聯的雙穩壓芯片我們前期采用的是插針式7805，但經過一段時間的調試后我們發現7805的穩壓對電池電壓的要求比較高，當車模在加速期間電池電壓會被拉低到6.5V，而7805穩壓自身的壓降為2V此時7805穩壓輸出電壓為4.5V給芯片供電就會偏低，所以后期我們將2個7805穩壓芯片全部更換為2940以獲取更穩定的電源供電，圖4-3為雙并聯插針式2940電源穩壓模塊。電源供電原理圖如下：圖4-4穩壓電源供電模塊原理4.2.3車模直立電路模塊車模直立電路使用了陀螺儀、加速度傳感器，兩者配合使用以判斷車模傾角姿態。陀螺儀采用日本村田公司的ENC-03如圖4-4所示，供電電壓采用AMS1117穩壓芯片輸出的3.3V，靜止狀態輸出1.35V左右，其輸出電壓隨著旋轉成線性變化。該產品是一種應用科氏力原理的角度傳感器，在器件中利用壓電陶瓷做成振動單元。當旋轉器件時會改變振動頻率從而反映出物體旋轉的角速度，它輸出一個和角度成正比的模擬電壓信號，特征為：體積小重量輕；快的響應；低的驅動電壓和功耗。圖4-5ENC-03加速度計則采用MMA7361三軸模擬信號加速度計，通過微機械加工技術在硅片上加工形成了一個機械懸臂。它與相鄰的電極形成了兩個電容。由于加速度使得機械懸臂與兩個電極之間的距離發生變化，從而改變了兩個電容的參數。通過集成的開關電容放大電路量測電容參數的變化，形成了與加速度成正比的電壓輸出，，式中，為重力加速度；gθ為車模傾角；為加速度傳感器靈敏度系數系數。當傾角kθ比較小的時候，輸出電壓的變化可以近似與傾角成正比。圖4-6MMA7361及其原理圖角度計算電路：下圖給出了使用一個運算放大器計算角度的電路。其中電位器P1可以調節陀螺儀的比例因子。圖4-7車模角度計算電路圖4-8角度傳感模塊原理4.2.4車模驅動模塊車模采用BTS7960芯片作為主驅動芯片，驅動能力完全能夠滿足電機要求，我們采用了H橋驅動電路來驅動電機，橋驅動電路”是因為它的形狀酷似字母H。圖4-9H橋驅動電路要使電機運轉，必須使對角線上的一對三極管導通，車模的電機驅動模塊原理圖如下圖所示：圖4-10H橋電路驅動電機順時針轉動及逆時轉動圖4-11車模驅動模塊原理圖由于摩擦力等因素的存在，電機與PWM給定信號存在非線性死去特性。為了使系統具有更好的線性，首先需要對電機死區進行補償。4.2.5車模編碼器測速模塊由于直立車轉彎時是靠差速來轉，所以會有左右兩個輪子速度不一樣的情況出現，為此我們采用了CD4051開關，分時計數左右兩個編碼器的速度脈沖，11引腳來控制選擇計數13和14哪個通道的脈沖數，3腳接PT7口對脈沖數計數，由于需要知道車模的方向，所以我們采用兩個編碼器，一個是157線，不帶有檢測車輪方向功能，一個是512線，可以檢測車模方向。圖4-12CD4051原理圖4.2.6液晶及藍牙模塊本屆比賽光電平衡組采用TSL1401系列的線性CCD作為傳感器進行循跡，為方便查看CCD采集來的道路信息以及二值化后的圖像，我們自制了液晶模塊，并加有按鍵功能，液晶與按鍵配合作為人機接口，實現操作員與單片機之間的信息交換，這樣可以大大地提高調試的效率，由于采用了兩個CCD，通過按鍵可以實現上下CCD的切換，為調試帶來很多方便。圖4-13液晶及按鍵模塊調試參數時，在程序里面改好參數再用BDM燒寫進去會很麻煩，因此運用了單片機的異步通信模塊來把內部數據以數據包的形式發到無線數據傳輸模塊上，然后無線模塊再傳輸到電腦上查看，我們采用的是濟南華茂科技公司的HM系列藍牙模塊，HM系列藍牙模塊采用CSRBlueCore芯片，配置6-8Mbit的軟件存儲空間，支持AT指令，用戶可根據需要更改SPP角色（主、從模式）以及串口波特率、設備名稱、配對密碼等參數，使用靈活。圖4-14藍牙模塊車模整體電路板PCB圖如下：圖4-12最小系統板圖4-13車模電路板頂層第八屆全國大學生智能汽車邀請賽技術報告第五章軟件系統設計第五章軟件系統設計軟件的主要功能包括有：車模運行狀態檢測，電機PWM輸出，車模運行控制，其中包括直立控制、速度控制、方向控制，車模運行流程控制，程序初始化、車模啟動與結束，系統界面，包括狀態顯示、上位機監控、參數設定等。其中車模運行控制是核心，為了簡化系統，我們把三個部分的輸出做簡單的疊加后輸出給兩個電機。其實三個環并不是完全的獨立的控制環路，而是相互協同配合的，這三部分的參數調整是后期控制算法調試所要做的主要工作，系統主程序框架如右圖所示。5.1直立控制設計這部分是軟件設計中最基礎的一個部分，因為所有的程序運行必須建立在小車可以站立不倒的情況下進行，所以對車模的直立穩定性就提出了較高的要求，我們在車模的直立控制上采用的是PD控制，通過角度傳感模塊和上位機采回車模傾角數據，然后利用MATLAB擬合出跟隨平滑曲線，以此方法來調整出合適的PD參數，由于機械結構有所改進，車模重心比去年降低了很多，并對去年的算法進行了改進，所以使小車擁有更強的直立剛度。5.1.1小車直立模型的建立兩輪直立小車的模型是一個倒立擺模型，它本身是一個不穩定非線性的系統。單擺模型能夠穩定在垂直位置的條件有兩個：（1）受到與位移相反的回復力；（2）受到與運動速度相反的阻尼力。圖5-1單擺受力分析圖5-2不同阻尼下的單擺運動而倒立的擺中，重力在運動方向上產生的力是與運動方向一致的，不僅不能提供回復力而且還會加速擺倒下，但是可以通過輪子的運動來提供回復力：圖5-3通過車輪運動來達到車模平衡這個回復力是由慣性力產生的，如下圖受力分析：圖5-4倒立擺受力分析，假設負反饋控制是車輪加速度與偏角θ成正比，比例為k1。如果比例k1>g，那么回復力的方向便于位移方向相反了,這樣就可以產生回復力，來保持車模的直立。當然回復力越大，對于保持直立越好，但是太大也會導致系統振蕩，當系統發生振蕩時，可以加入一定的阻尼力來消除振蕩，這就可以通過車模的角速度分量來控制車模平衡。5.1.2小車直立控制器的設計與調試根據所學的知識以及通過對直立系統的進行分析，我們最終采用了現在工業上用得比較多的PID控制器，PID控制器的實現比較簡單，而且效果比較好。要想車模直立，必須控制車模的傾角穩定在平衡位置，在直立控制這部分，對基于平衡位置的角度偏差值進行控制，角速度為角度的微分值，屬于微分量，所以最終我們采用PD控制。為了準確地進行控制，關鍵的部分在于車模的角度和角速度的精確測量。比賽規定了我們用基于微機械MEMS原理的陀螺儀和加速度計。這兩種傳感器各有各的特點，陀螺儀的特點是動態特性好，能比較精確的測得轉動的角速度，但是靜態特性不好，有溫漂和積分漂移存在。加速度計的特點正好與陀螺儀互補，它的特點是靜態特性好，能在靜態時比較精確的測得角度，但在動態時不能很好地反應車模的傾斜角度。所以我們把兩者進行互補，融合后可以得到相對較準確的車模角度和角速度。加速度計測量重力場方向的原理是通過測量微小形變來實現，當加速度計靜止放在水平臺面上時，它受到臺面給它的支持力而產生在Z軸即重力方向上的形變，這時Z軸的輸出值最大，當它和水平臺面的傾角逐漸變大時，在Z軸上的形變會漸漸變小，而在Y軸上的形變會變大，實際上Z軸和Y軸上的分量就是重力在這兩個方向上的分量，用這兩個量就可以測得重力方向了。從以上分析得知這種方式非常適合靜態測量角度，而一旦有水平方向上的加速度時，這就會干擾我們的測量，所以這時就得用到陀螺儀了，陀螺儀能直接很精確的得到角速度的值，它的積分就可以得到角度。兩者融合就可以得到精確的角度和角速度了。在得到車模的角度和角速度后，就可以通過PD控制器來控制車模的平衡，只需調節PID的參數就可以讓車模直立起來，而且效果很好。控制器這里還有一個重要的思想是我們采用了分段控制，對車模相對平衡位置-6度到+6度的偏差進行控制，不同的偏差應用不同的PD控制參數，融合后控制的框圖如下：其中加速度計所占的比重很小，也是為了消除它帶來的噪聲的影響。圖5-5直立控制的算法框圖直立控制的調試首先是調節角度和角速度計算公式前面的系數，車模在小角度范圍內晃動時，使加速度計采樣值轉換為對應的角度，使陀螺儀采樣值得到對應的角速度，其次調節由加速度計測得的角度所占的比重Tg，即加速度計的補償力度，PD控制中的比例系數K1,微分系數K2。通過實驗分析，加速度計的補償力度可以很小，陀螺儀的積分誤差也不會太大。若補償力度太大反而會使系統振蕩，因為加速度計的噪聲是很大的。對于比例和微分這兩個參數，先調節比例參數，可以逐漸加大K1，直到系統出現大幅度振蕩，這時再增加微分參數K2，因為微分參數是起阻尼作用的，它可以在一定程度上消除由于比例系數較大而引起的振蕩，但是微分系數太大會使系統出現高頻振蕩，所以適中即可。5.2速度控制設計5.2.1速度控制理論速度控制部分是由光電編碼器把輪子的速度反饋回來，然后通過負反饋機制做一個PID控制器就可以控制速度，但是在直立小車系統里的速度控制就不是那么簡單了，它是一個正反饋的機制。當需要車模加速時，并不是加大給電機的電壓讓輪子加速，速度控制反而是通過讓電機減速，這樣車模就會往前傾，角度控制與速度控制如此會達到一個平衡，這時車模會持續向前加速，因為車模往前有一個傾角就會有往前倒的角加速度，為了保持角度輪子也必須有一個向前的加速度，這樣車模就可實現加速了。要實現減速則要讓車模向后仰就可以了。圖5.6速度控制的框圖5.2.2速度控制器設計速度控制器選用PID控制器，車模的速度用左右輪子速度的平均值來表示，因為轉向時左右兩個輪子速度不同，如果單輪測速測到的速度不準確，由于速度控制會對直立控制產生干擾，通過理論分析得出，角度控制是讓車不倒而速度控制是讓車倒，為了車模穩定高速的運行，把速度控制的周期設定為25ms，直立控制周期為2ms，以此來保證直立控制不受到直立控制的影響。速度控制器的調試主要是PID三個參數，參數的調試主要借助MATLAB和藍牙兩個調試工具完成。在速度控制中，車模速度要從零加速到設定的速度值，如果直接把速度設定值減去零作為偏差進行PID控制，會使車模很不穩定，所以我們采用了分階段設定速度設定值，使設定值呈階梯狀穩定上升，這樣速度偏差值較小，速度控制比較穩定。在速度控制參數的調試上，我們花費了很多時間，主要是控制容易出現較大的超調，當第一次達到設定值時，緊接著后面會出現較大的超調，我們把達到設定值作為分界點，前后使用不同的控制參數，后來達到了比較好的效果，減小了超調量。5.3方向控制設計小車采用采用了兩個TSL1401CL系列線陣CCD，方向控制思想可以參考攝像頭組的循跡思想。5.3.1CCD的采樣TSL1401CL線陣CCD包含128個光電二極管的線性陣列。與面陣CCD相比，線性CCD最明顯的特點就是其只能采集一行的可視像素，并且輸出的是模擬量，所以需要進行AD采樣。它的時序波形與操作波形如下如所示：圖5-7CCD時序波形圖5-8CCD操作波形128個像素的采集與輸出用到了SI和CLK信號，在這128個像素之外，還有一個開關邏輯控制和移位寄存器電路。SI通過該電路，控制每一個像素的積分和復位操作；CLK通過該電路控制每一個像素電壓的依次輸出。在軟件中，我們需要按照上圖所述的CCD時序對CCD進行曝光與采集的控制。圖中，第19個CLK到下一個SI開始的這段時間，就是CCD的積分時間，及曝光時間，之前的18個CLK的時間為積分器的復位時間。在每一個SI信號之后采集到的像素均是這個SI信號之前曝光得到的圖像。具體的方法是：在SI持續高電平20ns后產生第一個CLK信號，并在每個CLK信號的下降沿時采集AO引腳輸出的電壓值。在采集了128個像素后，還必須生成第129個CLK信號以結束本次采集。在第129個CLK之后到下一個SI信號之間的時間就是下次采集的曝光時間。采集程序如下： unsignedinti=0;unsignedintj=0;SETSI;/*SI=1*/SamplingDelay();SETCLK;/*CLK=1*/SamplingDelay();RESETSI;/*SI=0*/SamplingDelay(); AD_CCD_sample(pCCD_BUFF_H,pCCD_BUFF_L);/*對CCD進行采樣*/RESETCLK;/*CLK=0*/for(i=0;i<127;i++){SamplingDelay();SETCLK;/*CLK=1*/SamplingDelay(); AD_CCD_sample(pCCD_BUFF_H+i+1,pCCD_BUFF_L+i+1);RESETCLK;/*CLK=0*/}SamplingDelay();SETCLK;/*CLK=1*/SamplingDelay();RESETCLK;5.3.2CCD采集圖像的圖像處理1、圖像預處理圖像處理簡單的來說就是根據CCD傳回來的視頻信號中提取出黑線的位置。常用的黑線提取算法分為二值化算法、直接邊緣檢測算法和跟蹤邊緣檢測算法。二值化算法的思路是：設定一個閾值，從左至右比較各像素值和閾值的大小，若像素值大于或等于閾值，則判定該像素對應的是白色賽道；反之，則判定對應的是黑色的目標引導線。記下第一次和最后一次出現像素值小于閾值時的像素點的列號，算出兩者的平均值，以此作為該行上目標引導線的位置。直接邊緣檢測算法：首先找到從白色像素到黑色像素的下降沿和從黑色像素到白色像素的上升沿，然后計算上升沿和下降沿的位置差，如果大于一定的標準值，即認為找到了黑線，并可求平均值算出黑線的中心點。至于上升沿、下降沿的檢測，可以通過上上次采樣數與這次采樣數的差值的絕對值是否大于一個閾值來判斷，如果“是”且差值為負，則為上升沿；如果“是”且差值為正，則為下降沿。跟蹤邊緣檢測算法：由于黑色的目標引導線是連續曲線，所以相鄰兩行的左邊緣點比較靠近。跟蹤邊緣檢測正是利用了這一特性，對直接邊緣檢測進行了簡化。其思路是若已尋找到某行的左邊緣，則下一次就在上一個左邊緣附近進行搜尋。這種方法的特點是始終跟蹤每行左邊緣的附近，去尋找下一列的左邊緣，所以稱為“跟蹤”邊緣檢測算法。我們的思路是：對一行中所有像素點求平均，然后再逐點對比像素點與平均值之間的關系，求出高于平均值的像素點的平均值以及低于平均值的像素點的平均值。這兩個平均值乘以一定的系數之后，這兩個值分別作為判斷為白色的的閾值與判斷為黑色的閾值。這種思路的好處是，對于一行的灰度圖像閾值有一定的死區，可以有效濾掉一部分可能產生的噪點與反光，這個思路是源于數電里面學的史密特觸發器。2、邊緣提取對于邊緣提取我們使用了捕捉二值化后圖像的上跳沿與下降沿的方法，我們的思路是：在一定圖像范圍內，檢測到上跳變沿時把當前列記入上升沿數組中，檢測到下降沿時把當前列記入下降沿數組。然后逐對地讓下降沿與上跳沿作差，保留其最大一對的差值，即為賽道的中心線。3、CCD的曝光時間控制最初我們采用的是10ms的固定時間曝光，后來發現在強光下會產生比較大的畸變，對提取邊線有一定的影響，因此改用自適應曝光。自適應曝光我們有兩種方案：第一種為在車輛運行的時候對當前的光線進行實時的調整；第二種方案為只在小車初始化時確定本次運行的曝光時間，之后曝光時間不變。經過我們實際調試，發現第一種情況對光線變化程度大的場地適應性會稍微好一點，但是由于每次曝光時間都不一樣，會導致采集的圖像不太穩定，第二種情況反之。我們的自適應曝光的程序設計思路如下：先對所有點作平均值，再逐點對高于平均值的點取平均值。當此平均值大于設定值時，讓曝光時間減小，當此平均值小于設定值時，讓曝光時間增大。4、起始線的識別我們有幾種方案檢測起始線：第一種：由于起跑線只可能出現在直道，因此在接近起始線時，車身位置較為固定。因此在實際測定的基礎上，讓程序在一定的范圍內檢測黑色點的數值，若黑色點在合理范圍內即判定為起始線。第二種：在正常賽道中，提取出來的上升沿與下降沿應為一對。在線性CCD采集到起始線圖像時，圖像應有多對上升沿與下降沿。如車身正對著起始線時，線性CCD捕捉到的圖像經過二值化后應是“黑白黑白黑白黑”，此時有三個上跳沿，三個下降沿；如車身不正，采集到的二值化后的圖像應是“黑白黑白”或“白黑白黑”，即兩個下跳沿，兩個上升沿。通過判斷跳變沿的信息即可判斷起始線。這種判斷在虛線小s時會遇到一些問題，因虛線小s背景板為白色，所以也會出現多個跳變沿。我們的解決方法是在原判斷上加上對黑色段寬度的判斷，經測試可行。第三種：在車身兩側加裝紅外對管，當兩側紅外對管同時檢測到黑線時即為起始線。5、路障的識別與處理由于直立平衡組的路障顏色是黑色的，因此我們通過圖像檢測的方法檢測路障。在采集到路障的圖像時，圖像的大部分為黑色，此時可以通過對黑色點計數或者對整幅圖像的平均灰度進行判斷，當黑色點數值大于設定值或平均灰度小于設定值時即為路障。過路障對于直立小車來說是一個比較大的困難，開始我們不加速度控制，純靠直立控制讓小車過路障，發現在低速時可以過去，速度達到一定值小車會摔倒，然后加上速度控制讓小車過路障，發現同一速度下穩定性要比只有直立環時的差，所以我們在檢測到路障后采用關閉速度環，只靠直立環的方法來過路障，由于高CCD有60cm的前瞻，在檢測到路障后，通過給小車前傾的角度讓小車快速減速，這樣可以提高降速的速度，還以提高小車過路障的穩定性，當小車通過路障后，打開速度環，小車正常行駛。第六章開發工具說明第六章開發工具說明6.1Codewarrior開發工具簡介此次智能車大賽的軟件開發平臺為Metroworks公司的CodeWarrior4.7，Codewarrior是由Metrowerks公司提供的專門面向Freescale所有MCU與DSP嵌入式應用開發的軟件工具。其中包括集成開發環境IDE、處理器專家、全芯片仿真、可視化參數顯示工具、項目工程管理、C交叉編譯器、匯編器、鏈接器以及調試器。用戶可以將芯片的類庫添加到集成環境開發環境中生成就是一個最小系統，就能直接在新建工程時添加所需的程序代碼。圖6-1為Codewarrior用戶操作界面。圖6-1Codewarrior用戶操作界面6.2基于MATLAB的參數計算及仿真MATLAB是矩陣實驗室（MatrixLaboratory）的簡稱，是美國MathWorks公司出品的商業數學軟件，用于算法開發、數據可視化、數據分析以及數值計算的高級技術計算語言和交互式環境，主要包括MATLAB和Simulink兩大部分。在小車調試中，我們主要使用MATLAB完成數據的圖像化處理以及參數調試。直接在單片機上運行算法有效果不直觀、程序下載耗費大量時間的缺點。使用MATLAB可以進行方便的仿真和計算，參數整定也可以使用MATLAB的相關函數進行計算。而且MATLAB有強大的繪圖能力，我們通過藍牙回傳數據，并在MATLAB上進行繪圖，可以觀測傳感器信號的質量和變化規律且可以參看參數效果。我們主要利用MATLAB在直立角度的擬合和觀測速度曲線上，通過藍牙將加速度計和陀螺儀的采樣值發回到電腦上，然后在MATLAB中編寫直立的角度擬合曲線的程序，通過觀察擬合角度的曲線和加速度計算得到的曲線的跟隨程度，來調整參數，如圖6-2所示。圖6-2使用MATLAB進行直立控制參數整定其中藍色是加速度計采樣值計算得到的角度值，紅色是擬合出來的角度曲線，黃色是角速度值。速度控制的參數調試比較麻煩，通過藍牙我們進行速度控制參數和速度的設定值的發送，并通過藍牙接收速度值，然后在MATLAB里面繪制成曲線，通過曲線來查看速度控制參數是否合適。圖6-3速度控制效果圖綠色是實際速度，黃色是速度設定值，藍色是角度。第七章總結第七章總結7.1車模的主要技術參數表7-1車模的主要技術參數項目參數車模幾何尺寸長、寬、高（mm）170*130*310車模總體重量(kg)1.5車模軸距、前輪距、后輪距（mm）后輪距115電容總容量（uf）傳感器種類與個數CCD*2、MMA7361*1、ENC03*1、編碼器*2除了車模原有的驅動電機、舵機之外伺服電機個數無賽道信息檢測精度(mm)賽道信息檢測頻率（Hz）無7.2制作心得在備賽的一年里，學校和學院在場地、經費、技術方面給予了我們極大的幫助，能順利完成車模的調試去參加全國比賽，不可忘的就學校和學院里一直支持和關注智能車比賽的領導、老師和學長學姐們，尤其是徐耀良老師、楊寧老師、黃云峰老師、黃福珍老師、師兄們的指導和鼓勵。同時也要感謝飛思卡爾賽事組委會組織了這個意義深長的比賽，它不僅讓我們在大學里真正通過動手實踐學習到了許多知識，而且最重要的是教會了我們如何發現問題和解決問題的方法，這一點是終身受益的。飛思卡爾實驗室對我們來說已經不單單是一個可以動手制作的平臺，這里其實更像是一個家庭，所有的隊員們為了車模調試的進度和性能的穩定，大家不約而同的都住在了實驗室里，每天一起衣食住行、一起發現問題、解決問題，在這里我們所建立的情感是大學里其他同學們所不能體會的。雖然大家有時會因為意見不同而發生爭執，但當我們走到常州的區域賽場上時才猛然發現我們其實早已融為一個整體，大家心靈相惜，情緒相系，強有力的凝聚力是我們最大的士氣。賽場上我們的車并不是很順利，決賽時因為輪胎太滑在過路障的時候兩次摔倒，所有人的心都揪在了一起，唯一的希望都寄托在最后一次機會上——當車模最終順利沖過終點時，我們壓抑了數分鐘的沉寂終于被喊破，此刻我只能用淚奔語塞來描述我的心情。這個車模以遠遠超出了比賽，它承載了我們太多的努力、汗水、還有情感，我想這份經歷將會成為我們青春中最絢爛的標記，為我們書寫下正能量的始點。參考文獻[1]張陽，吳曄，滕勤等編著.MC9S12XS單片機原理及嵌入式系統開發[M].北京：電子工業出版社，2011.[2]馮智勇,曾瀚,張力,趙亦欣,黃偉.基于陀螺儀及加速度計信號融合的姿態角度測量[J].西南師范大學學報，2011,36（4）:137-141[3]陳靜.兩輪自平衡機器人模型及控制方法研究[D].北京工業大學碩士學位論文,2008.[4]孫同景.Freescale9S12十六位單片機原理及嵌入式開發技術[M].北京：機械工業出版社，2007.[5]羅華飛編著.MATLABGUI設計學習手記[M].北京：北京航空航天大學出版社，2010.[6]胡仁喜.ALTIUMDESIGNER從入門到精通[M]北京：機械工業出版社，2011[7]卓晴.電磁組智能車調試方案V2.0[8]施頌椒.陳學中，杜秀華編著，現代控制理論（第二版）高等教育出版社2009年[9]譚浩強著C程序設計（第四版）清華大學出版社2010年[10]卓晴，黃開勝，邵貝貝.學做智能車-挑戰“飛思卡爾”杯[M].北京航空航天大學出版社,2007.3.附錄附錄A電路原理圖附錄B核心算法直立平衡算法：#include"balance.h"#include"AD.h"#include"speed.h"#ifdef_FLOAT #defineGYROSCOPE_ANGLE_RATIO-0.11 #defineGRAVITY_ANGLE_RATIO0.05 intangleouttable[11]={0,3,8,16,22,28,35,37,38,39,40};#endif#defineGYO_OFFSET_RESET2132#defineGRAVITY_OFFSET_RESET1600#defineGRAVITY_ADJUST_TIME_CONSTANT2#defineGYROSCOPE_JIFEN_FREQUENCY400#defineCAR_ANGLE_SPEED_SET0floatangle_d_out[5]={0.10,0.12,0.22,0.22,0.20};intVOLTAGE_GRAVITY=0;intVOLTAGE_GYRO=0;intGYROSCOPE_OFFSET=0;intGRAVITY_OFFSET=0;intg_fAngleControlOut;intg_fanglecontrolout_offset=0;#ifdef_FLOATfloatg_fGravityAngle=0;floatg_fCarAngle=0;/*前傾為負，后仰為正*/floatg_fGyroscopeAngleSpeed=0;floatg_fGyroscopeAngleIntegral=0;voidbalance_control(void){ floatfValue; floatfDeltaValue,tempangle; uintangle; AD_banlance_save(&(uint)VOLTAGE_GYRO,&(uint)VOLTAGE_GRAVITY); g_fGravityAngle=(VOLTAGE_GRAVITY-GRAVITY_OFFSET)*GRAVITY_ANGLE_RATIO;//-0.145 if(g_fGravityAngle>20)//對角度進行限幅 g_fGravityAngle=20; elseif(g_fGravityAngle<-20) g_fGravityAngle=-20; g_fGyroscopeAngleSpeed=(VOLTAGE_GYRO-GYROSCOPE_OFFSET)*GYROSCOPE_ANGLE_RATIO;//-0.17 g_fCarAngle=g_fGyroscopeAngleIntegral; fDeltaValue=(g_fGravityAngle-g_fCarAngle)*GRAVITY_ADJUST_TIME_CONSTANT;//2 g_fGyroscopeAngleIntegral+=(g_fGyroscopeAngleSpeed+fDeltaValue)/GYROSCOPE_JIFEN_FREQUENCY;//200 tempangle=(g_fCarAngle-bar_speed_handle_angleset)*1.35;if(tempangle<10&&tempangle>-10){if(tempangle>=0){angle=(int)tempangle;fValue=-(angleouttable[angle]+(tempangle-angle)*(angleouttable[angle+1]-angleouttable[angle])); }else{tempangle=-tempangle;angle=(int)tempangle;fValue=(angleouttable[angle]+(tempangle-angle)*(angleouttable[angle+1]-angleouttable[angle]));}//分段控制if(tempangle<=0.8){fValue+=(CAR_ANGLE_SPEED_SET-g_fGyroscopeAngleSpeed)*angle_d_out[0];}elseif(tempangle<=2){fValue+=(CAR_ANGLE_SPEED_SET-g_fGyroscopeAngleSpeed)*angle_d_out[1];}elseif(tempangle<=3.5){fValue+=(CAR_ANGLE_SPEED_SET-g_fGyroscopeAngleSpeed)*angle_d_out[2];}elseif(tempangle<=6){fValue+=(CAR_ANGLE_SPEED_SET-g_fGyroscopeAngleSpeed)*angle_d_out[3];}else{fValue+=(CAR_ANGLE_SPEED_SET-g_fGyroscopeAngleSpeed)*angle_d_out[4];}}else{if(tempangle>10)//對角度輸出限幅fValue=-40;elseif(tempangle<-10)fValue=40;}if(fValue>0) g_fAngleControlOut=fValue*100;else if(fValue<0) g_fAngleControlOut=fValue*100; else g_fAngleControlOut=0; g_fAngleControlOut+=g_fanglecontrolout_offset;}#endif/平衡位置基準值采樣/voidAD_banlance_OFFSET_save(uint*pFirst_channel,uint*pSecond_channel){ uintcount=100; uchara=2; uinttemp_f,temp_s; AD_banlance_save(pFirst_channel,pSecond_channel); while(count--) { AD_banlance_save(&temp_f,&temp_s); *pFirst_channel=((*pFirst_channel)*(10-a)+(temp_f)*a)/10; *pSecond_channel=((*pSecond_channel)*(10-a)+(temp_s*a))/10; delay1ms(1); }}速度控制算法：int speed_control_out=0;int speed_left,speed_right;uchar speed_sample_flag;int speed_inter=0;int speed,speed_error;int speedcon_P[5]={14,23,28,32,34}; uchar speedcon_D[5]={10,10,20,20,0}; uchar speedcon_I[5]={3,3,2,2,1}; uchar speedcon_index;int speed_setvalue=0,temp_speedset=0;long speedcon_I_inter;int speedcon_Kpvalue;int speedcon_Dvalue;#defineSPEEDCON_SHIFT_1 (speedcon_P[1]-speedcon_P[0])*16#defineSPEEDCON_SHIFT_2 SPEEDCON_SHIFT_1+(speedcon_P[2]-speedcon_P[1])*32#defineSPEEDCON_SHIFT_3 SPEEDCON_SHIFT_2+(speedcon_P[3]-speedcon_P[2])*48#defineSPEEDCON_SHIFT_4 SPEEDCON_SHIFT_3+(speedcon_P[4]-speedcon_P[3])*64#defineBAR_SPEED_HANDLE_MAX_DISTANCE 4000#defineBAR_SPEED_SET 20uchar bar_speed_handle_flag;int bar_speed_handle_distance;intspeed_error_buff[4],speed_error_buff_index;voidspeed_control_PID(){ speed_error=(temp_speedset-speed); speed_error_buff[speed_error_buff_index]=speed_error; speed_error_buff_index=(++speed_error_buff_index)%4; speedcon_Dvalue=(speed_error-speed_error_buff[speed_error_buff_index])*speedcon_D[0]; if(absc(speed_error)<80) { speedcon_index=absc(speed_error)>>4; speedcon_I_inter+=speed_error*speedcon_I[speedcon_index]; }else { speedcon_I_inter+=0; speedcon_index=4; } if(speedcon_I_inter>50000) speedcon_I_inter=50000; elseif(speedcon_I_inter<-50000) speedcon_I_inter=-50000; switch(speedcon_index) { case0:speedcon_Kpvalue=absc(speed_error)*speedcon_P[speedcon_index];break; case1:speedcon_Kpvalue=absc(speed_error)*speedcon_P[speedcon_index]-(SPEEDCON_SHIFT_1);break; case2:speedcon_Kpvalue=absc(speed_error)*speedcon_P[speedcon_index]-(SPEEDCON_SHIFT_2);break; case3:speedcon_Kpvalue=absc(speed_error)*speedcon_P[speedcon_index]-(SPEEDCON_SHIFT_3);break; case4:speedcon_Kpvalue=absc(speed_error)*speedcon_P[speedcon_index]-(SPEEDCON_SHIFT_4);break; default:break; } speed_control_out=(speed_error>0?(-speedcon_Kpvalue):speedcon_Kpvalue)-speedcon_I_inter/20; }速度采集程序：voidspeed_sample(void){ if(SPEEDINPUT_CHANNEL_JUDGE==SPEED_CHANNEL_RIGHT) if(SPEEDINPUT_DIRECT_JUDET==SPEED_BACKWARD) { speed_right=PACNT; speed_right=-speed_right; }else{ speed_right=PACNT; } elseif(SPEEDINPUT_CHANNEL_JUDGE==SPEED_CHANNEL_LEFT) if(SPEEDINPUT_DIRECT_JUDET==SPEED_BACKWARD) { speed_left=PACNT; speed_left=-speed_left*32/10; }else speed_left=PACNT*32/10; SPEEDINPUT_CHANNEL_CHANGE; speed_sample_flag=1; PACNT=0;}方向控制算法#defineCAR_SPEED_OFFSET_LOWCCD19intdirect_control_out[]={0,0};unsignedchardifferential[];charbias_old;unsignedchardirect_control_KP[5]={13,23,32,35,40};unsignedchardirect_control_section[4]={5,10,20,30};#defineSHIFT_1 (direct_control_KP[1]-direct_control_KP[0])*direct_control_section[0]#defineSHIFT_2SHIFT_1+(direct_control_KP[2]-direct_control_KP[1])*direct_control_section[1]#defineSHIFT_3SHIFT_2+(direct_control_KP[3]-direct_control_KP[2])*direct_control_section[2]#defineSHIFT_4SHIFT_3+(direct_control_KP[4]-direct_control_KP[3])*direct_control_section[3]#defineDIRECTCON_LEFTMOTOR 0#defineDIRECTCON_RIGHTMOTOR 1voiddirect_control(char*pbias,intcar_speed){ inttemp_differential; inttempbias; inttempbias_ccd_high,tempbias_ccd_low; inttemp_speedoffset; tempbias_ccd_high=*pbias++; tempbias_ccd_low=*pbias; if(startline_flag==1) { tempbias=tempbias_ccd_low;//(tempbias_ccd_high*1+tempbias_ccd_low*3)/4; }else { temp_speedoffset=absc(car_speed)/7; tempbias=(tempbias_ccd_high*temp_speedoffset+tempbias_ccd_low*CAR_SPEED_OFFSET_LOWCCD)/(temp_speedoffset+CAR_SPEED_OFFSET_LOWCCD); } if(absc(tempbias)<direct_control_section[0]) { temp_differential=absc(tempbias)*direct_control_KP[0]; }elseif(absc(tempbias)<direct_control_section[1]) { temp_differential=absc(tempbias)*direct_control_KP[1]-(SHIFT_1); }elseif(absc(tempbias)<direct_control_section[2]) { temp_differential=absc(tempbias)*direct_control_KP[2]-(SHIFT_2); }elseif(absc(tempbias)<direct_control_section[3]) { temp_differential=absc(tempbias)*direct_control_KP[3]-(SHIFT_3); }else { temp_differential=absc(tempbias)*direct_control_KP[4]-(SHIFT_4); } if(tempbias<0) temp_differential=-temp_differential; temp_differential+=(tempbias-bias_old)*160; //if(temp_differential>0) //{ direct_control_out[DIRECTCON_LEFTMOTOR]=temp_differential; // direct_control_out[DIRECTCON_RIGHTMOTOR]=-temp_differential*(1+(car_speed>200?0:(0.002*(car_speed-200)))); //}else //{ // direct_control_out[DIRECTCON_LEFTMOTOR]=temp_differential*