車(chē)模直立方案

1、目錄年度電航學(xué)社2014PS：本資料僅限大連海事大學(xué)電航學(xué)社內(nèi)部交流使用目錄前言1硬件安裝與機(jī)械結(jié)構(gòu)2買(mǎi)到車(chē)模后(E車(chē)模)2陀螺儀與加速度計(jì)3電池固定方式3攝像頭桿子固定4加速度計(jì)與陀螺儀信號(hào)采集(模塊說(shuō)明&程序)4陀螺儀模塊簡(jiǎn)介(最開(kāi)始用的ENC03)4陀螺儀模塊簡(jiǎn)介(最終使用的L3G4200D)4加速度模塊簡(jiǎn)介(MMA7361)4MMA7361程序詳解&加速度計(jì)得到角度方式5L3G4200D程序詳解&陀螺儀積分得到角度方式7互補(bǔ)濾波-一種很好的控制思想8兩種獲取角度的方式及他們的特點(diǎn)8互補(bǔ)濾波思路圖解8電機(jī)控制10PID與車(chē)模直立原理12調(diào)整角度0位置值12直立原理簡(jiǎn)介12直立程序(就一句

2、話(huà))13調(diào)整角度零位值與為什么不用自動(dòng)校正13速度控制！讓車(chē)子自己走起來(lái)13龍丘編碼器簡(jiǎn)介14測(cè)速程序講解14車(chē)模方向控制17前言本次大賽基本是從0開(kāi)始做直立，其間有很多經(jīng)驗(yàn)，也有很多問(wèn)題，只是寫(xiě)出來(lái)能與大家分享，基本就是我的方法的敘述，不能算多么權(quán)威的方案，大家以后參考著做，是肯定能把車(chē)子立起來(lái)走起來(lái)的，但如果有更好的方法能讓車(chē)子走的更穩(wěn)定，一定要時(shí)刻更新！我盡量用最簡(jiǎn)單的話(huà)語(yǔ)表達(dá)實(shí)現(xiàn)車(chē)模直立的步驟，一般初學(xué)者肯定是可以跟著做出來(lái)的。再次強(qiáng)調(diào)一次，以后大家做車(chē)，跟著這個(gè)方案做出來(lái)后，肯定還有大量的時(shí)間嘗試更多的更好的算法或者思考出自己的更好的想法，一定要補(bǔ)充，這樣才能使我們的整體水平一屆比一

3、屆強(qiáng)吧。車(chē)模直立運(yùn)行從第七屆飛思卡爾開(kāi)始出現(xiàn)，到現(xiàn)在第九屆已經(jīng)3屆了，第七屆是電磁直立，第八屆光電直立，第九屆攝像頭直立，傳感器不同但對(duì)于車(chē)模的直立、速度、方向控制是完全相通的，我今年做的攝像頭組直立，如果以后的比賽出現(xiàn)了其他組別的直立大家仍然可以參考這個(gè)文檔使自己的車(chē)跑起來(lái)。直立車(chē)是一門(mén)大學(xué)問(wèn)，里邊有很多東西自己沒(méi)能弄清楚，還得等以后大家慢慢完善。這里說(shuō)明一些東西，為了閱讀方便和對(duì)應(yīng)程序，對(duì)陀螺儀加速度計(jì)方向做以下規(guī)定(這與硬件上的標(biāo)注也是對(duì)應(yīng)的)：l 1：測(cè)量車(chē)模傾角那個(gè)方向，就是直立PID所用的方向，稱(chēng)為Y軸(變量都是以Y標(biāo)定的，例如GYRO_Y就是這個(gè)方向的角速度)。l 2：測(cè)量車(chē)模轉(zhuǎn)

4、彎的那個(gè)方向，例如車(chē)模轉(zhuǎn)彎產(chǎn)生的角速度，稱(chēng)為X軸。硬件安裝與機(jī)械結(jié)構(gòu)一個(gè)原則就是重心越低越好，越集中越好，重心低會(huì)使控制更順滑，車(chē)模極限速度更高，抗干擾能力更強(qiáng)(速度越快，賽道顛簸越大也就是干擾越大，抗干擾能力直接影響著提速過(guò)程！)，而重心集中則會(huì)讓車(chē)模轉(zhuǎn)彎更加靈活，不需要太大的改變就可以完成較大的轉(zhuǎn)彎，但最好不要隨意增加配重，車(chē)子太沉?xí)绊懞芏鄸|西，我曾經(jīng)嘗試過(guò)使用配重將車(chē)子重心極限的降低(不需要開(kāi)電就直接立在那里，很重)，但帶來(lái)的影響就是轉(zhuǎn)彎很遲鈍，加速很慢(如果想要變速肯定是不利的)，最終還是將配重都取消掉了。機(jī)械結(jié)構(gòu)的調(diào)試對(duì)于智能車(chē)是很重要的，首頁(yè)那張圖便是今年拿冠軍的哈爾濱工業(yè)大學(xué)的

5、車(chē)子，他們的車(chē)據(jù)說(shuō)在實(shí)驗(yàn)室跑到了2.8ms，我的車(chē)最快才跑到過(guò)2.4，可以看到他們也是加了配重的，但沒(méi)有我一次性加的那么多，我的機(jī)械結(jié)構(gòu)并不是最優(yōu)的那一種，最終版本的車(chē)子過(guò)于輕了(基本就是和理工那一組差不多的結(jié)構(gòu)，但比他們的要輕，要低)，導(dǎo)致彎道跳輪，坡道飛起，而之前的是過(guò)于重了，導(dǎo)致過(guò)度遲鈍?？傊?，機(jī)械結(jié)構(gòu)建議最好在一開(kāi)始就仿照之前大牛成熟的結(jié)構(gòu)(如哈工大那種結(jié)構(gòu))，以上邊的原則定下來(lái)，這樣有助于長(zhǎng)時(shí)間的調(diào)試。我們今年的一大敗筆就是機(jī)械不停的改變方案，導(dǎo)致控制方案沒(méi)有太大的進(jìn)展。買(mǎi)到車(chē)模后(E車(chē)模)今年我們主要做的是E車(chē)模，為了不讓大家買(mǎi)太多的車(chē)模(囧)，有必要說(shuō)說(shuō)這車(chē)的坑爹之處:1：一定要

6、將所有的內(nèi)六角螺絲更換成十字螺絲或者淘寶上買(mǎi)航模級(jí)的那種優(yōu)質(zhì)內(nèi)六角螺絲(電子城李姐家斜對(duì)面，K6剛進(jìn)去左邊那家螺絲挺全的都能買(mǎi)到)，原車(chē)的螺絲質(zhì)量很差，很容易脫扣，對(duì)以后機(jī)械的調(diào)整很不利。2：固定電機(jī)的那兩個(gè)座和地盤(pán)連接的那10個(gè)螺絲(下圖)內(nèi)部很容易脫扣，不到迫不得已不要拆卸這兩個(gè)地方，大約拆裝5次左右這車(chē)就廢了！3： 電機(jī)安裝前看把輪子拆掉，看一下旋轉(zhuǎn)輪軸時(shí)候那個(gè)大的齒輪是不是圓的，如果不圓基本就是大齒輪上的四個(gè)螺絲沒(méi)有擰緊，一定要保證這個(gè)旋轉(zhuǎn)的順滑，基本上都擰緊就好了4：安裝電機(jī)的時(shí)候，那兩個(gè)螺絲千萬(wàn)不要直接擰，力氣一大螺絲會(huì)把塑料擠斷，要弄一個(gè)墊片！還有就是齒輪之間要有一定的空程(有一

7、點(diǎn)點(diǎn)即可，不能太大了)，否則兩個(gè)輪子阻力很難一樣，有空程其實(shí)無(wú)所謂，跑起來(lái)是不受任何影響的。5：關(guān)于安裝好電機(jī)后旋轉(zhuǎn)輪子產(chǎn)生的咔咔聲，如果你留了足夠的空程，大齒輪上的螺絲都緊過(guò)的話(huà)，這種聲音是很小的，基本可以忽略。有這種聲音基本就是兩個(gè)齒輪的母線(xiàn)不平行造成的。6：安裝編碼器的時(shí)候，店家給的齒輪與支架直接安裝是無(wú)法與大齒輪接觸的，需要適當(dāng)?shù)拇蚰ル姍C(jī)支架部分。就可以無(wú)壓力咬合了，齒輪咬合還是需要一點(diǎn)點(diǎn)空程。陀螺儀與加速度計(jì)這兩個(gè)東西我是用膠槍固定到攝像頭桿子上的，如下圖所示需要注意的是安裝陀螺儀時(shí)一定要保證陀螺儀Y軸與車(chē)模輪軸平行！否則會(huì)出現(xiàn)轉(zhuǎn)彎速度的改變，這個(gè)可以用推力軸承測(cè)試，把車(chē)模水平放置在

8、推力軸承上，旋轉(zhuǎn)車(chē)模，通過(guò)上位機(jī)觀察Y軸的角速度，在車(chē)模旋轉(zhuǎn)時(shí)候理論應(yīng)該只有X軸的角速度，不應(yīng)該有Y軸角速度的，如果有了一定是安裝斜了。這兩個(gè)東西要保持在一條豎直線(xiàn)上，豎直線(xiàn)要與車(chē)輪軸心相交，盡量保持在車(chē)子中間，這樣可以很大程度減少運(yùn)動(dòng)對(duì)信號(hào)采集的影響，具體怎么影響沒(méi)有理論上分析過(guò)，總之之前試過(guò)當(dāng)兩個(gè)不再一起，車(chē)子出現(xiàn)了劇烈抖動(dòng)，最后陀螺儀與加速度計(jì)還是保持了原來(lái)的計(jì)劃，如果有人可以從理論上解釋這倆該怎么安裝那一定要添加進(jìn)來(lái)?？傊褪且惭b的盡可能的正，能讓對(duì)應(yīng)的信號(hào)最舒服的采集。電池固定方式我用了兩種方式，一種是用鋁片彎折(這種鋁片是從大工的一個(gè)老爺爺那里要來(lái)的，就在大工體育場(chǎng)底下有一個(gè)金工

9、的老爺爺，人特別好，也很樂(lè)意幫助同學(xué)們，鋁片硬度適中很適合做支架，加工鋁片可以去大活弄，那里的設(shè)備比較多)，另一種是直接困扎在電路板上，都差不多，鋁片那種方式比較復(fù)雜，也不容易平衡，但調(diào)整好了重心會(huì)更低(見(jiàn)哈工大神車(chē)的固定方式)，或者就直接將驅(qū)動(dòng)板子開(kāi)的大一些，就和我們車(chē)一樣，驅(qū)動(dòng)&電源板也是地盤(pán)固定板和電池支架，我覺(jué)得這個(gè)想法還是不錯(cuò)的下圖是我的電池固定方案：攝像頭桿子固定這個(gè)沒(méi)什么說(shuō)的，原則就是不要讓它晃動(dòng)就好，各種三角形各種支架。圖：輪胎處理方式：1：淘寶上買(mǎi)軟化劑，各種店鋪都有，模型店，這里給個(gè)鏈接，非常給力的液體：2：打磨，盡量把紋路打磨少了，這個(gè)輪子外表皮不均勻，得用細(xì)砂紙打磨，具

10、體方法就是將輪子空轉(zhuǎn)，然后用砂紙放上去打磨，看我的都這個(gè)樣子了：3：填充，在輪轂里邊的兩條楞之間，放一個(gè)B車(chē)輪胎(用小刀修到合適大小正好放進(jìn)去的)，然后把海綿拆掉(否則輪子不圓！)。4：粘住，用3m膠水將輪胎與輪轂徹底粘死，這樣會(huì)讓輪子更結(jié)實(shí)，摩擦力更強(qiáng)膠水鏈接：這樣處理下來(lái)基本不會(huì)打滑了，我提速的瓶頸不像別人在摩擦力，摩擦力足夠大到翻車(chē)。加速度計(jì)與陀螺儀信號(hào)采集(模塊說(shuō)明&程序)陀螺儀模塊簡(jiǎn)介(最開(kāi)始用的ENC03)淘寶購(gòu)買(mǎi)鏈接：這算是使用最成熟的飛思卡爾陀螺儀了，最一開(kāi)始也是用它做的，輸出模擬量，操作很簡(jiǎn)單，直接供電后用示波器就可以觀察波形。說(shuō)到程序就直接AD采集唄，但缺點(diǎn)也很多，溫漂和線(xiàn)

11、性度都不如大多數(shù)數(shù)字陀螺儀好。尤其溫漂很大。不建議使用，但這個(gè)用著真是簡(jiǎn)單，特別方便。所以還是提一下了。陀螺儀模塊簡(jiǎn)介(最終使用的L3G4200D)淘寶購(gòu)買(mǎi)鏈接：便宜、木有溫漂，總之這個(gè)陀螺儀特別好，我們是在比賽的中后期換的，使用模擬I2C通信(無(wú)視上邊的SPI接口吧，我們用SDA和SCL那兩根線(xiàn)和電源線(xiàn)接入單片機(jī)就可以了)，換了以后車(chē)子運(yùn)行明顯更加平穩(wěn)了(應(yīng)該是線(xiàn)性度的問(wèn)題)，建議直接使用這個(gè)型號(hào)的陀螺儀吧，親測(cè)很給力。加速度模塊簡(jiǎn)介(MMA7361)淘寶購(gòu)買(mǎi)鏈接：這個(gè)也很簡(jiǎn)單，配置兩路AD即可，需要注意的是這貨有個(gè)使能端，就是SL那個(gè)管腳，要給他接到高才能用。否則啥都采集不到的MMA736

12、1程序詳解&加速度計(jì)得到角度方式說(shuō)明一下我的程序?qū)懙貌皇且?guī)范，代碼全部都分布在兩個(gè)文件中，只是為了自己調(diào)整的方便，沒(méi)有考慮可讀性神馬的，代碼基本上都在main.c中，只有Get_Ang函數(shù)(得到角度、調(diào)整PID)和Calculate_Speed函數(shù)(測(cè)速)還有控制電機(jī)的函數(shù)Go_S在Read_Data.c中。首先是加速度計(jì)的AD采集：第一步：在初始化部分中，初始化ADadc_init (ADC1_SE6a); /初始化ADC1 采集陀螺儀和加速度計(jì)傳回來(lái)的值adc_init (ADC1_SE7a);第二步：配置一個(gè)定時(shí)器，用于定時(shí)采集并計(jì)算角度、速度等值pit_init_ms(PIT0, 1)

13、; /初始化PIT0，定時(shí)時(shí)間為： 1msset_vector_handler(PIT0_VECTORn ,PIT0_IRQHandler); /設(shè)置PIT0的中斷服務(wù)函數(shù)enable_irq (PIT0_IRQn); /使能PIT0中斷PS：這是野火最新庫(kù)的中斷初始化方式。第三步：在定時(shí)器中斷服務(wù)函數(shù)中，運(yùn)行Get_Ang函數(shù)計(jì)算角度，在這一步我們先只是采集加速度計(jì)的值Get_Ang();/1ms調(diào)整一次姿態(tài)第四步：編寫(xiě)Get_Ang函數(shù)(在我的工程中，這個(gè)函數(shù)在Read_Data.c中)，在這個(gè)函數(shù)中運(yùn)行AD采集函數(shù)，得到兩路加速度的值，并用atan2函數(shù)(在math.h中)計(jì)算通過(guò)加速度

14、計(jì)得到的角度值。PS：計(jì)算角度，有幾種方法，首先說(shuō)一下math.h中，atan函數(shù)原型是double atan(double x)，就是直接求x的反正切，還有一個(gè)atan2，它的原型是double atan2(double y, double x)， 求角度方法如下圖，如果粉色是z軸，藍(lán)色是x軸，豎直線(xiàn)為0位置，當(dāng)車(chē)模處于0位置時(shí)候，x軸為0，z軸為g，那么當(dāng)它有角度時(shí)，可用以下方式求角度：1:ang=arctan(x/z)2:ang=arcsin(x/g)3:ang=arccos(z/g)用第二種和第三種要求ad采集到的值再乘以一個(gè)系數(shù)或者自己設(shè)置g的值才可以求出角度，而第一種直接ad值就可

15、以用了，而且用上了兩個(gè)方向的傳感器的值，也會(huì)更準(zhǔn)一些，所以我用了atan2函數(shù)。uint16 ADresult; /保存ADC轉(zhuǎn)換結(jié)果/* 讀取一次 */ADresult = adc_once(ADC1_SE6a, ADC_16bit); /讀取 ADC1_SE6a ，16位精度A_X=ADresult-22000;ADresult = adc_once(ADC1_SE7a, ADC_16bit); /讀取 ADC1_SE7a ，16位精度A_Z=ADresult-28000;PS：AD減去那個(gè)值是實(shí)驗(yàn)出來(lái)的，當(dāng)加速度計(jì)水平時(shí)讀數(shù)應(yīng)該為0，而AD采集的范圍是0到65535，所以就要減去一個(gè)系數(shù)

16、嘍。然后計(jì)算角度：Ang_Acc=173*atan2(A_X,A_Z);/角度 PS:前邊乘著的那個(gè)系數(shù)是將這個(gè)數(shù)放大一下讓他基本符合角度，可以叫做靈敏度系數(shù)，由于atan2出來(lái)的值是弧度，太小了不利于和一些整型數(shù)計(jì)算所以才這么弄。L3G4200D程序詳解&陀螺儀積分得到角度方式L3G4200D通信采用I2C協(xié)議，具體協(xié)議沒(méi)有必要背會(huì)，直接調(diào)用成熟的模塊即可，我們用的是大工寫(xiě)的IIC模塊，其間有些小插曲，之前用自己以前在STM32上用的IIC程序沒(méi)調(diào)出來(lái)，不知道為什么，然后大工的車(chē)友幫著調(diào)，用他的程序就調(diào)好了，我看了看真沒(méi)啥區(qū)別，就是IO口用的不一樣，他用的我板子上用作串口的那兩個(gè)口(A14和

17、A15)，我就試著把管腳換到之前預(yù)留的IO口(之前用模擬陀螺儀，AD口，E2和E3)上，然后怪了，又出問(wèn)題了，讀數(shù)很奇怪，要么是-1，要么是亂七八糟的數(shù)字，結(jié)論就是那倆IO口有問(wèn)題，不知道為啥！換了IO口就好了！然后解決方案就是把加速度那兩個(gè)口用來(lái)i2c了，陀螺儀那兩個(gè)口還是用作AD，接了模擬加速度計(jì)。IIC移植完畢后直接調(diào)用函數(shù)，就可以采集到X 與Y 方向的角速度值了：GYRO_X=Get_Gyro(1,X)-30; /用于轉(zhuǎn)彎的角速度GYRO_Y=Get_Gyro(1,Y)-20; 直接調(diào)用Get_Gyro函數(shù)即可，后邊的常數(shù)是0偏值。由于是模擬IIC，運(yùn)行速度取決于IIC.h中最上邊no

18、p5與nops后邊nop指令的數(shù)量(延時(shí))，延時(shí)越短，運(yùn)行越快。默認(rèn)的這么多已經(jīng)足夠快了。得到角速度后，乘一個(gè)系數(shù)得到真正的角速度后積分：Ang_IGyro=Ang_IGyro+GYRO_Y*0.00017;/積分得到的角度這個(gè)系數(shù)也是通過(guò)上位機(jī)調(diào)出來(lái)的，直到積分出來(lái)的角度大致符合真實(shí)的角度。至此，兩種角度值就都獲得了，然后想要得到更精準(zhǔn)的角度值需要經(jīng)過(guò)互補(bǔ)濾波。PS：模擬陀螺儀的采集方式和模擬加速度計(jì)采集一樣的，這里就不敘述了?；パa(bǔ)濾波-一種很好的控制思想兩種獲取角度的方式及他們的特點(diǎn)使用加速度計(jì)獲得角度(上文中的Ang變量)，毛刺很多，而且受運(yùn)動(dòng)影響極大，一旦運(yùn)動(dòng)起來(lái)，加速度計(jì)會(huì)將運(yùn)動(dòng)的震

19、動(dòng)體現(xiàn)出來(lái)，毛刺更是多，如左邊圖：加速度計(jì)獲得的角度波形之前調(diào)試時(shí)沒(méi)有截圖，便取材其它資料了，此圖來(lái)自第七屆官方直立方案，這幅圖中藍(lán)色就是通過(guò)加速度計(jì)獲得的角度波形，紅色是實(shí)際的角度，一旦運(yùn)動(dòng)，毛刺會(huì)更多，無(wú)法直接用它控制電機(jī)的。而用角速度積分得到的角度，則沒(méi)有這么多的毛刺，也不會(huì)受到運(yùn)動(dòng)的影響，但是陀螺儀有溫漂，角速度的靜偏差，導(dǎo)致這個(gè)角度值會(huì)一直不停地增大或者減小互補(bǔ)濾波思路圖解 Ang_Acc整體趨勢(shì)的正確使其可以準(zhǔn)確反映角度變化的正確走勢(shì)，于是將其與Ang_IGyro做差就得到了一個(gè)誤差值，這個(gè)值就是陀螺儀此時(shí)的靜誤差，將這個(gè)誤差值修復(fù)到Ang_IGyro上，得出來(lái)的角度值就相對(duì)準(zhǔn)確了

20、，這就是互補(bǔ)濾波的思路，效果如下圖：黃色是Ang_Acc紅色是濾波后的輸出，這個(gè)角度完全可以用作PID的位置值了。PS：官網(wǎng)方案與之略有不同，偏差值不是直接修復(fù)到角度上，而是修復(fù)到陀螺儀采集值，效果是一樣的，官網(wǎng)的框圖是比較規(guī)范的控制流程圖，如下圖：下面是我的程序，直接用偏差修復(fù)角度的，整體來(lái)說(shuō)還是比較簡(jiǎn)單的：/互補(bǔ)濾波Ang_Error=Ang_Acc-Ang_IGyro;Ang_IGyro=Ang_IGyro+Ang_Error*0.0005;/綠的那個(gè)參數(shù)越大曲線(xiàn)跟隨效應(yīng)越好但是毛刺越多需要調(diào)Ang=Ang_IGyro;PS:互補(bǔ)濾波在1ms中斷函數(shù)中，實(shí)時(shí)得出角度得到Ang_Error

21、后，將其乘以一個(gè)系數(shù)修復(fù)到Ang_IGyro上，就可以消除Ang_IGyro的靜差了，那個(gè)系數(shù)越小，曲線(xiàn)修復(fù)的越少，也就是說(shuō)反應(yīng)越遲鈍，曲線(xiàn)越圓滑，當(dāng)車(chē)子有突然變化時(shí)，角度不是很突然的改變，而是慢慢的變化過(guò)去，當(dāng)系數(shù)較大時(shí)候，最終曲線(xiàn)更加跟隨加速度曲線(xiàn)，當(dāng)車(chē)子有突然變化時(shí)候，角度也會(huì)很快的改變過(guò)去，但有時(shí)候就像加速度計(jì)一樣有一些毛刺，大家做的時(shí)候可以試試這兩種特點(diǎn)的曲線(xiàn)，建議這個(gè)值還是小一些，這樣曲線(xiàn)會(huì)更加的平穩(wěn)，陀螺儀積分得到的角度成分更大，而這個(gè)算法是一個(gè)積累過(guò)程，在上電時(shí)只要加速度計(jì)得出的角度也在0附近(將車(chē)子擺到平衡位置上電即可)，很快曲線(xiàn)就可以跟隨過(guò)去。如果上電時(shí)沒(méi)有擺正，在上電的一

22、瞬間加速度得出的角度是其真實(shí)角度，而陀螺儀從0開(kāi)始積分，得出的角度是不準(zhǔn)確的，需要互補(bǔ)濾波中加速度計(jì)的成分逐漸將其修復(fù)過(guò)去，此處大家可以通過(guò)上位機(jī)看看波形我就不一一截圖了，還是比較簡(jiǎn)單的。電機(jī)控制使用K60內(nèi)部FTM模塊發(fā)出PWM波控制電機(jī)，對(duì)于直立車(chē)推薦使用成熟的電機(jī)驅(qū)動(dòng)芯片，因?yàn)檫@些芯片線(xiàn)性度更好，更穩(wěn)定，沒(méi)有必要像趴著的車(chē)那樣追求加速性能而用MOS管自己搭H橋電路，4塊BTN7971就足夠穩(wěn)定足夠用了。 這里需要看一下K60庫(kù)中對(duì)FTM的一些描述，電機(jī)控制需要協(xié)調(diào)好管腳的，控制電機(jī)的PWM頻率是3Khz(不知道是不是最佳，最優(yōu)頻率需要根據(jù)電機(jī)參數(shù)計(jì)算的，反正這個(gè)值科學(xué))，只需要一種頻率，

23、而需要4種不同的占空比，所以控制兩個(gè)電機(jī)只需要一個(gè)FTM模塊，一個(gè)擁有4個(gè)通道輸出的FTM模塊，而K60的FTM模塊如下(在FIRE_PORT_cfg.h中):/* FTM */ 模塊通道 端口 可選范圍 建議#define FTM0_CH0 PTC1 /PTC1、PTA3 PTA3不要用（與Jtag、SWD沖突）#define FTM0_CH1 PTA4 /PTC2、PTA4#define FTM0_CH2 PTA5 /PTC3、PTA5#define FTM0_CH3 PTA6 /PTC4、PTA6#define FTM0_CH4 PTA7 /PTD4、PTA7#define FTM0_C

24、H5 PTD5 /PTD5、PTA0 PTA0不要用（與Jtag、SWD沖突）#define FTM0_CH6 PTD6 /PTD6、PTA1 PTA1不要用（與Jtag沖突）#define FTM0_CH7 PTD7 /PTD7、PTA2 PTA2不要用（與Jtag沖突）/ 模塊通道 端口 可選范圍 建議#define FTM1_CH0 PTA8 /PTA8、PTA12、PTB0#define FTM1_CH1 PTA13 /PTA9、PTA13、PTB1/ 模塊通道 端口 可選范圍 建議#define FTM2_CH0 PTB18 /PTA10、PTB18#define FTM2_CH1

25、PTA11 /PTA11、PTB19/正交解碼模塊通道 端口 可選范圍 建議#define FTM1_QDPHA PTA12 /PTA8、PTA12、PTB0#define FTM1_QDPHB PTA13 /PTA9、PTA13、PTB1#define FTM2_QDPHA PTA10 /PTA10、PTB18#define FTM2_QDPHB PTA11 /PTA11、PTB19從上面能看出FTM0有8個(gè)通道(！),而FTM1和FTM2都是各有2個(gè)，所以FTM0正好可以用來(lái)控制電機(jī)，而FTM1和FTM2以后用來(lái)測(cè)速更合適。然后就是首先初始化FTM0的4個(gè)通道： FTM_PWM_init(

26、FTM0, FTM_CH4, 3000, 0); /四路pwm去電機(jī) FTM_PWM_init(FTM0, FTM_CH1, 3000, 0); FTM_PWM_init(FTM0, FTM_CH2, 3000, 0); FTM_PWM_init(FTM0, FTM_CH3, 3000, 0);初始化為3k，%0，這個(gè)比較簡(jiǎn)單不多說(shuō)，初始化了后就可以通過(guò)使用FTM_PWM_Duty函數(shù)改變各個(gè)通道的頻率來(lái)控制電機(jī)的轉(zhuǎn)速了，我編寫(xiě)了Go函數(shù)，接受一個(gè)數(shù)字，如果是正正轉(zhuǎn)，負(fù)則反轉(zhuǎn)，方便用PID調(diào)整時(shí)直接出來(lái)的值就可以給到函數(shù)中去做出正確的運(yùn)動(dòng)。Go函數(shù)中的代碼是：If(Speed0) FTM_PW

27、M_Duty(FTM0,FTM_CH2,(uint32)Speed); FTM_PWM_Duty(FTM0,FTM_CH4,(uint32)Speed); FTM_PWM_Duty(FTM0,FTM_CH3,0); FTM_PWM_Duty(FTM0,FTM_CH1,0); else if(Speed=0) FTM_PWM_Duty(FTM0,FTM_CH2,0); FTM_PWM_Duty(FTM0,FTM_CH4,0);FTM_PWM_Duty(FTM0,FTM_CH3,0+(uint32)(0-Speed); FTM_PWM_Duty(FTM0,FTM_CH1,0+(uint32)(0-

28、Speed); 嗯，就是這些，這樣就可以動(dòng)了，至于硬件連線(xiàn)就不用多說(shuō)了吧。PID與車(chē)模直立原理調(diào)整角度0位置值在直立之前，需要先調(diào)整一下角度的0位置值，更改一下上面的加速度計(jì)獲取角度那句話(huà)：Ang_Acc=173*atan2(A_X,A_Z) -Ang_Set;/角度 Ang_Set是一個(gè)常數(shù)，可以用小鍵盤(pán)調(diào)整，調(diào)整方式后邊有解釋?zhuān)@個(gè)變量的作用主要是修復(fù)加速度計(jì)的安裝誤差，如果加速度計(jì)的豎直方向沒(méi)有與車(chē)模平衡位置豎直方向平行，那么車(chē)模處在平行位置時(shí)測(cè)得的角度不是0，所以需要加上一個(gè)變量將這個(gè)誤差修復(fù)，而實(shí)際上由于器件難以避免的溫漂，這個(gè)值是需要經(jīng)常調(diào)整的，否則會(huì)造成車(chē)子直立時(shí)的前進(jìn)或后退，也

29、會(huì)影響車(chē)模運(yùn)行的速度與設(shè)定速度產(chǎn)生靜誤差。這一步雖然重要但相當(dāng)簡(jiǎn)單，只需要將車(chē)模擺在平衡位置，看一下上位機(jī)就能知道這個(gè)值取什么了。最終的結(jié)果就是車(chē)模處在平衡位置時(shí)角度輸出要是0。直立原理簡(jiǎn)介這里再簡(jiǎn)單說(shuō)一下車(chē)?？梢灾绷⒌脑?，還是比較好動(dòng)的，官網(wǎng)也有詳細(xì)的理論分析，我就幾句話(huà)講一下這原理吧，沒(méi)有那么復(fù)雜那么亂的，理解一下也方便調(diào)試：首先大家想一想單擺，為什么空氣中的單擺拉起來(lái)后可以很快的靜止下來(lái)。這里邊有兩個(gè)條件，第一是有一個(gè)回復(fù)力拉著他向平衡位置運(yùn)動(dòng)。而只有這一個(gè)條件是不夠的，它會(huì)不停的做往復(fù)運(yùn)動(dòng)，還有一個(gè)條件就是空氣阻力，是因?yàn)樽枇Φ拇嬖诓攀箚螖[停下來(lái)。而我們的車(chē)?；旧暇褪且粋€(gè)倒過(guò)來(lái)的單

30、擺了，如果想讓車(chē)模直立住(停留在平衡位置)，需要一個(gè)回復(fù)力，而此時(shí)重力提供的回復(fù)力是遠(yuǎn)離平衡位置的，也就是和我們想要的力 相反，于是這就需要電機(jī)提供一個(gè)回復(fù)力了，這個(gè)力就是kp*Ang。如果大家把這個(gè)值直接給電機(jī)，你會(huì)發(fā)現(xiàn)當(dāng)kp值合適時(shí)，車(chē)子會(huì)立住的(機(jī)械結(jié)構(gòu)要好要不然很難調(diào))，用手給車(chē)模一個(gè)干擾，它會(huì)前后擺動(dòng)，最終再次回到平衡?；氐狡胶獾脑砭褪撬幸粋€(gè)回復(fù)力了，而空氣和機(jī)械摩擦又提供了一點(diǎn)點(diǎn)的阻尼力，所以他可以震蕩著回去。阻力的作用就是減速，剛才只加了kp*Ang，阻力完全靠空氣和機(jī)械摩擦，如果我們?cè)侔裬d*GYRO_Y加到電機(jī)上就相當(dāng)于用電機(jī)提供了一個(gè)令其減速的阻尼力，這就是D，當(dāng)D合適

31、時(shí)，車(chē)子就根本倒不了了，最終的效果就是只要輪子在地上它肯定不會(huì)倒否則肯定是程序有bug了。上面就是簡(jiǎn)單的原理簡(jiǎn)介，看過(guò)后應(yīng)該大致能知道直立車(chē)直立的原理了，詳細(xì)的理論分析請(qǐng)見(jiàn)第七屆官方直立指南，我這個(gè)是白話(huà)，人家那個(gè)才專(zhuān)業(yè)呢。直立程序(就一句話(huà))將上面的那些寫(xiě)成程序就是這一句話(huà)了，同樣放在1ms中斷中，1ms調(diào)整一次姿態(tài)：Speed_L = (0-Ang)*35 - GYRO_Y*0.27;/直立PIDGo(Speed_L);這就是讓車(chē)子直立起來(lái)的那句話(huà)，35是kp，0.27是kd，調(diào)整這兩個(gè)參數(shù)即可讓車(chē)模非常穩(wěn)定的直立。調(diào)整方式：首先將kd設(shè)為0，調(diào)整kp使車(chē)子能夠直立，然后用手按倒它可以很快

32、的立起來(lái)再反復(fù)震蕩幾次再立在哪里。Kp其實(shí)很小就可以立住，然后給kd，直到調(diào)整到你覺(jué)得很穩(wěn)定為止。對(duì)于直立應(yīng)該硬還是軟，我是取個(gè)折中，因?yàn)槎加泻锰?，不是越硬越好的。調(diào)整角度零位值與為什么不用自動(dòng)校正這里在說(shuō)一下自己調(diào)了一星期的自動(dòng)校正零位，最后沒(méi)有成功：首先這個(gè)校正需要?jiǎng)傞_(kāi)始就擺到很正的位置，讀取一個(gè)角度值后設(shè)定為Ang_Set，校正后如果一開(kāi)始拜訪(fǎng)的有誤差，車(chē)子角度略有誤差而摩擦力又足夠讓其停在賽道上，這個(gè)誤差就無(wú)法消除，所以在這里不建議大家搞自動(dòng)校正，除非有更好的算法能徹底消除器件的溫度漂移。用兩個(gè)按鍵來(lái)調(diào)整是最好最方便的。速度控制！讓車(chē)子自己走起來(lái)直立車(chē)的速度控制如果從理論的角度分析會(huì)很

33、復(fù)雜，具體的理論分析參見(jiàn)第七屆官方直立指南，我這里只是說(shuō)一下程序中的實(shí)現(xiàn)方法，大家照著這個(gè)來(lái)即可，還有一些我自己的調(diào)試經(jīng)驗(yàn)，理論部分我也沒(méi)弄清楚，真的比較復(fù)雜，大家有興趣可以去研究，實(shí)現(xiàn)方式其實(shí)也不難，首先是要得到兩個(gè)輪子的轉(zhuǎn)速:龍丘編碼器簡(jiǎn)介淘寶購(gòu)買(mǎi)鏈接：注意要一并買(mǎi)對(duì)應(yīng)的齒輪與支架，要不不好安裝，這種編碼器的好處一是體積小，質(zhì)量輕，二是有一個(gè)管腳直接輸出高低來(lái)表示旋轉(zhuǎn)方向，不需要配置正交解碼，具體管腳如下：只有前四個(gè)管教有用。旋轉(zhuǎn)方向輸出高低電平表示了旋轉(zhuǎn)方向。測(cè)速程序講解也是用k60的FTM模塊，這時(shí)候用FTM1和FTM2(PS，測(cè)速不能用一個(gè)模塊的兩個(gè)通道，因?yàn)橛玫氖撬牟东@模塊)。首

34、先初始化兩路FTM和中斷函數(shù)：FTM_Input_init(FTM1, FTM_CH0, FTM_Falling); /初始化FTM輸入捕捉模式（用于第一路測(cè)速） port_init_NoALT(FTM1_CH0 ,PULLUP); set_vector_handler(FTM1_VECTORn ,FTM1_INPUT_IRQHandler);/設(shè)置FTM1的中斷服務(wù)函數(shù) enable_irq (FTM1_IRQn); FTM_Input_init(FTM2, FTM_CH0, FTM_Rising); /初始化FTM輸入捕捉模式（用于第二路測(cè)速） port_init_NoALT(FTM2_C

35、H0 ,PULLUP); set_vector_handler(FTM2_VECTORn ,FTM2_INPUT_IRQHandler);/設(shè)置FTM2的中斷服務(wù)函數(shù) enable_irq (FTM2_IRQn);初始化兩個(gè)IO口用來(lái)讀取旋轉(zhuǎn)方向：gpio_init (PTB10, GPI, 0); /初始化 PTB10(第一路測(cè)速方向位)gpio_init (PTB11, GPI, 0); /初始化 PTB11(第二路測(cè)速方向位)然后編寫(xiě)兩路輸入捕捉的中斷函數(shù)(以一路為例)：void FTM1_INPUT_IRQHandler(void) /第一路測(cè)速，右輪 uint8 s = FTM1_S

36、TATUS; /讀取捕捉和比較狀態(tài) uint8 CHn; FTM1_STATUS = 0x00; /清中斷標(biāo)志位 CHn = 0; if( s & (1 CHn) ) /* 用戶(hù)任務(wù) */ if(gpio_get(PTB10) RightWheel_Count-; else RightWheel_Count+; ; /*/ 上面的函數(shù)里邊，關(guān)鍵只有一句話(huà)，就是讓RightWheel_Count自增或自減，這個(gè)變量是全局變量，兩路測(cè)速不停的捕獲，配合定時(shí)器中斷服務(wù)函數(shù)中的速度計(jì)算函數(shù)就可以不停的得出車(chē)輪轉(zhuǎn)動(dòng)速度了，同樣用1ms定時(shí)器中斷來(lái)計(jì)算，不過(guò)設(shè)置了時(shí)間標(biāo)志位，50ms測(cè)速一次，在1ms中斷

37、服務(wù)函數(shù)中的代碼如下：time_counter1+; /時(shí)間標(biāo)志位 if(50=time_counter1) /50ms測(cè)一次速度 Calculate_Speed(); /測(cè)速函數(shù)，展開(kāi)是：Check_Speed_Left=(LeftWheel_Count+RightWheel_Count)/2; /平均左右輪的數(shù)據(jù)，得到切向的速度 LeftWheel_Count=0; /清零測(cè)速數(shù)據(jù)，等于是每50ms計(jì)數(shù)一次 RightWheel_Count=0; D_Check_Speed_Left=Check_Speed_Left-Check_Speed_Left_Last; /速度的D Check_Speed_Left_Last=Check_Speed_Left; /儲(chǔ)存本次速度，下次用來(lái)算D I_Check_Speed_Left+=Check_Speed_Left; /速度的