【正文】 ==BIT5) //左方傳感器接 { P2OUT |=BIT1。=~BIT0。BIT6)==BIT6) //右方傳感器接 15 { P2OUT |=BIT0。=~BIT1。=~BIT0。=~BIT1。 P2OUTamp。 //電機(jī)正轉(zhuǎn)前進(jìn) P2OUT|=BIT2。pwm_count0。 P2OUT|=BIT3。=~BIT2。pwm_count0。 if(((P4INamp。BIT6)!=BIT6)) //偏離位置 { if((P4INamp。 //右邊偏離 ,向左轉(zhuǎn) P2OUT amp。 P2OUT |=BIT2。=~BIT3。pwm_loop0。pwm_count0。 P2OUT amp。 P2OUT |=BIT3。pwm_count0。 } if((P4INamp。 //左邊偏離 ,向右轉(zhuǎn) 16 P2OUT amp。 P2OUT |=BIT2。=~BIT3。pwm_loop0。pwm_count0。 P2OUT amp。 P2OUT |=BIT3。pwm_count0。 } } else //未偏離位置 { P2OUT amp。 //方向直流電機(jī)停止 P2OUT amp。 } } void Balance() { unsigned int number,pwm_loop,pwm_time,xiaodou。 if(number1403) { P2OUT amp。 //方向直流電機(jī)停止 P2OUT amp。 P2OUT|=BIT2。=~BIT3。pwm_time0。 P2OUT |=BIT0。=~BIT1。=~BIT2。 for(pwm_time=500。pwm_time)。=~BIT2。=~BIT3。xiaodou0。pwm_time0。 } else if(number1382) { P2OUT amp。 //方向直流電機(jī)停止 P2OUT amp。 P2OUT|=BIT3。=~BIT2。pwm_time0。 P2OUT amp。 //方向直流電機(jī)停止 P2OUT |=BIT1。=~BIT3。 for(pwm_time=500。pwm_time)。=~BIT2。=~BIT3。xiaodou0。pwm_time0。 } else { P2OUT amp。 P2OUT amp。 //平衡 ,電機(jī)停止 P2OUT amp。 //方向直流電機(jī)停止 P2OUT amp。 for(pwm_loop=3。pwm_loop) for(pwm_time=60000。pwm_time)。 break。 for(pwm_loop=4。pwm_loop) { 18 MotorDriveGo_Gnd_and_Low 。 P2OUTamp。 P2OUT|=BIT2。pwm_count0。 P2OUTamp。 P2OUTamp。 } } /****************************************************************************** Name: Description： 電機(jī)驅(qū)動(dòng)部分 .h 文件。 void GoSlowForward()。 void BackSlowForward()。 void Stop()。 extern AYFilter()。time0。 } unsigned int GetAX(void) //獲得 X軸傾角值 { unsigned int Data=0。 19 ClrCSB。 SetCSB。 ClrCSB。 Data = ReceiveData()。 return Data。 Delay(TSCK)。 SendCommand(MEAS)。 Delay(T150us)。 SendCommand(RDAY)。 SetCSB。 } unsigned int ReceiveData() //使用 作為 MISO { unsigned int i,Data=0。i11。 SetSCK。 if( P4IN amp。 Delay(TSCK)。 } return Data。 for(i=0。i++) { if(Command amp。 else ClrMOSI。 Delay(TSCK)。 Delay(TSCK)。 } } //****************對(duì) AX,AY進(jìn)行采樣濾波處理 ,增強(qiáng)抗外界干擾能力 ****************// unsigned int AXFilter(void) //對(duì) X軸值進(jìn)行濾波 { unsigned int i,Max,Min,Average=0。 for(i=0。i++) //采集數(shù)據(jù) Data[i]=GetAX()。 for(i=1。i++) //尋找最值 { if( Data[i] Max ) Max=Data[i]。 } 21 for(i=0。i++) Average += Data[i]。 //去掉最值，求平均 Average = Average/3。 } unsigned int AYFilter(void) //對(duì) Y軸值進(jìn)行濾波 { unsigned int i,Max,Min,Average=0。 for(i=0。i++) //采集數(shù)據(jù) Data[i]=GetAY()。 for(i=1。i++) //尋找最值 { if( Data[i] Max ) Max=Data[i]。 } for(i=0。i++) Average += Data[i]。 //去掉最值，求平均 Average = Average/3。 } /****************************************************************************** Name: Description： 平衡測(cè)量與控制部分 .h文件。=~BIT0 define SetSCK P4OUT |=BIT1 //SCK接 define ClrSCK P4OUT amp。=~BIT2 define T150us 150 define TSCK 28 define MEAS 0x00 define RWTR 0x08 define STX 0x0e define STY 0x0f define RDAX 0x10 define RDAY 0x11 unsigned int AXFilter(void)。 unsigned int GetAX(void)。 unsigned int ReceiveData()。 void Delay(unsigned int t