【正文】 SpeedPSet=。 } else if(speedcoderp1==0amp。amp。speedcoderp2==0){ //撥碼開(kāi)關(guān)3,4打開(kāi) SpeedPSet=。 } else if(speedcoderp1==1amp。amp。speedcoderp2==1){ //撥碼開(kāi)關(guān)3,4關(guān)閉 SpeedPSet=。 } //速度調(diào)節(jié)撥碼開(kāi)關(guān) if(speedcoder1==0amp。amp。speedcoder2==1){ //撥碼開(kāi)關(guān)1打開(kāi) CarSpeedSet1=69。 } else if(speedcoder2==0amp。amp。speedcoder1==1){ //撥碼開(kāi)關(guān)2打開(kāi) CarSpeedSet1=74。 } else if(speedcoder1==0amp。amp。speedcoder2==0){ //撥碼開(kāi)關(guān)1,2打開(kāi) CarSpeedSet1=76。 } else if(speedcoder1==1amp。amp。speedcoder2==1){ //撥碼開(kāi)關(guān)1,2關(guān)閉 CarSpeedSet1=80。 } //平衡調(diào)節(jié)撥碼開(kāi)關(guān) if(coder1==0amp。amp。coder2==1amp。amp。coder3==1amp。amp。coder4==1){ //撥碼開(kāi)關(guān)1打開(kāi) ACCELERSET+=5。 } else if(coder2==0amp。amp。coder1==1amp。amp。coder3==1amp。amp。coder4==1){ //撥碼開(kāi)關(guān)2打開(kāi) ACCELERSET+=10。 } else if(coder3==0amp。amp。coder1==1amp。amp。coder2==1amp。amp。coder4==1){ //撥碼開(kāi)關(guān)3打開(kāi) ACCELERSET+=15。 } else if(coder4==0amp。amp。coder1==1amp。amp。coder2==1amp。amp。coder3==1){ //撥碼開(kāi)關(guān)4打開(kāi) ACCELERSET+=20。 } else if(coder1==0amp。amp。coder2==0amp。amp。coder3==1amp。amp。coder4==1){ //撥碼開(kāi)關(guān)1,2打開(kāi) ACCELERSET+=25。 } else if(coder1==0amp。amp。coder3==0amp。amp。coder2==1amp。amp。coder4==1){ //撥碼開(kāi)關(guān)1,3打開(kāi) ACCELERSET+=30。 } else if(coder1==0amp。amp。coder4==0amp。amp。coder2==1amp。amp。coder3==1){ //撥碼開(kāi)關(guān)1,4打開(kāi) ACCELERSET+=35。 } else if(coder2==0amp。amp。coder3==0amp。amp。coder1==1amp。amp。coder4==1){ //撥碼開(kāi)關(guān)2,3打開(kāi) ACCELERSET=5。 } else if(coder2==0amp。amp。coder4==0amp。amp。coder1==1amp。amp。coder3==1){ //撥碼開(kāi)關(guān)2,4打開(kāi) ACCELERSET=10。 } else if(coder3==0amp。amp。coder4==0amp。amp。coder1==1amp。amp。coder2==1){ //撥碼開(kāi)關(guān)3,4打開(kāi) ACCELERSET=15。 } else if(coder1==0amp。amp。coder2==0amp。amp。coder3==0amp。amp。coder4==1){ //撥碼開(kāi)關(guān)1,2,3打開(kāi) ACCELERSET=20。 } else if(coder1==0amp。amp。coder2==0amp。amp。coder4==0amp。amp。coder3==1){ //撥碼開(kāi)關(guān)1,2,4打開(kāi) ACCELERSET=25。 } else if(coder1==0amp。amp。coder3==0amp。amp。coder4==0amp。amp。coder2==1){ //撥碼開(kāi)關(guān)1,3,4打開(kāi) ACCELERSET=30。 } else if(coder2==0amp。amp。coder3==0amp。amp。coder4==0amp。amp。coder1==1){ //撥碼開(kāi)關(guān)2,3,4打開(kāi) ACCELERSET=35。 } else if(coder1==0amp。amp。coder2==0amp。amp。coder3==0amp。amp。coder4==0){ //撥碼開(kāi)關(guān)1,2,3,4打開(kāi) ACCELERSET=40。 } else if(coder1==1amp。amp。coder2==1amp。amp。coder3==1amp。amp。coder4==1){ //撥碼開(kāi)關(guān)1,2,3,4全關(guān)上 } flag=1。 }/*********************************1ms中斷函數(shù)************************************/pragma CODE_SEG __NEAR_SEG NON_BANKEDinterrupt VectorNumber_Vpit0 void PIT(){ PITTF_PTF0 = 1。 //清中斷標(biāo)志 if(flag){ //處理水平陀螺儀和轉(zhuǎn)向陀螺儀的零偏完成 second++。 if(pa_flag==0amp。amp。end==0){ PORTA_PA0 = 1。 //電機(jī)使能 PORTA_PA1 = 1。 //電機(jī)使能 } if(second15000amp。amp。bar==0){ if(PTM_PTM0==1amp。amp。PTM_PTM1==0amp。amp。PTM_PTM2==1){ PORTA_PA0 = 0。 //電機(jī)使能 PORTA_PA1 = 0。 //電機(jī)使能 end=1。 } } EventCount++。 TimerCnt10ms++。 if(TimerCnt10ms=10){ TimerCnt10ms=0。 TimerFlag10ms=1。 } if(second10000){ if(second15000){ second=15006。 } speedpcount++。 if(speedpcount4000){ if(CarSpeed){ SpeedP=。 } else{ if(SpeedPSpeedPSet){ SpeedP=SpeedP+。 } else{ SpeedP=SpeedPSet。 } } } else if(speedpcount=4000){ speedpcount=50005。 SpeedP=SpeedPSet。 } CarSpeedSet=CarSpeedSet1。 if(speedpcount100amp。amp。speedpcount1211){ CarSpeedSet=CarSpeedSet+35。 } Angle_D=。 if(bar==1){ secondbar++。 if(secondbar=900){ Angle_D=。 SpeedP=。 CarSpeedSet=0。 } else if(secondbar900amp。amp。secondbar=2500){ CarSpeedSet=25。 if(secondbar1000amp。amp。secondbar=1500){ CarSpeedSet=45。 } else if(secondbar1500amp。amp。secondbar=2000){ CarSpeedSet=50。 } else if(secondbar2000amp。amp。secondbar=2300){ CarSpeedSet=60。 } else if(secondbar2300amp。amp。secondbar=2500){ CarSpeedSet=70。 } } else if(secondbar2500){ CarSpeedSet=CarSpeedSet1。 } if(secondbar=3000){ secondbar=0。 bar=0。 } } } if(second5000){ secondflag=1。 } SpeedControlPeriod++。 SpeedControlOutPut()。 switch(EventCount){ case 1: GetMotorPulse()。 break。 case 2: break。 case 3: AngleCalculate()。 MotorOutPut()。 break。 case 4: SpeedControlCount++。 if(SpeedControlCount=20){ SpeedControl()。 SpeedControlCount=0。 SpeedControlPeriod=0。 } break。 case 5: EventCount=0。 break。 } }}pragma CODE_SEG DEFAULT/******************************主函數(shù)*******************************************/ void main(void) { system_init()。 gyro_zero()。 EnableInterrupts。 for(。) { if(TimerFlag10msamp。amp。secondflag){ TimerFlag10ms=0。/**************捕獲CCD數(shù)據(jù)*****************/ RD_TSL()。 directionSpeed()。 piancha1()。/*********10ms向上位機(jī)發(fā)送一次數(shù)據(jù)********/ SendImageData(Pixel)。 } } }33