pheng.c

小车跷跷板源程序

/************************/
/*						*/
/*  跷跷板平衡程序模块	*/
/*						*/
/************************/
#include<reg51.h>
void pheng()
 {if((p2_5==0||phzt==2)&&phzt!=4)//后仰了
		{	fy++;//反应参数自加一
			qj++;//前进（后仰）参数；
			TC=1;//打开电机
			if(cf||phzt==2)
			{	//若在冲锋,反应参数到达20,自动满足前进条件
				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)//若没有冲锋,则反应参数到200后,自动对前进后退参数置一,并且打开微调
			{fy=0;
			qj=1;
			ht=1;
			cs=8;
			}

			if(qj>=fdd*ht)//防平衡器抖动；
			{	ph=0;//电机打开,平衡态系数置零
				fx=2;//方向为前


				for(i=0;i<=(bf)/*步进补偿*/;i++)
				{

//////////////////////////////////前进循迹程序段///////////////////////////
xunji();
/////////////////////////////////////////////////////////

					if((cs%2)&&(cs<=10))//跷跷板过平衡点次数
					{	cs++;
					}
					 qj=1;//前进参数置一
					if (cf||bc==2)
					{ 	ht=30;
						if(bc==2)
				 		bc=4;
					}//若在冲锋,后退参数置为30
					else if(tz<=100||(ht<2))//若没有冲锋,且没有进入平衡状态或后退参数<2时,就将其置一,否则除以2
						ht=1;
					else ht=6;

					}
			}
			TC=0;
	    	p1_2=1;
		    p1_5=1;//使电机停止
			}

/////////////////////////////////////////////////////////////////////////////
if(phzt==0)
	if(p2_4==0||(p2_3&&p2_5))//平衡了
		{   TC=1;	//准备电机运行

			tz++;	//停止状态次数累加
			fy=0;	//可能进入平衡态,不需电机转动,反应参数清零

			if(cf==0&&tz>=100)//若无冲锋信号,平衡状态>100,跷跷板翻滚次数置到最大
			cs=8;             //直接进入微调
			if(cf==0&&tz>=5)//若无冲锋号,则可以进入平衡状态
			{/*	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;  //停止态标志置到最大
				ph++;   //平衡态系数自加1
				if(ph>=15)
				{	
				qj=10;ht=10;
				}

				if(ph>=24&&setnumber>4)//若平衡态系数>=10,则前进、后退系数初值置为10,使其判断时间加长
				{tz=tz+10;
				ph=0;
				setnumber--;}
				if(ph>=500)//若平衡系数>=500,已经平衡.关闭中断,显示当前时间
				{phzt=1;
				p3_4=0;
				setnumber=4;
				}
				if(ph>=25&&fdd<=25)
			{	//防抖动参数,逐渐增加,加长判断时间
				fdd=fdd+1;
			}
			}//增大防抖动参数
			TC=0;//关闭电机
			fx=0;//方向为无
	    	p1_2=1;//关闭电机使能
		    p1_5=1;//关闭电机使能

		}




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

		{	if(cf)

				fy++;	//反应系数自加一
			if(phzt==4)
				ht=fdd*qj;	/****************还需要加程序********************/////////////////////////////////////////
			if(fy>=20&&cf)//若在冲锋,等待反应系数到100时,自动退出冲锋状态
			{	cf=0;
				fy=0;
				setnumber++;   //通过PWM调节前进的速度
			}
/*			else if(!cf&&bc==1)//若没有冲锋,当防抖动系数<=15时,直接使后退参数满足后退条件
			{	ht=fdd*qj;
				bc=2;
			}*/
			ht++;		//后退（前倾）参数自加一。
			TC=1;		//打开电电机
			if(fy>=200)//若反应参数到200则自动对前进后退参数,自动置一
			{fy=0;
			qj=1;
			ht=1;}
			if(fy>=333&&tz<=4)
			tz=0;
			if(ht>=fdd*qj&&(tz>=10)&&cf==0)//防平衡器抖动；
			{	ph=0;//电机启动,平衡态系数,自动归零
				fx=1;//方向为后
				for(i=0;i<=bf*(bc+1);i++)
				{
//////////////////////////////////////////////////////////////////////后退循迹程序段
htxunji();
/////////////////////////////////////////////////////////////////////////////

					if(cs>=3)//若打开微调或当跷跷板翻滚次数达到3以上则调小步进
					{
						if(bf!=200)
 					bf=200;//再次调小步进

						if(!(cs%2)&&(cs<=10))//记录翻滚次数
						{	cs++;

						}
					}
				}
		 ht=1;//后退完毕,后退参数置一
			if(tz<=100||(qj<2))//没有进入过平衡态时或前进参数小于2时,前进参数置一,否则,除以2
			qj=1;
			else qj=6;
			if(bc>=1)
			{	bc=0;
				fdd=fdd+2;
				ht=100;
				qj=100;
				setnumber=6;
				bf=300;
			}
			}//重新设置前进后退参数
			TC=0;
	    	p1_2=1;
		    p1_5=1;//使电机停止
     	}


	}