【正文】 =1。fpll=fvco/(2*POSTDIV)。 //_asm匯編語言，nop做空操作；延時(shí) _asm(nop)。 //16位定時(shí)器初值設(shè)定 time_out period = (79+1)*(799+1)/64000000 = 1ms PITINTE_PINTE0 = 1。 //設(shè)置波特率為38400Kbps(寫一個(gè)字) AMAP = 0 總線頻率64MHz //SCI0BD = 139。 //發(fā)送數(shù)據(jù) }}/****************************************************功能 :初始化定時(shí)器模塊(TIM模塊輸入撲捉功能初始化) Ic0上升沿 , Ic1輸入撲捉下降沿入口參數(shù):無出口參數(shù):無******************************************************/void Time_init_CCD(void){ //ok() TSCR1 |= 0X90。 //PWM0,1選擇ClockA作為時(shí)鐘,PWM2,3選擇ClockB作為時(shí)鐘 PWMPRCLK = 0X00。 //SB的分頻因子為40 ClockSB=1600KHz PWMPER7 = 10。 //全部使用模擬輸入 ATD0CTL1 = 0X00。 for(j=0。}//*********中斷代碼執(zhí)行選擇標(biāo)志 unsigned char state_flag=0。 extern unsigned char runflag。 //PAC 初始化 PIT_Init()。int g_chrRightPulsesum = 0。/*************************************************************** ok____5月4日函數(shù)功能:獲取電機(jī)脈沖數(shù)入口參數(shù):無讀取計(jì)數(shù)器的值出口參數(shù):無g_inMotorPulse 正反計(jì)數(shù)脈沖總數(shù)小于128說明 :********************************************************************/ void GetMotorPulse(void) //1ms { g_chrleftPulse = (char)PORTA。 Leftcounter_S1=1。 g_chrleftPulsesum=0。 //積分環(huán)節(jié)比例控制 if(g_inSpeedIntegral =Speed_Motor_Max) //積分限幅 g_inSpeedIntegral = Speed_Motor_Max。 //控制次數(shù)增加一次 if(g_inSpeedperiod==20) { g_inSpeedperiod=0。 //轉(zhuǎn)換的角度(90~90)extern void MotorOutput(float)。 g_inAngleOut =(CAR_ANGLE_SET g_fCarAngle) * ANGLE_CONTROL_P + (CAR_ANGLESPEED_SET kalmanangle_dot) * ANGLE_CONTROL_D。 if(rightMotor =0) rightMotor =rightMotor + DEATH_CONTROL。 } if(leftMotor =0) //車模向后傾斜,角度為正,電機(jī)反轉(zhuǎn) { PORTK_PK2 = 0。 //等待IC0撲捉到CLK的上升沿 x = TC0。 //等待IC0撲捉到CLK的上升沿 x = TC0。 //等待IC1撲捉到CLK的下降沿 x= TC1。i=128。 //等待IC1撲捉到CLK的下降沿 x= TC1。 //等待IC1撲捉到CLK的下降沿 x= TC1。 //清除標(biāo)志位 //******啟動(dòng)采集CCD數(shù)據(jù)輸出,并同步時(shí)序 CCD_SI2 = 0。 //短暫延時(shí) CCD_SI2 = 0。 //等待IC0撲捉到CLK的上升沿 x = TC2。 Sendflag2 = 0。 //等待IC0撲捉到CLK的上升沿 x = TC2。 //等待IC1撲捉到CLK的下降沿 ATD0CTL5 = 0X0B。 //短暫延時(shí) CCD_SI2 = 1。 int x。 //等待IC1撲捉到CLK的下降沿 x= TC1。 x= TC1。 //等待IC0撲捉到CLK的上升沿 x = TC0。 //等待IC0撲捉到CLK的上升沿 x = TC0。 //等待IC0撲捉到CLK的上升沿 x = TC0。 } }/****************************************************************程序功能:采集CCD1數(shù)據(jù),通過上位機(jī)生成圖像 下面鏡頭入口參數(shù):無出口參數(shù):unsigned char uchr_ccd_data(128)共128個(gè)數(shù)據(jù),其性質(zhì)為全局?jǐn)?shù)組電氣連接:PWM7CCD_CLK PORTE_PE2CCD_SI CCD_AOAN10 850us (OK)******************************************************************/define CCD_SI PORTK_PK4 //定義CCD操作接口void CCD_Collect(void){ //*********定義發(fā)送數(shù)據(jù)數(shù)組,并初始化起始位 int i=0。 } if(rightMotor =0) //車模向后傾斜,角度為正,電機(jī)反轉(zhuǎn) { PORTK_PK3 = 0。 leftMotor = duty + g_inDirControlOut。 //選擇通道8 采集角度 g_inCarAngle = ATD_uchar_convert() 82。float g_inAngleOut。 //測量值每100ms更新一次 }/***************************************************************************函數(shù)功能:將每次更新的控制量按比例輸出控制每5ms調(diào)用一次 速度差最大時(shí),取控制量的1/20,然后依次增加11/20,2/20,3/20...20/20入口參數(shù):無全局變量g_inSpeedControlOutOld, g_inSpeedControlOutNew出口參數(shù):無全局變量g_inSpeedControlOut速度控制的PWM輸出****************************************************************************/ void speed_control(void) { float inValue 。 // 加速，加速 //} inDelta = (CAR_SPEED_SET g_inCarSpeed)。 g_chrRightPulsesum= g_chrRightPulsesum + (int)g_chrRightPulse。 //左計(jì)數(shù)器清零 g_chrRightPulse = (char)PORTB。 //比例積分項(xiàng)float g_inSpeedOutNew=0。 //速度控制中的比例控制,用于反饋比例反饋速度差信號(hào)//float SPEED_CONTROL_D =53。 //SCI0初始化 ok ATD_init()。 //靜態(tài)采集點(diǎn) int icount。 //數(shù)碼管的表 PTM = ShuMa[i]。 //采樣時(shí)間為ATD時(shí)鐘周期的4倍,ATD時(shí)鐘為系統(tǒng)時(shí)鐘的10分頻 ATD0CTL5 = 0X20。 //設(shè)置占空比為50% PWME = 0X0f。 //PWM頻率為4KHz PWMPER23 = 15000。 //IC0,IC2上升沿 , IC1,IC3輸入撲捉下降沿 TIE = 0X00。 for(i=0。 //設(shè)置波特率為19200Kbps(寫一個(gè)字) AMAP = 0 總線頻率32MHz //SCI0BD = 208。 // PIT disabled, PITCE_PCE0 = 1。 //fbus = 2*16*(9+1)/(1+1)=80m。 }//*****************設(shè)置系統(tǒng)時(shí)鐘為64MHzvoid SystemCLK_64M(void){ CLKSEL = 0。 //全部禁止上拉電阻 DDRK_DDRK2 =1。 PORTE_PE5 = 0。解決方法：增大Kg。 直立、速度閉環(huán)控制框圖第七章車模參數(shù)調(diào)試圖解在完成了所有控制算法方案的制定，以及程序代碼的編寫之后，接下來需要對(duì)一些參數(shù)在實(shí)際系統(tǒng)運(yùn)行中進(jìn)行整定以及微調(diào)。但根據(jù)實(shí)際經(jīng)驗(yàn)，是可以通過速度控制進(jìn)行車模傾角控制的。在重力的作用下，車模就會(huì)朝傾斜的方向加速前進(jìn)。因此最后我們選擇了PD控制器。首先我們忽略速度控制，僅僅只考慮角度閉環(huán)系統(tǒng)。同時(shí)，無論是控制角度還是控制電機(jī)，執(zhí)行機(jī)構(gòu)都為電機(jī)，所以系統(tǒng)也具有強(qiáng)耦合性。解決方法，逐漸增加dt即可。首先是靜態(tài)整定。需要特別指出的是，這里的dt并不只是一個(gè)采樣間隔。然后再看下圖注意方框中的公式，根據(jù)我們的觀察，不難發(fā)現(xiàn)，整個(gè)方框中都是為了獲得卡爾曼增益（矩陣Kg），我們設(shè)想，能否使用一個(gè)常數(shù)來等效替代卡爾曼增益那？根據(jù)我們在實(shí)驗(yàn)中的觀察，卡爾曼增益是一個(gè)收斂的變量，并且針對(duì)到我們的這個(gè)系統(tǒng)，他的值非常小，直立狀態(tài)下趨近于一個(gè)常數(shù)。上述五個(gè)公式便是卡爾曼濾波的五條數(shù)學(xué)公式在本項(xiàng)目中的使用。和分別是加速度傳感器和陀螺儀測量的協(xié)方差，其數(shù)值代表卡爾曼濾波器對(duì)其傳感器數(shù)據(jù)的信任程度，數(shù)值越小表明信任程度越高。這也就是為什么車模直立必須使用兩個(gè)傳感器來完成信號(hào)獲取。我們不難獲得以下一些結(jié)論：陀螺儀的噪聲還在可以接受范圍，但是加速度計(jì)噪聲已經(jīng)遠(yuǎn)遠(yuǎn)超出可控范圍接受對(duì)于加速度計(jì)做一個(gè)補(bǔ)充說明，加速度計(jì)在車模直立狀態(tài)下，電機(jī)PWM占空比100%噪聲情況下，幅值映射到實(shí)際角度大約是60度左右。他的廣泛應(yīng)用已經(jīng)超過30年，包括機(jī)器人導(dǎo)航，控制，傳感器數(shù)據(jù)融合甚至在軍事方面的雷達(dá)系統(tǒng)以及導(dǎo)彈追蹤等等。缺點(diǎn)在于重心依然較高。合理降低重心是最為行之有效的一個(gè)方法。在經(jīng)過幾次測試后我們發(fā)現(xiàn)，如果使用彈性連接，且將MEMS傳感器安裝在前部地盤上，由于彈性將使得角度信號(hào)產(chǎn)生滯后，對(duì)于信號(hào)的處理帶來很大影響，因此，這部分使用了剛性連接，為了減輕重量，考慮使用2mm碳纖維版作為連接部件。所以使用PID控制器來完成控制。假設(shè)車模開始保持靜止，然后增加給定速度，為此需要車模往前傾斜以便獲得加速度。這是由于未加速速度閉環(huán)的結(jié)果，未對(duì)車輪轉(zhuǎn)速進(jìn)行必要的控制，則最終由于電機(jī)轉(zhuǎn)速的飽和，而發(fā)生了無法對(duì)倒立擺進(jìn)行調(diào)整的后果。那么就可以使用PID控制器。調(diào)整車模直立時(shí)間常數(shù)很小，此時(shí)電機(jī)基本上運(yùn)行在加速階段。在將來的比賽中，如果規(guī)則增加了靜止要求，或者需要通過路橋等障礙，速度控制將會(huì)發(fā)揮作用。只要保證在、條件下，可以維持車模直立狀態(tài)?？刂频沽[底部車輪，使得它作加速運(yùn)動(dòng)。如果沒有阻尼力，單擺會(huì)在垂直位置左右擺動(dòng)。圖 車?？梢院喕傻沽⒌膯螖[。但相對(duì)于上面的木棒直立相對(duì)簡單。車模在