/************************/
/*						*/
/*  蹺蹺板平衡程序模塊	*/
/*						*/
/************************/
#include<reg51.h>
void pheng()
 {if((p2_5==0||phzt==2)&&phzt!=4)//后仰了
		{	fy++;//反應(yīng)參數(shù)自加一
			qj++;//前進(jìn)（后仰）參數(shù)；
			TC=1;//打開電機(jī)
			if(cf||phzt==2)
			{	//若在沖鋒,反應(yīng)參數(shù)到達(dá)20,自動(dòng)滿足前進(jìn)條件
				qj=fdd*ht;
				bc=1;
               	if(cf&&tk>=35)
                   	setnumber=5;
				if(cf&&tk>=45)
					setnumber=4;
				if(phzt==2)
				if(p2_0&&p2_1&&p2_2)
					phzt=3;
                         }
			if(!cf&&fy>=200)//若沒有沖鋒,則反應(yīng)參數(shù)到200后,自動(dòng)對(duì)前進(jìn)后退參數(shù)置一,并且打開微調(diào)
			{fy=0;
			qj=1;
			ht=1;
			cs=8;
			}

			if(qj>=fdd*ht)//防平衡器抖動(dòng)；
			{	ph=0;//電機(jī)打開,平衡態(tài)系數(shù)置零
				fx=2;//方向?yàn)榍?

				for(i=0;i<=(bf)/*步進(jìn)補(bǔ)償*/;i++)
				{

//////////////////////////////////前進(jìn)循跡程序段///////////////////////////
xunji();
/////////////////////////////////////////////////////////

					if((cs%2)&&(cs<=10))//蹺蹺板過平衡點(diǎn)次數(shù)
					{	cs++;
					}
					 qj=1;//前進(jìn)參數(shù)置一
					if (cf||bc==2)
					{ 	ht=30;
						if(bc==2)
				 		bc=4;
					}//若在沖鋒,后退參數(shù)置為30
					else if(tz<=100||(ht<2))//若沒有沖鋒,且沒有進(jìn)入平衡狀態(tài)或后退參數(shù)<2時(shí),就將其置一,否則除以2
						ht=1;
					else ht=6;

					}
			}
			TC=0;
	    	p1_2=1;
		    p1_5=1;//使電機(jī)停止
			}

/////////////////////////////////////////////////////////////////////////////
if(phzt==0)
	if(p2_4==0||(p2_3&&p2_5))//平衡了
		{   TC=1;	//準(zhǔn)備電機(jī)運(yùn)行

			tz++;	//停止?fàn)顟B(tài)次數(shù)累加
			fy=0;	//可能進(jìn)入平衡態(tài),不需電機(jī)轉(zhuǎn)動(dòng),反應(yīng)參數(shù)清零

			if(cf==0&&tz>=100)//若無(wú)沖鋒信號(hào),平衡狀態(tài)>100,蹺蹺板翻滾次數(shù)置到最大
			cs=8;             //直接進(jìn)入微調(diào)
			if(cf==0&&tz>=5)//若無(wú)沖鋒號(hào),則可以進(jìn)入平衡狀態(tài)
			{/*	if(bc==1)
				if(fx==2)
				{	for(i=0;i<=((bf-100)/2);i++)
					forwordrun();
					for(i=0;i<=((bf-50)/2);i++)
					backrun();
					bc=0;
				}
				else if(fx==1)
				{	for(i=0;i<=((bf-50)/2);i++)
					backrun();
					for(i=0;i<=((bf-100)/2);i++)
					forwordrun();
					bc=0;
				}*/
				if(tz>=300)
				tz=300;  //停止態(tài)標(biāo)志置到最大
				ph++;   //平衡態(tài)系數(shù)自加1
				if(ph>=15)
				{	
				qj=10;ht=10;
				}

				if(ph>=24&&setnumber>4)//若平衡態(tài)系數(shù)>=10,則前進(jìn)、后退系數(shù)初值置為10,使其判斷時(shí)間加長(zhǎng)
				{tz=tz+10;
				ph=0;
				setnumber--;}
				if(ph>=500)//若平衡系數(shù)>=500,已經(jīng)平衡.關(guān)閉中斷,顯示當(dāng)前時(shí)間
				{phzt=1;
				p3_4=0;
				setnumber=4;
				}
				if(ph>=25&&fdd<=25)
			{	//防抖動(dòng)參數(shù),逐漸增加,加長(zhǎng)判斷時(shí)間
				fdd=fdd+1;
			}
			}//增大防抖動(dòng)參數(shù)
			TC=0;//關(guān)閉電機(jī)
			fx=0;//方向?yàn)闊o(wú)
	    	p1_2=1;//關(guān)閉電機(jī)使能
		    p1_5=1;//關(guān)閉電機(jī)使能

		}




		 if((p2_3==0||phzt==4)&&phzt!=2)//前傾了

		{	if(cf)

				fy++;	//反應(yīng)系數(shù)自加一
			if(phzt==4)
				ht=fdd*qj;	/****************還需要加程序********************/////////////////////////////////////////
			if(fy>=20&&cf)//若在沖鋒,等待反應(yīng)系數(shù)到100時(shí),自動(dòng)退出沖鋒狀態(tài)
			{	cf=0;
				fy=0;
				setnumber++;   //通過PWM調(diào)節(jié)前進(jìn)的速度
			}
/*			else if(!cf&&bc==1)//若沒有沖鋒,當(dāng)防抖動(dòng)系數(shù)<=15時(shí),直接使后退參數(shù)滿足后退條件
			{	ht=fdd*qj;
				bc=2;
			}*/
			ht++;		//后退（前傾）參數(shù)自加一。
			TC=1;		//打開電電機(jī)
			if(fy>=200)//若反應(yīng)參數(shù)到200則自動(dòng)對(duì)前進(jìn)后退參數(shù),自動(dòng)置一
			{fy=0;
			qj=1;
			ht=1;}
			if(fy>=333&&tz<=4)
			tz=0;
			if(ht>=fdd*qj&&(tz>=10)&&cf==0)//防平衡器抖動(dòng)；
			{	ph=0;//電機(jī)啟動(dòng),平衡態(tài)系數(shù),自動(dòng)歸零
				fx=1;//方向?yàn)楹?				for(i=0;i<=bf*(bc+1);i++)
				{
//////////////////////////////////////////////////////////////////////后退循跡程序段
htxunji();
/////////////////////////////////////////////////////////////////////////////

					if(cs>=3)//若打開微調(diào)或當(dāng)蹺蹺板翻滾次數(shù)達(dá)到3以上則調(diào)小步進(jìn)
					{
						if(bf!=200)
 					bf=200;//再次調(diào)小步進(jìn)

						if(!(cs%2)&&(cs<=10))//記錄翻滾次數(shù)
						{	cs++;

						}
					}
				}
		 ht=1;//后退完畢,后退參數(shù)置一
			if(tz<=100||(qj<2))//沒有進(jìn)入過平衡態(tài)時(shí)或前進(jìn)參數(shù)小于2時(shí),前進(jìn)參數(shù)置一,否則,除以2
			qj=1;
			else qj=6;
			if(bc>=1)
			{	bc=0;
				fdd=fdd+2;
				ht=100;
				qj=100;
				setnumber=6;
				bf=300;
			}
			}//重新設(shè)置前進(jìn)后退參數(shù)
			TC=0;
	    	p1_2=1;
		    p1_5=1;//使電機(jī)停止
     	}


	}