【導(dǎo)讀】電磁組比賽要求車模在直立的狀態(tài)下以兩個輪子著地沿著賽道進(jìn)行比賽，相比四輪著地狀態(tài)，車?？刂迫蝿?wù)更為復(fù)雜。為了分析方便，根據(jù)比賽規(guī)則，假設(shè)維持車模直立、運行的動力都來自于車模的兩個后車輪，后輪轉(zhuǎn)動由兩個直流電機(jī)驅(qū)動。直流電機(jī)的力矩最終來自于電機(jī)驅(qū)動電壓產(chǎn)生的電流。在上述三個任務(wù)中保持車模直立是關(guān)鍵?？刂频沽[底部車輪，使得它作加速運動。此外，為了使得倒立擺能夠盡快地在垂直位置穩(wěn)定下來，還需要增加阻尼力，與偏角的速度成正比，方向相反。式中，為車模傾角；'為角速度；k1、k2均為比例系數(shù)；兩項相加后作為車輪加速度的控制量。只要保證在、條件下，可以維持車模直立狀態(tài)。其中，決定了車模是否能夠穩(wěn)定到垂直位置，它必須大于重力加速度；決定了車?；氐酱怪蔽恢玫淖枘嵯禂?shù)，選取合適的阻尼系數(shù)可以保證車模盡快穩(wěn)定在垂直位置。

　　

【正文】 e_dot) * ANGLE_CONTROL_D。 MotorOutput(g_inAngleOut + g_inSpeedOut)。 }/**********************MotorOutput電機(jī)輸出函數(shù)******************************************函數(shù)功能:通過int duty 來改變占空比設(shè)置電機(jī)的速度入口參數(shù): int duty占空比出口參數(shù): 無說明 :,需確認(rèn)電機(jī)是正轉(zhuǎn)還是反轉(zhuǎn) 0左轉(zhuǎn) 0時右轉(zhuǎn),車輪向前運動,PWM0****************************************************************/define DEATH_CONTROL 400 //電機(jī)死區(qū)占空比void MotorOutput(float duty) { float leftMotor。 //左右電機(jī)PWM占空比 float rightMotor。 leftMotor = duty + g_inDirControlOut。 rightMotor = duty g_inDirControlOut。 if(leftMotor =0) //克服電機(jī)死區(qū) leftMotor =leftMotor + DEATH_CONTROL。 else leftMotor =leftMotor DEATH_CONTROL。 if(rightMotor =0) rightMotor =rightMotor + DEATH_CONTROL。 else rightMotor =rightMotor DEATH_CONTROL。 if(PWMflag ==1) //保護(hù)部分標(biāo)志位 { rightMotor =0。 leftMotor =0。 } if(rightMotor =0) //車模向后傾斜,角度為正,電機(jī)反轉(zhuǎn) { PORTK_PK3 = 0。 PWMDTY01 = (uint)rightMotor。 } else //polar90 車模向前傾斜,角度為負(fù),電機(jī)正轉(zhuǎn) { PORTK_PK3 = 1。 //反轉(zhuǎn) PWMDTY01 = (uint)( rightMotor)。 } if(leftMotor =0) //車模向后傾斜,角度為正,電機(jī)反轉(zhuǎn) { PORTK_PK2 = 0。 PWMDTY23 = (uint)leftMotor。 } else //polar90 車模向前傾斜,角度為負(fù),電機(jī)正轉(zhuǎn) { PORTK_PK2 = 1。 PWMDTY23 =(uint)( leftMotor)。 } }/****************************************************************程序功能:采集CCD1數(shù)據(jù),通過上位機(jī)生成圖像 下面鏡頭入口參數(shù):無出口參數(shù):unsigned char uchr_ccd_data(128)共128個數(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。 int x。 PWME = 0X8f。 while(!TFLG1_C0F)。 //等待IC0撲捉到CLK的上升沿 x = TC0。 //清除標(biāo)志位 while(!TFLG1_C1F)。 //等待IC1撲捉到CLK的下降沿 x= TC1。 //清除標(biāo)志位 while(!TFLG1_C0F)。 //等待IC0撲捉到CLK的上升沿 x = TC0。 //清除標(biāo)志位 while(!TFLG1_C1F)。 //等待IC1撲捉到CLK的下降沿 x= TC1。 //清除標(biāo)志位 while(!TFLG1_C0F)。 //等待IC0撲捉到CLK的上升沿 x = TC0。 //清除標(biāo)志位 while(!TFLG1_C1F)。 //等待IC1撲捉到CLK的下降沿 x= TC1。 //清除標(biāo)志位 while(!TFLG1_C0F)。 //等待IC0撲捉到CLK的上升沿 x = TC0。 //清除標(biāo)志位 delay_1us()。 //短暫延時 //******啟動采集CCD數(shù)據(jù)輸出,并同步時序 CCD_SI = 0。 while(!TFLG1_C1F)。 //等待IC1撲捉到CLK的下降沿 x= TC1。 //清除標(biāo)志位 delay_1us()。 //短暫延時 CCD_SI = 1。 //開始采集pixel的值 while(!TFLG1_C0F)。 //等待IC0撲捉到CLK的上升沿 x = TC0。 //清除標(biāo)志位 delay_1us()。 //短暫延時 CCD_SI = 0。 //********以后每次撲捉CLK的下降沿128次 for(i=1。i=128。i++) { while(!TFLG1_C1F)。 //等待IC1撲捉到CLK的下降沿 ATD0CTL5 = 0X0A。 //開通AN10 uchr_ad_data[i] =ATD_uchar_convert() 。 x= TC1。 //清除標(biāo)志位 } // end of for //********捕獲和等待第129個時鐘完成,以結(jié)束本次CCD輸出 while(!TFLG1_C0F)。 //等待IC0撲捉到CLK的上升沿 x = TC0。 //清除標(biāo)志位 while(!TFLG1_C1F)。 //等待IC1撲捉到CLK的下降沿 x= TC1。 //清除標(biāo)志位 while(!TFLG1_C0F)。 //等待IC0撲捉到CLK的上升沿 x = TC0。 //清除標(biāo)志位 while(!TFLG1_C1F)。 //等待IC1撲捉到CLK的下降沿 x= TC1。 //清除標(biāo)志位 while(!TFLG1_C0F)。 //等待IC0撲捉到CLK的上升沿 x = TC0。 //清除標(biāo)志位 while(!TFLG1_C1F)。 //等待IC1撲捉到CLK的下降沿 x= TC1。 //清除標(biāo)志位 PWME = 0x0f。 Sendflag = 0。}/***************************************************************************程序功能:采集CCD2數(shù)據(jù),通過上位機(jī)生成圖像入口參數(shù):無出口參數(shù):unsigned char uchr_ccd_data(128)共128個數(shù)據(jù),其性質(zhì)為全局?jǐn)?shù)組電氣連接:PWM6CCD_CLK PORTK_PK5CCD_SI2 CCD_AOAN11 850us (OK)*******************************************************/define CCD_SI2 PORTK_PK5 //定義CCD操作接口void CCD_Collect2(void) { //*********定義發(fā)送數(shù)據(jù)數(shù)組,并初始化起始位 int i=0。 int x。 PWME = 0X4f。 while(!TFLG1_C2F)。 //等待IC0撲捉到CLK的上升沿 x = TC2。 //清除標(biāo)志位 //******啟動采集CCD數(shù)據(jù)輸出,并同步時序 CCD_SI2 = 0。 while(!TFLG1_C3F)。 //等待IC1撲捉到CLK的下降沿 x= TC3。 //清除標(biāo)志位 delay_1us()。 //短暫延時 CCD_SI2 = 1。 //開始采集pixel的值 while(!TFLG1_C2F)。 //等待IC0撲捉到CLK的上升沿 x = TC2。 //清除標(biāo)志位 delay_1us()。 //短暫延時 CCD_SI2 = 0。 //********以后每次撲捉CLK的下降沿128次 for(i=1。i=128。i++) { while(!TFLG1_C3F)。 //等待IC1撲捉到CLK的下降沿 ATD0CTL5 = 0X0B。 //開通AN10 uchr_ad_data2[i] =ATD_uchar_convert() 。 x= TC3。 //清除標(biāo)志位 } // end of for //********捕獲和等待第129個時鐘完成,以結(jié)束本次CCD輸出 while(!TFLG1_C2F)。 //等待IC0撲捉到CLK的上升沿 x = TC2。 //清除標(biāo)志位 while(!TFLG1_C3F)。 //等待IC1撲捉到CLK的下降沿 x= TC3。 //清除標(biāo)志位 while(!TFLG1_C2F)。 //等待IC0撲捉到CLK的上升沿 x = TC2。 //清除標(biāo)志位 while(!TFLG1_C3F)。 //等待IC1撲捉到CLK的下降沿 x= TC3。 //清除標(biāo)志位 PWME = 0x0f。 Sendflag2 = 0。}void main(vo