攝攝攝像頭8888隊(duì)第十屆飛思杯全國(guó)生

委會(huì)和飛思卡爾半導(dǎo)體公司可以在相關(guān)主頁上收錄并公開參賽作品的設(shè)計(jì)譚幫帶隊(duì)教師簽名陳小日期 本文設(shè)計(jì)的智能車系統(tǒng)以MK40DN512VLQ10微控制器為控制單元，通過CMOS頭檢測(cè)賽道信息，提取賽道兩邊黑色引導(dǎo)線，用于賽道識(shí)別；通過光電編檢測(cè)模型車的實(shí)時(shí)速度，使用PID控制算法調(diào)節(jié)關(guān)鍵字：MK40N512VLQ10，OV7725，PID，飛思卡爾智 第一 引 第二章系統(tǒng)總體方案設(shè) 第三 機(jī)械結(jié)構(gòu)設(shè) 舵機(jī)的安 3.4.1頭支撐桿的選 3.4.2頭標(biāo)定和矯 第四 硬件系統(tǒng)設(shè)計(jì)及實(shí) 4.2.1 編 13單片機(jī)最小系統(tǒng) 電源穩(wěn)壓電路及檢測(cè)電 圖像處理電 電機(jī)驅(qū)動(dòng)電 舵機(jī)接口電 鍵盤撥碼電 第五 軟件系統(tǒng)設(shè)計(jì)及實(shí) 原始圖像的特 原始圖像的矯 賽道搜線算 賽道中心的計(jì) 起跑線的檢 5.2.3PIDI智能車大賽簡(jiǎn)辦大學(xué)生數(shù)學(xué)建模、電子設(shè)計(jì)、機(jī)械設(shè)計(jì)、結(jié)構(gòu)設(shè)計(jì)等四大競(jìng)賽的基礎(chǔ)上，委托教育部高等學(xué)校自動(dòng)化專業(yè)教學(xué)指導(dǎo)舉辦每年一度的全智能汽車制作情況概根據(jù)競(jìng)賽規(guī)則及功能要求，本智能車以飛思卡爾公司的32位單片機(jī)MK40N512VLQ10為控制器，以CMOS傳感器OV7725為核心傳感器，在提供統(tǒng)一車模平臺(tái)上，構(gòu)建完整智能小車系統(tǒng)。小車通過頭道路圖像信息送入單片機(jī)，在單片機(jī)中對(duì)輸入的原始圖像信息進(jìn)行處理，提取出賽道的特息，據(jù)此選擇最優(yōu)行進(jìn)路線并進(jìn)行速機(jī)械結(jié)構(gòu)設(shè)計(jì)主要包括前后輪的調(diào)節(jié)、PCB板的布局、車身底盤的改裝、TPS7350和TPS7333作為的穩(wěn)壓電路，可為系統(tǒng)的各功能模塊提3)速度傳感器采用的是光電編，構(gòu)成的測(cè)速部分，用以完成對(duì)速度的實(shí)時(shí)測(cè)量和反饋控制；4)MOS搭建的電機(jī)驅(qū)動(dòng)電路，驅(qū)動(dòng)電機(jī)穩(wěn)定快PIDIAREmbeddedWorkbench進(jìn)行編程開發(fā)，用J-LINK進(jìn)行程序。這些工具的使用，使得軟件的設(shè)計(jì)編程和調(diào)試工作得到了保證。所設(shè)計(jì)的系統(tǒng)經(jīng)過測(cè)試，能夠快速安文獻(xiàn)綜在智能車制作過程中，我們參考了大量的相關(guān)資料文獻(xiàn)。從scale公司，得到K40系列單片機(jī)開發(fā)技術(shù)手冊(cè)，了解所有寄存器本文主要結(jié)根據(jù)大賽的統(tǒng)一要求，須在統(tǒng)一提供的車模平臺(tái)上，自主選擇系統(tǒng)總體結(jié)其中檢測(cè)系統(tǒng)采用CMOS數(shù)字頭控制決策系統(tǒng)用MK40N512VLQ10作為主控，動(dòng)力系統(tǒng)主要控制舵機(jī)的轉(zhuǎn)角和直流電機(jī)的轉(zhuǎn)速。整體的，通過視覺傳感器來檢測(cè)前方的賽道信息，并將賽道給單片機(jī)。同時(shí)，通過光電編構(gòu)成的反饋將車體的行駛速度信息傳送給機(jī)做出決策，并通過信號(hào)控制直流電機(jī)和舵機(jī)進(jìn)行相應(yīng)動(dòng)作，從而頭頭波2.1的硬件電路主要有七個(gè)部分組成：MK40N512VLQ10及其外路，舵機(jī)驅(qū)動(dòng)模塊，WIFI等輔助調(diào)試模塊。、MK40N512VLQ10及其電路是系統(tǒng)的部分，負(fù)責(zé)接收賽道圖像數(shù)據(jù)，速度等反饋信息，并對(duì)這些信息進(jìn)行恰當(dāng)?shù)奶?、圖像采樣處理模塊采用數(shù)字頭OV7725，用于獲得前方道路車 車 編飛思卡爾16位單2.2整車效果1.32.4本章小根據(jù)的相關(guān)規(guī)定，今年頭組比模更換為C型車模。針下舵機(jī)的轉(zhuǎn)動(dòng)速度對(duì)車轉(zhuǎn)向的靈活程度起到了根本性的作用。頭是采地到良好的賽道信息。而整車的重心越低，自然才能在控制時(shí)更加靈本次比賽采用東莞市博思電子數(shù)碼科技1：10仿真車模，有裝如圖3.1所示。

2.1要低，三是前瞻要比較大。由于今年同樣采引導(dǎo)線的路徑，對(duì)頭

圖3.2頭安裝圖及轉(zhuǎn)接我們的第一輛車，就是因?yàn)闆]有掌握好頭桿對(duì)重心的影響，為了使頭固定得非常牢靠，而對(duì)桿進(jìn)行配重，最后使得在高速轉(zhuǎn)彎時(shí)非常容易側(cè)翻。參加賽區(qū)賽的第二輛車更換了較輕的頭桿，同樣的算法取剛剛買來的頭，因?yàn)榧庸淼恼`差，每一個(gè)產(chǎn)品的光學(xué)特性都編的安編是測(cè)速用的模塊，必須將其和電機(jī)牢牢咬合才能準(zhǔn)確測(cè)量500線的歐姆龍編，其性能穩(wěn)定且返回的速度值穩(wěn)定。安裝采用一個(gè)線切割的空白PCB板和車身固定在一起這樣便使得編能非常牢的和電機(jī)齒輪咬合。具體安裝圖如圖3.5所示。圖3.5編的安裝3.8整車效果3.83.9本章小本章主要介紹了小車的安裝制作和調(diào)試過程中機(jī)械方面的具體的問這版是能夠適應(yīng)算法并且最穩(wěn)定的機(jī)械結(jié)構(gòu)系統(tǒng)。硬件設(shè)計(jì)方電磁兼容性設(shè)計(jì)，做好各部分的接地、、濾波等工作，將高速數(shù)字電采用0.5mmFFC排線作為頭數(shù)據(jù)線，減小接頭體積，減輕質(zhì)量，做好，提高信號(hào)傳輸?shù)姆€(wěn)定性；傳感器的選4.2.1

COMSCCD頭具有對(duì)比度高、動(dòng)態(tài)特性好的優(yōu)點(diǎn)，但需要工作在12V電壓下，對(duì)于整個(gè)系統(tǒng)來于耗電，而且CCD體積大，質(zhì)量重，會(huì)抬與之相比，COMS頭具有體積小、質(zhì)量輕、功耗低、圖像動(dòng)態(tài)特COMS頭。1、大234567描以及隔行掃描，像OV5116就是隔行掃描。世面上的頭主要分為數(shù)字和模擬兩種，數(shù)字頭主要6O7感光本身會(huì)有一段適應(yīng)時(shí)間，這段時(shí)間要求越小越好。OV7725，BF3003比較，OV7725具有成像穩(wěn)定，技術(shù)成熟等優(yōu)點(diǎn)，選用了新的頭OV7620。經(jīng)試驗(yàn)證明，與OV7725相比，OV7620具有實(shí)現(xiàn)動(dòng)態(tài)調(diào)節(jié)，但是OV7620功耗大，對(duì)電源要求較高，復(fù)合信號(hào)紋波較大，穩(wěn)定性不如OV7725。OV7725OmniVision公司生產(chǎn)的較為典型的CMOS圖像傳感器模5V)2為方便使用26P-0.5mm-FFC排線，我們特地制作了OV7725頭的轉(zhuǎn)接板電路原理圖如圖4.1所示：GND0GND0GND0GND0GND0GND011111111111111114.2.2

圖 控制信號(hào)波形要求不是太高，我們使用歐姆龍的500線編，型號(hào)3.2圖4.3編波形電路設(shè)計(jì)方系統(tǒng)板、主板、頭轉(zhuǎn)接板、驅(qū)動(dòng)模塊。我們把單片機(jī)最小系統(tǒng)直接做在了主板上，組成了信號(hào)、信號(hào)處理、電機(jī)控制、舵機(jī)控制單元。為MK40N512VLQ10是K40系列MCU。Kinetis系列微控制器是Cortex-M4系列的內(nèi)核。K40內(nèi)存空間可擴(kuò)展，從32KB閃存8KBRAM到1MB閃存128KBRAM16KB緩存用于優(yōu)化總線帶

4.4K40最小系統(tǒng)使用K40144PIN封裝，用到的接口為：接口，外部復(fù)位電路、串行通訊接口、BDM接口和SPI接口。電機(jī)波輸出舵機(jī)波輸出VSYNC場(chǎng)信號(hào)輸入：PTB1頭圖像信號(hào)輸入：PTC0、PTC1、PTC2、PTC3、PTC4、PTC5PTC6、鍵盤輸入信號(hào)：PTA24、PTA25、PTA26、PTA27、PTA28、撥碼開關(guān)輸入信號(hào)：PTA13、PTA14、PTA15、本系統(tǒng)中電源穩(wěn)壓電路分別需要有+5V，+3.3V，+6V供電。+3.3V給單片機(jī)供電；+5V給頭、鍵盤撥碼供電；+6V給舵機(jī)和編供電由于整個(gè)系統(tǒng)中+5V電路功耗較小，為了降低電源紋波，我們考慮使過壓、電壓反接保護(hù)。使用這個(gè)只需要極少的元件就能構(gòu)成高效4.5TPS7333超是低噪，高電源電壓抑制比，高速射頻，線性調(diào)節(jié)的低壓出+3.3V為單片機(jī)供電。

4.6TPS7333我們的智能模型車自動(dòng)控制系統(tǒng)中使用黑白全電視信號(hào)格式CMOSD觸發(fā)器分頻電路原理如圖4.7所示：

23

4567

SET1SET1

圖 電機(jī)驅(qū)動(dòng)采用比較方便使用的驅(qū)動(dòng)，Infineon公司的大功和短路保護(hù)等診斷功能。供5～45V，最大電50A，驅(qū)動(dòng)信號(hào)頻率為利用控制直流電機(jī)，不僅可以簡(jiǎn)化電路設(shè)計(jì)，而且使得控制更加簡(jiǎn)單。為了防止系統(tǒng)在工作過程中因?yàn)楸Ｗo(hù)而停止工作，在小車設(shè)2MOS組成一個(gè)完整H橋驅(qū)動(dòng)電路驅(qū)動(dòng)小車3.9所示。其中，Motor端分別接電機(jī)的兩端；ISIS端輸出電壓的范圍；P1與P2，分別輸出非零占空比的和零占空比的，保證左右半橋上下背各有一個(gè)導(dǎo)通，組成一個(gè)回路。SR引腳通過MCUHCPL2630，防止驅(qū)動(dòng)電路發(fā)生擊穿633

123

5

6R26R20INHIN7INHIN7SRINHINISSRINHINISSR444488116

VSOUTVSOUTOUTISGND

63265GND3265GND

GND_121

G

R

GND_1GND_1

4.8

4.9

1818273645

4.10理就成了整個(gè)軟件的內(nèi)容。而在智能車的轉(zhuǎn)向和速度控制方面，參考賽道中心線提取及優(yōu)化處在單片機(jī)圖像信號(hào)后需要對(duì)其進(jìn)行處理以提取主要的賽道信息，由于頭自身的特性和遠(yuǎn)小近大的視覺原理，圖像會(huì)產(chǎn)生梯形式變形，這使得頭看到的賽道信息和實(shí)際真實(shí)的賽道信息有所區(qū)別。因此我們利用賽道進(jìn)量和標(biāo)定，得到一系列的參數(shù)，將頭采到的原始應(yīng)最遠(yuǎn)處，大約160cm，圖像的最底部一行對(duì)應(yīng)最近處，大約10cm。遠(yuǎn)處的頭返回的這個(gè)矩陣中，每一個(gè)像素點(diǎn)都有一個(gè)從0至255的灰度顯示

5.1S5.2

5.3我們自己的原因造成的，因此可以很容易地矯正成原始樣子。由于我服這種誤差，我們特地對(duì)頭進(jìn)行了標(biāo)定，這樣對(duì)于賽道信息的判斷，由圖5.3可以看出，原始圖像反映了安裝在小車上的頭直接看到為了方便對(duì)頭采到的圖像信息進(jìn)行處理，我們建立了一個(gè)橫向坐5.4-5.6是經(jīng)過矯正后的原始圖像，可以看出它們5.4S5.5

5.6但是是通過隔3行掃描一個(gè)黑當(dāng)連續(xù)幾行搜索黑線次進(jìn)入連續(xù)I/O口輸入一個(gè)對(duì)應(yīng)幅值的模擬信號(hào)，通過調(diào)整對(duì)周期為2ms。PIDPID調(diào)節(jié)。PID70年當(dāng)我們不完全了解一個(gè)系統(tǒng)和被控對(duì)象，或不能通過有效的測(cè)量來獲PID控制器是一種線性控制器，它根據(jù)給定值與實(shí)際輸出值構(gòu)成控制被控對(duì)象進(jìn)行控制，故稱PID控制器，原理框圖如圖5.7所示。5.7PIDe(k)r(k)c(k (T u(k)KP{e(k)

e(j) [e(k)e(kTjT

(k——采樣序號(hào)，kc(k——ke(k——kKP——

r(k——ku(k——k次輸出控制量；e(k1)——k-1次偏差；TI——積分時(shí)間常數(shù)；TD——微分時(shí)間常數(shù)； T——采樣周期。簡(jiǎn)單說來，PID控制器各校正環(huán)節(jié)的作用如下：數(shù)字PID控制算法通常分為位置式PID控制算法和增量式PID位置u(k)的值和執(zhí)行機(jī)構(gòu)的位置(如閥門開度)是一一對(duì)應(yīng)的，所以通常稱(5.2)為位置式PID控制算法。去的狀態(tài)有關(guān)，計(jì)算時(shí)要對(duì)過去e(k)進(jìn)行累加，計(jì)算機(jī)工作量大；而且因?yàn)橛?jì)算機(jī)輸出的u(k對(duì)應(yīng)的是執(zhí)行機(jī)構(gòu)的實(shí)際位置，如計(jì)算機(jī)出現(xiàn)故障，u(k的大幅度變化，會(huì)引起執(zhí)行機(jī)構(gòu)位置的大幅度變化，這種情況往往是PIDPID是指數(shù)字控制器的輸出只是控制量的增量u(k)。增量(5.2)PID控制算式。由式(5.2)可以推出式(5.3)，式(5.2)Tk u(k1)KP{e(k1)

e(j) [e(k1)e(kTjT

(u(k)K{[e(k)e(k1)]Te(k)TD[e(k)2e(k1)e(k (KPe(k)KIe(k)KD[e(k)e(k式中e(k)e(k)e(k1)；KKT； K

TT TT(5.4)稱為增量式PID控制算法，可以看出由于一般計(jì)算機(jī)控制

、TD，只要使用前后控制u(k)的確定僅與最近k次的采樣值有關(guān)，所以較容易通過處理而獲得比較好的控制效果。PIDPID參數(shù)整賴的系統(tǒng)，并且還需要考慮賽體環(huán)境帶來的影響，要建立精確的智能轉(zhuǎn)向舵機(jī)的PID控制算驅(qū)動(dòng)電機(jī)的PID控制算速度控制對(duì)于小車的快速行進(jìn)以及必要時(shí)候的起到了至關(guān)重要的IAREmbeddedWorkbenchIDE平臺(tái)IAREmbeddedWorkbench8位、1632位IAREmbeddedWorkbench有效提高用戶的工作效率，通過IAR工具，用戶可以工作時(shí)間。IAR的調(diào)6.16.1IARC-SPYDebugger允許用戶查看在源代碼中查看變量或表達(dá)式，使用戶可以在運(yùn)行程序時(shí)其值的變化，并且查看Livewatch功能，在小車實(shí)時(shí)運(yùn)行中觀察本章小模型車整體制作情況及技術(shù)參本系統(tǒng)以飛思卡爾單片機(jī)40作為控制單元，以數(shù)字頭O7725路況信息，以光電編對(duì)小速度進(jìn)行實(shí)時(shí)，完成智能車工程制作及調(diào)試，自主智能識(shí)別賽道，自動(dòng)地驅(qū)動(dòng)動(dòng)力系統(tǒng)和轉(zhuǎn)向系統(tǒng)完成智能控制。整個(gè)設(shè)計(jì)主要分為車體機(jī)械結(jié)構(gòu)設(shè)計(jì)和軟/硬件系統(tǒng)設(shè)計(jì)。車體機(jī)械結(jié)構(gòu)設(shè)計(jì)主要包括編和圖像傳感器的安裝、車身底盤的固定、舵機(jī)和轉(zhuǎn)向輪調(diào)節(jié)等；硬件系統(tǒng)設(shè)計(jì)完成了電源管理模塊、電機(jī)驅(qū)動(dòng)模塊、舵機(jī)控制模塊、速度測(cè)量模塊、輔助調(diào)試模塊、圖像處理模的ID算法設(shè)計(jì)。所設(shè)計(jì)的系統(tǒng)經(jīng)過測(cè)試，能夠快速安全行駛。經(jīng)過了賽區(qū)賽的檢驗(yàn)之后，我們也對(duì)自己的小車更加有信心。表7.1路徑檢測(cè)方法（賽題組車模幾何尺寸（長(zhǎng)、寬、高）（毫米車模軸距/輪距（毫米電路電容總量（微法車模平均電流（勻速行駛）(毫安 對(duì)光管ST188×40精 檢測(cè)頻 30有效距 檢測(cè)頻 500 車模重量（帶有電池1.583存在的問題及解決方硬件方面:頭車的硬件不是非常復(fù)雜，主要是電源部分和驅(qū)動(dòng)AMS1117系列電源穩(wěn)壓，給頭、單片機(jī)、舵機(jī)等供電。剛開始使用時(shí)還比較順暢，可用是1117系列電源穩(wěn)壓差過高，在2V以上。而我們給舵機(jī)供電電壓為間。所以，我們后面更換穩(wěn)壓為TPS73XX系列，壓差在0.35V都可單純使用兩片MOS組成的全橋電路發(fā)熱嚴(yán)重，甚至出現(xiàn)跑幾米就停車的現(xiàn)象。所以，為增強(qiáng)驅(qū)動(dòng)能力，我們采用MOS，兩兩并聯(lián)，非常完美地解決發(fā)熱過大問題，小車連續(xù)跑10圈都不會(huì)因發(fā)熱而停車。心得體 看到我們奮斗的，因此，我們對(duì)小車是有很深刻的感情。是科研。要保證自己的思想足夠天馬行空，也要保證每一個(gè)想法最比賽的同樣不得不讓我們感慨，看到同的兄弟姐妹們相繼總有人會(huì)出局。沒有熱情、沒有付出努力所得到，大家覺得是自然::

學(xué)D-I隊(duì)技術(shù)報(bào)告[R].:工業(yè)大學(xué),2012.大學(xué)頭一隊(duì)技術(shù)報(bào)告[R].:科技大學(xué),2012.院閃電六隊(duì)技術(shù)報(bào)告[R].常熟:常熟理工學(xué)院,2012.報(bào),2011,(129,江智軍,郭鵬,基于OV7620頭智能車道路信息及處理研究, /introduce.html.大學(xué)生智能汽車競(jìng)賽主函#include"commonh"#include"HAL_GPIO.h"#include"HAL_FTMh"#include"HAL_LPTMR.h"#include"HAL_PIT.h"#include"camerah"#include"search.h"#include"yseh"#include"servoh"#include"motorh"#include"system.h"unsignedcharprocess_finish;unsignedcharunsignedcharL_line_1,L_line_2,R_line_1,R_line_2;unsignedcharstart_line_lock; |名稱 |作用:電機(jī)控制定時(shí)中 void{speed_pit_lock=L_line_1=L_line_2=R_line_1=IR_line_2=}|名稱 |作用:主函 voidmain{intservo_error;intunsignedcharramp_state=LPLD_FTM1__Open(0,LPLD_GPIO_Set_b(PTC_BASE_PTR,1,0);LPLD_PIT_Init(0,2000,speed_pit_isr);L_line_1=0;L_line_2=R_line_1=R_line_2=start_line_lock=start_line_delay_time=stop_delay_flag={if(field_finish=={field_finish=0;start_line_lock=0;if(!start_line_lock&&car_state==CAR_RUN&&((L_line_1||L_line_2)&&||L_line_1=0;L_line_2=R_line_1=R_line_2={{caseRAMP_NO:indicator_set(LOFF);break;caseRAMP_UP:indicator_set(NAVY);break;caseRAMP_DN:indicator_set(PINK);break;caseRAMP_WT:indicator_set(LOFF);break;}t=} if(car_state==CAR_RUN t>car_state=if(car_state=={goto}process_finish=}if(speed_pit_lock=={speed_pit_lock=if(process_finish=={process_finish=}}}}頭子程#include"commonh"#include"HAL_GPIO.h"#include"image.h"SCL-PortA11SDA- #defineCAMERA_SCL_LOWLPLD_GPIO_Set_b(PTA_BASE_PTR,11,#defineCAMERA_SDA_LOWLPLD_GPIO_Set_b(PTA_BASE_PTR,12,0)#defineCAMERA_SDA_DATALPLD_GPIO_Get_b(PTA_BASE_PTR,12)void //初始SCCB端口為void void voidcamera_stop(void); voidcamera_regWrite(uint8_tdevice,uint8_taddress,uint8_tdata);voidvoidcamera_dma_isr(void);LPLD_eDMA_Cfg_t //DMA初始化結(jié)構(gòu)unsignedchar unsigned unsigned unsignedchar |名稱 |作用:延時(shí)函 void{intfor(i=0;i<300;{asm}}|名稱 |作用:開始信 Vvoid{}|名稱 |作用:停止信 void{}|名稱 |作用:字節(jié)寫 uint8_tcamera_sendByte(uint8_t{uint8_ti;for(i=0;i<8;{if(data&{}{}data<<=1;returnack;}|名稱 |作用:在指定設(shè)備的指定地址中寫入指定數(shù) {uint8_ti;for(i=0;i<20;{ack=if(ack==1{}if(ack==1){}if(ack==1){}if(ack==0)}}|名稱 |作用:頭所用IO口初始 void{ //選擇IOD作為數(shù)據(jù)口 //選擇PTE0作為觸發(fā) LPLD_GPIO_SetIsr(PORTB_BASE_PTR //PortB口的自定義中斷服序}|名稱 |作用:IOB雙邊沿觸發(fā)產(chǎn)生的中斷請(qǐng)求 void{if((PORTB_ISFR& )&&{ t== { //使能通1DMA請(qǐng) }}elseif(PORTB_ISFR& { t t= field_over= }}|名稱 |作用:DMA中斷設(shè) void{ t=={field_over= field_finish=}}|名稱 |作用:頭設(shè)置主函 void{ t=t=field_over //設(shè)置為通道1User_Def_DMA_Init.Peri_DmaReq=PORTE_DMAREQ; //IOE產(chǎn)生DMA請(qǐng)求User_Def_DMA_Init.Minor_loop_Length=IMAGE_COLUMNS; =(uint32_t)&GPIOD_PDIR;//設(shè)置源地址 //目的地址為 //初始化DMA模塊enable_irq //PORTB中斷PB1產(chǎn)生場(chǎng)中斷PB0產(chǎn)生行中}電機(jī)控制子程序#include"common#include"HAL_FTM#include#include#include"motor t=unsignedcharcar_state=constfloatmotor_I=constfloatmotor_D=constfloatspe=floatmotor_spe_ratio=charramp_speed_dec=intH_speed=intM_speed=intL_speed=voidcar_brake(int{if(car_state=={if(stop_delay_flag==0&¤t_speed>{}elseif(stop_delay_flag=={stop_delay_flag=}elseif(stop_delay_flag=={ t>{,, }{car_state=CAR_STOP;LCD_P6x8Str(0,3,"MotorStop");LPLD_GPIO_Init(PORTA_BASE_PTR,24,0,1,1);}}}}void{}|名稱 |作用:電機(jī)控 voidmotor(intspeed_error,unsignedchar{staticintconfig_speed;staticintstaticfloatmotor_deviation[2] staticfloatmotor_difference[2] staticintmotor_= staticunsignedcharcurrent_speed=(int)(LPLD_LPTMR_GetPulseAcc()*motor_spe_ratio);if(car_state=={}if(car_state=={}if(speed_error<speed_error=0;speed_error=if(speed_error<15)elseif(speed_error>70)elseconfig_speed=M_speed-if(ramp_state==config_speed-=config_speed_record[0]=config_speed;motor_deviation[1]=motor_deviation[0];motor_deviation[0]=config_speed-current_speed;motor_difference[0]=motor_deviation[0]-motor_+=motor_PID_ratio*(spe*(config_speed_record[0]-config_speed_record[1])motor_P*motor_difference[0]+motor_I*motor_deviation[0]+motor_D*(motor_difference[0]if(motor_>={,, if(motor_>motor_=}{,, if(motor_<-motor_=-LPLD_FTM0__ChangeDuty(1,motor_+}}舵機(jī)控制子程序#include"common#include"HAL_FTM#include"servo#define floatservo_P=floatservo_D=intservo_middle=#define#defineservo_middle-#defineservo_middle+|名稱||作用:| voidservo(int staticfloatservo_deviation[2]= //控制行偏1為當(dāng)前0為上一staticfloatservo_difference[2] servo_deviation[0]=servo_error;servo_difference[1]=servo_difference[0];servo_difference[0]=servo_deviation[0]-servo_deviation[1];servo_=servo_middle-(int)(servo_deviation[0]*servo_P+(servo_A*servo_difference[1]+(1-servo_A)*servo_difference[0])*servo_D);if(servo_>SERVO_MAX_R)servo_=SERVO_MAX_R;if(servo_<SERVO_MAX_L)servo_=SERVO_MAX_L;}系統(tǒng)初始化子程序#include"commonh"#include"HAL_GPIO.h"#include"HAL_FTMh"#include"search.h"#include"system.h"#include"servoh"#include"motorh"#include" yseh"intstart_line_delay;unsignedcharbutton_lock=#define #defineSWITCH_OFF#defineUP #define #define #define #define #define #defineSWITCH_1LPLD_GPIO_Get_b(PTA_BASE_PTR,16)#defineSWITCH_2LPLD_GPIO_Get_b(PTA_BASE_PTR,15)#defineSWITCH_3LPLD_GPIO_Get_b(PTA_BASE_PTR,14)#defineSWITCH_4LPLD_GPIO_Get_b(PTA_BASE_PTR,13)constunsignedcharcolor_code[8][3]=void{LPLD_GPIO_Set_b(PTA_BASE_PTR,5,1);}voidindicator_set(unsignedchar{if(color>7)}|名稱 |作用:鍵盤中 void{if(PORTA_ISFR==0x {button=ESC;button_lock=1;} {button=LT;button_lock=1;} {button=OK;button_lock=1;} {button=DN;button_lock=1;} {button=UP;button_lock=1;} {button=RT;button_lock=1;}}voidDis{,,,LPLD_GPIO_Init(PORTC_BASE_PTR16,0,2LPLD_GPIO_Init(PORTC_BASE_PTR,17,0,2,LPLD_GPIO_Init(PORTC_BASE_PTR,18,0,2,LPLD_GPIO_Init(PORTC_BASE_PTR,19,0,2,staticunsignedcharL_line_1,L_line_2,R_line_1,R_line_2,L_line,R_line;staticunsignedchargears=2;enable_irq{if(SWITCH_1==SWITCH_ON)if(SWITCH_2==SWITCH_ON)if(SWITCH_3==SWITCH_ON)if(SWITCH_4==SWITCH_ON)}{if(button_lock=={}}if(SWITCH_1==SWITCH_OFF)gotoswitch2;{L_line_1=LPLD_GPIO_Get_b(PTC_BASE_PTR,16);L_line_2=LPLD_GPIO_Get_b(PTC_BASE_PTR,17);R_line_1=LPLD_GPIO_Get_b(PTC_BASE_PTR,18);R_line_2=LPLD_GPIO_Get_b(PTC_BASE_PTR,19);L_line=L_line_1||L_line_2;R_line=R_line_1||if(L_line&&R_line)if(button_lock=={}}LCD_P6x8Str(0,3,"servomiddle:");{if(SWITCH_1==SWITCH_OFF)break;if(button_lock==1){button_lock=0;{caseUP:servo_middle+=25;break;caseDN:servo_middle-=25;break;caseRT:servo_middle+=5;break;caseLT:servo_middle-=5;break;}if(servo_middle>servo_middle=if(servo_middle<servo_middle=0;}{if(SWITCH_1==SWITCH_OFF)break;if(button_lock==1){button_lock=0;{caseUP:differential_threshold+=5;break;caseDN:differential_threshold-=5;break;caseRT:differential_threshold+=2;break;caseLT:differential_threshold-=2;break;}differential_threshold=30;}}if(SWITCH_2==SWITCH_ON)ramp_detect=1;elseramp_detect=if(ramp_detect=={LCD_P6x8Str(0,4,"Speeddec:");}{if(button_lock=={button_lock=0;if(button==ESC)gotoswitch1;if(ramp_detect==1){{caseUP:ramp_speed_dec+=25;break;caseDN:ramp_speed_dec-=25;break;caseRT:ramp_speed_dec+=5;break;caseLT:ramp_speed_dec-=5;break;}if(ramp_speed_dec>ramp_speed_dec=if(ramp_speed_dec<ramp_speed_dec=0;}gotoswitch2;}}if(ramp_detect=={LCD_P6x8Str(0,3,"up_time:");LCD_P6x8Str(0,5,"dn_time:");}{if(button_lock=={button_lock=0;if(button==ESC)gotoswitch1;if(ramp_detect==1){{caseUP:ramp_up_time+=2;break;caseDN:ramp_up_time-=2;break;caseRT:ramp_dn_time+=2;break;caseLT:ramp_dn_time-=2;break;}if(ramp_up_time<ramp_up_time=if(ramp_dn_time<ramp_dn_time=10;}gotoswitch2;}}if(SWITCH_3=={start_line_delay=3*60;gotoswitch4;}elsestart_line_delay=18*60;{if(button_lock=={button_lock=0;if(button==ESC)gotoswitch2;{caseUP:start_line_delay+=2*60;break;caseDN:start_line_delay-=2*60;break;caseRT:start_line_delay+=1*60;break;caseLT:start_line_delay-=1*60;break;}if(start_line_delay>100*start_line_delay=100*if(start_line_delay<3*start_line_delay=3*60;}}if(SWITCH_4==SWITCH_OFF){LCD_P6x8Str(0,3,"Real{if(button_lock=={{ca