分类：
2010-01-28 23:02:18
float tim_1=0.0; /*时间变量：不用*/
float gf_1=0.0; /*速度功率值 */
int mic_1=0; /*声音值,AI8或AI12*/
int gi_1=0; /*用控制是否跳出某房间循环的量：任务完成与否控制量*/
int gi_2=0; /*相对的左边火焰亮度值*/
int gi_3=0; /*相对的左右边火焰亮度值的差值*/
int gi_4=0; /*相对的右边火焰亮度值*/
int gi_5=0; /*高中、小学程序切换*/
int gi_6=0; /*二号房回家时用来数线的量*/
int gi_7=0; /*第一次去二号房时看到的有没有火的标记量*/
int gi_8=0; /*地面灰度设置值*/
int gi_9=0; /*侧面PSD测距值*/
int gi_10=0; /*区分环境光与蜡烛光的参考值*/
int ma_1=0; /*前左PSD值AI0*/
int ma_2=0; /*正前PSD值AI1*/
int ma_3=0; /*前右PSD值AI2*/
int ma_4=0; /*后右PSD值AI3*/
int ma_5=0; /*正后PSD值AI4*/
int ma_6=0; /*后左PSD值AI5*/
int ma_7=0; /*前地面灰度值AI6*/
int ma_8=0; /*后地面灰度值AI7*/
int ma_9=0; /*回家时所用的地面灰度的二次测量值*/
int ma_10=0; /*正前正后PSD测距值，例值 220*/
int sci_1=0; /*指南针读值*/
void main()
{
SCI_Set(0,9600,0,8,1 );/*串口设置：指南针的端口设置，小学、初中不用*/
/*****************************************************************************/
/*以下为调试参数*****************************************************************/
gi_10=450; /*区分环境光与蜡烛光的参考值，约比环境光最小值小50，例值：450*/
gi_8=310; /*地面灰度设置值，原则：稳定地区分黑白，取黑白值的中间值，例值：300*/
gi_9=280; /*侧面PSD测距值，例值： 280*/
ma_10=280; /*正前正后PSD测距值，例值 280*/
gi_5=1;/*小学取1，初高中取0*****/
gf_1=1.0;/*功率值，范围0.0~1.0*/
/*****************************************************************************/
while(1)
{
mic_1=AI(8);
if(mic_1 > 750)
{
break;
}
}
//SetMotor(0x1111,100,100,0,0);
SetMotor(0x1111,(int)(100*gf_1),(int)(100*gf_1),0,0);
wait( 0.300000 );
while(1)
{ /*四号房*****************************/
/************************************************************/
while(1)
{
ma_7 = AI(6);
if(ma_7 < gi_8)
{
gi_2=AI(9);
gi_4=AI(11);
if((gi_2 < gi_10) || (gi_4 < gi_10))
{
//SetMotor(0x1111,90,10,0,0);
SetMotor(0x1111,(int)(90*gf_1),(int)(10*gf_1),0,0); /*有火时进四号房*************/
wait( 0.100000 );
//SetMotor(0x1111,80,80,0,0);
SetMotor(0x1111,(int)(80*gf_1),(int)(80*gf_1),0,0); /*有火时进四号房*************/
wait( 0.050000 );
while(1)
{
ma_7 = AI(6);
gi_2=AI(9);
if((ma_7 < gi_8) && (gi_2 < 300))/*四号灭火*/
{
StopMotor(0x1111);
//SetMotor(0x1111,100,-100,0,0);
SetMotor(0x1111,(int)(100*gf_1),(int)(-100*gf_1),0,0);
wait( 0.080000 );
StopMotor(0x1111);
DO( 0x1, 1 );
tim_1 =seconds();
wait( 0.500000 );/*机器人停顿灭火时间*/
DO( 0x1, 0 );
/*gi_5=1;*小学***/
break;
}
else
{
SubRoutine_2 ();
}
}
if(gi_5 == 1)
{
gi_1=1;
break;
}
SubRoutine_3 ();
gi_1 = 1 ;
break;
}
else /*出四号房*************/
{
StopMotor(0x1111);
wait( 0.180000 );
//SetMotor(0x1111,-60,-100,0,0);
SetMotor(0x1111,(int)(-40*gf_1),(int)(-100*gf_1),0,0);
wait( 0.300000 );
break;
}
}
else
{
SubRoutine_1 ();
}
}
if(gi_1 == 1)
{
break;
}
/*三号房*****************************/
/************************************************************/
while(1)
{
ma_8 = AI(7);
if(ma_8 < gi_8)
{
gi_2=AI(13);
gi_4=AI(15);
if((gi_2 < gi_10) || (gi_4 < gi_10))
{
//SetMotor(0x1111,-80,-10,0,0);
SetMotor(0x1111,(int)(-80*gf_1),(int)(-10*gf_1),0,0); /*有火时进三号房*************/
wait( 0.100000 );
while(1)
{
ma_8 = AI(7);
gi_2=AI(15);
if((ma_8 < gi_8) && (gi_2 < 300)) /*三号灭火*/
{
StopMotor(0x1111);
DO( 0x10, 1 );
tim_1 =seconds();
wait( 0.500000 );/*机器人停顿灭火时间*/
DO( 0x10, 0 );
/*gi_5=1;*小学***/
break;
}
else
{
SubRoutine_5 ();
}
}
if(gi_5 == 1)
{
gi_1=1;
break;
}
SubRoutine_6 ();
gi_1 = 1 ;
break;
}
else
{
StopMotor(0x1111); /*没火时出三号房*************/
wait( 0.180000 );
//SetMotor(0x1111,100,15,0,0);
SetMotor(0x1111,(int)(100*gf_1),(int)(15*gf_1),0,0);
wait( 0.400000 );
break;
}
}
else
{
SubRoutine_4 ();
}
}
if(gi_1 == 1)
{
break;
}
/*一（-2-1）号房*****************************/
/************************************************************/
while(1)
{
ma_7 = AI(6);
if(ma_7 < gi_8)
{
gi_2=AI(9);
gi_4=AI(11);
if((gi_2 < gi_10) || (gi_4 < gi_10))/*此处不用改（第一次去二号门口测光，不进）*/
{
gi_7 = 1 ;
}
StopMotor(0x1111); /*第一次从二号门出***********/
wait( 0.180000 );
//SetMotor(0x1111,-80,-20,0,0);
SetMotor(0x1111,(int)(-10*gf_1),(int)(-80*gf_1),0,0);
wait( 0.40000 );
while(1)
{
ma_8 = AI(7);
if(ma_8 < gi_8)
{
gi_2=AI(13);
gi_4=AI(15);
if((gi_2 < gi_10) || (gi_4 < gi_10))
{
//SetMotor(0x1111,-80,-15,0,0);
SetMotor(0x1111,(int)(-80*gf_1),(int)(-15*gf_1),0,0); /*有火时进一号房*************/
wait( 0.200000 );
while(1)
{
ma_8 = AI(7);
gi_2=AI(15);
if((ma_8 < gi_8) && (gi_2 < 300)) /*一号灭火*/
{
StopMotor(0x1111);
DO( 0x10, 1 );
tim_1 =seconds();
wait( 0.500000 ); /*机器人停顿灭火时间*/
DO( 0x10, 0 );
/*gi_5=1;*小学***/
break;
}
else
{
SubRoutine_5 ();
}
}
if(gi_5 == 1)
{
gi_1=1;
break;
}
SubRoutine_10 ();
gi_1 = 1 ;
break;
}
else
{
StopMotor(0x1111);
wait( 0.180000 );
//SetMotor(0x1111,-60,60,0,0);
SetMotor(0x1111,(int)(-60*gf_1),(int)(60*gf_1),0,0); /**最后去2号房之前（即从一号去二号）**/
wait( 0.020000 );
StopMotor(0x1111);
//SetMotor(0x1111,100,55,0,0);
SetMotor(0x1111,(int)(100*gf_1),(int)(55*gf_1),0,0);
ma_2 = AI(1);
ma_3 = AI(2);
while((ma_2 <= ma_10) && (ma_3 <= gi_9))/**最后去2号房之前**/
{
ma_2 = AI(1);
ma_3 = AI(2);
} /**最后去2号房之前**/
SetMotor(0x1111,(int)(70*gf_1),(int)(70*gf_1),0,0);
wait( 0.2 );
SetMotor(0x1111,(int)(50*gf_1),(int)(-50*gf_1),0,0);
wait( 0.2 );
gi_1=2;
break;
}
}
else
{
SubRoutine_4 ();
}
if((gi_1 == 2) || (gi_1 == 1))
{
break;
}
}
if((gi_1 == 2) || (gi_1 == 1))
{
break;
}
}
else
{
SubRoutine_7 ();
}
}
if(gi_1 == 1)
{
break;
}
/*二号房*****************************/
/************************************************************/
while(1)
{
ma_7 = AI(6);
if(ma_7 < gi_8)
{
gi_2=AI(9);
gi_4=AI(11);
if(gi_7==1)gi_2=10;/*第一次已*/ /********************/
if((gi_2 < 1023) || (gi_4 < gi_10))
{
//SetMotor(0x1111,50,80,0,0);
SetMotor(0x1111,(int)(50*gf_1),(int)(80*gf_1),0,0);
wait( 0.100000 );
//SetMotor(0x1111,25,70,0,0);
SetMotor(0x1111,(int)(10*gf_1),(int)(100*gf_1),0,0);
wait( 0.3500 );
while(1)
{
ma_7 = AI(6);
gi_2=AI(11);
if((ma_7 < gi_8) && (gi_2 < 300)) /*二号灭火*/
{
StopMotor(0x1111);
DO( 0x1, 1 );
tim_1 =seconds();
wait( 0.500000 ); /*机器人停顿灭火时间*/
DO( 0x1, 0 );
/*gi_5=1;*小学***/
break;
}
else
{
SubRoutine_2 ();
}
}
if(gi_5 == 1)
{
gi_1=1;
break;
}
StopMotor(0x1111);
SubRoutine_9 ();
gi_1 = 1 ;
break;
}
else
{
StopMotor(0x1111);
wait( 0.180000 );
break;
}
}
else
{
SubRoutine_7 ();
}
}
if(gi_1 == 1)
{
break;
}
break; /**/
}
StopMotor(0x1111);
}
/***********************************************************************/
/***********************************************************************/
/***********************************************************************/
/***********************************************************************/
/**以下为子程序*********************************************************/
void SubRoutine_1( ) /*沿左墙*/
{
ma_1 = AI(0);
ma_2 = AI(1);
if((ma_1 > gi_9) && (ma_2 < ma_10))
{
//SetMotor(0x1111,100,100,0,0);
SetMotor(0x1111,(int)(100*gf_1),(int)(100*gf_1),0,0);
}
else
{
if((ma_1 < gi_9) && (ma_2 < ma_10))
{
//SetMotor(0x1111,100,25,0,0);
SetMotor(0x1111,(int)(100*gf_1),(int)(20*gf_1),0,0);/*行走过程中转弯速度***/
}
else
{
if((ma_1 > gi_9) && (ma_2 > ma_10))
{
//SetMotor(0x1111,-40,40,0,0);
SetMotor(0x1111,(int)(-40*gf_1),(int)(40*gf_1),0,0);
}
else
{
//SetMotor(0x1111,90,25,0,0);
SetMotor(0x1111,(int)(90*gf_1),(int)(25*gf_1),0,0);/*行走过程中转弯速度***/
}
}
}
//wait( 0.001000 );
}
void SubRoutine_2( ) /*趋光*/
{
gi_3=AI(9)-AI(11);
if(gi_3 < -10)
{
//SetMotor(0x1111,50,13,0,0);
SetMotor(0x1111,(int)(80*gf_1),(int)(13*gf_1),0,0);
}
else
{
if(gi_3 > 10)
{
//SetMotor(0x1111,13,50,100,100);
SetMotor(0x1111,(int)(13*gf_1),(int)(80*gf_1),100,100);
}
else
{
//SetMotor(0x1111,90,90,0,0);
SetMotor(0x1111,(int)(90*gf_1),(int)(90*gf_1),0,0);
}
}
//wait( 0.001000 );
}
/****************************************************/
void SubRoutine_3( ) /*4号回家************/
{
//SetMotor(0x1111,-100,-40,100,100);
SetMotor(0x1111,(int)(-100*gf_1),(int)(-40*gf_1),100,100);
wait( 0.250000 );
DO( 0x111111, 0 );
ma_9 = 1023 ;
resettime();
while(1)
{
ma_8 = AI(7);
if(ma_8 < gi_8)
{
wait( 0.080000 );
ma_9 = AI(7);
}
if(ma_9 <= gi_8)
{
StopMotor(0x1111);
break;
}
else
{
tim_1 =seconds();
if(tim_1 < 1.100000)
{
SubRoutine_4( );
}
else
{
SubRoutine_8( );
}
}
}
}
/****************************************************/
void SubRoutine_4( ) /*沿右墙B*/
{
ma_5 = AI(4);
ma_6 = AI(5);
if((ma_6 > gi_9) && (ma_5 < ma_10))
{
//SetMotor(0x1111,-100,-100,0,0);
SetMotor(0x1111,(int)(-100*gf_1),(int)(-100*gf_1),0,0);
}
else
{
if((ma_6 < gi_9) && (ma_5 < ma_10))
{
//SetMotor(0x1111,-100,-25,0,0);
SetMotor(0x1111,(int)(-100*gf_1),(int)(-20*gf_1),0,0);/*行走过程中转弯速度***/
}
else
{
if((ma_6 > gi_9) && (ma_5 > ma_10))
{
//SetMotor(0x1111,40,-40,0,0);
SetMotor(0x1111,(int)(40*gf_1),(int)(-40*gf_1),0,0);
}
else
{
//SetMotor(0x1111,-90,-25,0,0);
SetMotor(0x1111,(int)(-90*gf_1),(int)(-20*gf_1),0,0);/*行走过程中转弯速度***/
}
}
}
//wait( 0.001000 );
}
void SubRoutine_5( ) /*趋光B*/
{
gi_3=AI(13)-AI(15);
if(gi_3 < -10)
{
//SetMotor(0x1111,-13,-60,0,0);
SetMotor(0x1111,(int)(-13*gf_1),(int)(-80*gf_1),0,0);
}
else
{
if(gi_3 > 10)
{
//SetMotor(0x1111,-60,-13,0,0);
SetMotor(0x1111,(int)(-80*gf_1),(int)(-13*gf_1),0,0);
}
else
{
//SetMotor(0x1111,-90,-90,0,0);
SetMotor(0x1111,(int)(-90*gf_1),(int)(-90*gf_1),0,0);
}
}
//wait( 0.001000 );
}
/****************************************************/
void SubRoutine_6( ) /*三号回家*/
{
//SetMotor(0x1111,-100,100,0,0);
SetMotor(0x1111,(int)(-100*gf_1),(int)(100*gf_1),0,0);
wait( 0.100000 );
StopMotor(0x1111);
ma_7 = AI(6);
while(1)
{
ma_7 = AI(6);
if(ma_7 < gi_8)
{
break;
}
else
{
SubRoutine_1( );
}
}
DO( 0x111111, 0 );
resettime();
while(1)
{
tim_1 =seconds();
if(tim_1 >= 0.500000)
{
break;
}
else
{
SubRoutine_1( );
}
}
StopMotor(0x1111);
//SetMotor(0x1111,100,95,0,0);
SetMotor(0x1111,(int)(100*gf_1),(int)(95*gf_1),0,0);
ma_2 = AI(1);
while(ma_2 < 500)
{
ma_2 = AI(1);
}
StopMotor(0x1111);
//SetMotor(0x1111,-50,50,0,0);
SetMotor(0x1111,(int)(-80*gf_1),(int)(80*gf_1),0,0);
wait( 0.200000 );
StopMotor(0x1111);
ma_9 = 1023 ;
while(1)
{
ma_7 = AI(6);
if(ma_7 < gi_8)
{
//SetMotor(0x1111,100,20,0,0);
SetMotor(0x1111,(int)(100*gf_1),(int)(20*gf_1),0,0);
wait( 0.100000 );
ma_9 = AI(6);
}
if(ma_9 <= gi_8)
{
StopMotor(0x1111);
break;
}
else
{
SubRoutine_1( );
}
}
}
/****************************************************/
void SubRoutine_7( ) /*沿右墙*/
{
ma_2 = AI(1);
ma_3 = AI(2);
if((ma_3 > gi_9) && (ma_2 < ma_10))
{
//SetMotor(0x1111,100,100,0,0);
SetMotor(0x1111,(int)(100*gf_1),(int)(100*gf_1),0,0);
}
else
{
if((ma_3 < gi_9) && (ma_2 < ma_10))
{
//SetMotor(0x1111,25,100,0,0);
SetMotor(0x1111,(int)(20*gf_1),(int)(100*gf_1),0,0);/*行走过程中转弯速度***/
}
else
{
if((ma_2 > ma_10) && (ma_3 > gi_9))
{
//SetMotor(0x1111,40,-40,0,0);
SetMotor(0x1111,(int)(40*gf_1),(int)(-40*gf_1),0,0);
}
else
{
//SetMotor(0x1111,25,90,0,0);
SetMotor(0x1111,(int)(25*gf_1),(int)(90*gf_1),0,0);/*行走过程中转弯速度***/
}
}
}
//wait( 0.001000 );
}
void SubRoutine_8( ) /*沿左墙B*/
{
ma_4 = AI(3);
ma_5 = AI(4);
if((ma_4 > gi_9) && (ma_5 < ma_10))
{
//SetMotor(0x1111,-100,-100,0,0);
SetMotor(0x1111,(int)(-100*gf_1),(int)(-100*gf_1),0,0);
}
else
{
if((ma_4 < gi_9) && (ma_5 < ma_10))
{
//SetMotor(0x1111,-25,-100,0,0);
SetMotor(0x1111,(int)(-20*gf_1),(int)(-100*gf_1),0,0); /*行走过程中转弯速度***/
}
else
{
if((ma_4 > gi_9) && (ma_5 > ma_10))
{
//SetMotor(0x1111,-40,40,0,0);
SetMotor(0x1111,(int)(-40*gf_1),(int)(40*gf_1),0,0);
}
else
{
//SetMotor(0x1111,-25,-90,0,0);
SetMotor(0x1111,(int)(-25*gf_1),(int)(-90*gf_1),0,0);/*行走过程中转弯速度***/
}
}
}
//wait( 0.001000 );
}
/****************************************************/
void SubRoutine_9( ) /*二号回家*/
{
//SetMotor(0x1111,-90,-90,0,0);
SetMotor(0x1111,(int)(-100*gf_1),(int)(-80*gf_1),0,0);
wait( 0.85000 );
SetMotor(0x1111,(int)(100*gf_1),(int)(-100*gf_1),0,0);
wait( 0.15);
StopMotor(0x1111);
wait( 0.1500000 );
DO( 0x111111, 0 );
ma_9 = 1023 ;
while(1)
{
ma_8 = AI(7);
if(ma_8 < gi_8)
{
wait( 0.08 );
ma_9 = AI(7);
gi_6 = gi_6 + 1;
}
if(ma_9 <= gi_8)
{
StopMotor(0x1111);
break;
}
else
{
if(gi_6 == 2)
{
gi_6 = gi_6 + 1;
StopMotor(0x1111);
wait( 0.300000 );
//SetMotor(0x1111,65,100,0,0);
SetMotor(0x1111,(int)(65*gf_1),(int)(100*gf_1),0,0);
wait( 0.015000 );
}
else
{
if(gi_6 < 3)
{
SubRoutine_4( ); /**/
}
else
{
SubRoutine_7( ); /**/
}
}
}
}
}
/****************************************************/
/****************************************************/
void SubRoutine_10( ) /*一号回家*/
{
//SetMotor(0x1111,79,100,0,0);
SetMotor(0x1111,(int)(100*gf_1),(int)(100*gf_1),0,0);
wait( 0.550000 );
DO( 0x111111, 0 );
ma_9 = 1023 ;
while(1)
{
ma_8 = AI(6);
if(ma_8 < gi_8)
{
//SetMotor(0x1111,10,100,0,0);
SetMotor(0x1111,(int)(10*gf_1),(int)(100*gf_1),0,0);
wait( 0.100000 );
ma_9 = AI(6);
}
if(ma_9 <= gi_8)
{
StopMotor(0x1111);
break;
}
else
{
SubRoutine_7( );
}
}
}
/****************************************************/
