广茂达机器人灭火程序(纯C语言版)

1、 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

2、_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; /*回家时所用的地面灰度的二次测量

3、值*/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_

4、5=1;/*小学取1，初高中取0*/ gf_1=1.0;/*功率值，范围0.01.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. ); 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

5、) /SetMotor(0x1111,90,10,0,0); SetMotor(0x1111,(int)(90*gf_1),(int)(10*gf_1),0,0); /*有火时进四号房*/ wait( 0. ); /SetMotor(0x1111,80,80,0,0); SetMotor(0x1111,(int)(80*gf_1),(int)(80*gf_1),0,0); /*有火时进四号房*/ wait( 0. ); while(1) ma_7 = AI(6); gi_2=AI(9); if(ma_7 gi_8) & (gi_2 300)/*四号灭火*/ StopMotor(0x1111);

6、 /SetMotor(0x1111,100,-100,0,0); SetMotor(0x1111,(int)(100*gf_1),(int)(-100*gf_1),0,0); wait( 0. ); StopMotor(0x1111); DO( 0x1, 1 ); tim_1 =seconds(); wait( 0. );/*机器人停顿灭火时间*/ DO( 0x1, 0 ); /*gi_5=1;*小学*/ break; else SubRoutine_2 (); if(gi_5 = 1) gi_1=1; break; SubRoutine_3 (); gi_1 = 1 ; break; els

7、e /*出四号房*/ StopMotor(0x1111); wait( 0. ); /SetMotor(0x1111,-60,-100,0,0); SetMotor(0x1111,(int)(-40*gf_1),(int)(-100*gf_1),0,0); wait( 0. ); 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) /SetM

8、otor(0x1111,-80,-10,0,0); SetMotor(0x1111,(int)(-80*gf_1),(int)(-10*gf_1),0,0); /*有火时进三号房*/ wait( 0. ); 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. );/*机器人停顿灭火时间*/ DO( 0x10, 0 ); /*gi_5=1;*小学*/ break; else SubRo

9、utine_5 (); if(gi_5 = 1) gi_1=1; break; SubRoutine_6 (); gi_1 = 1 ; break; else StopMotor(0x1111); /*没火时出三号房*/ wait( 0. ); /SetMotor(0x1111,100,15,0,0); SetMotor(0x1111,(int)(100*gf_1),(int)(15*gf_1),0,0); wait( 0. ); break; else SubRoutine_4 (); if(gi_1 = 1) break; /*一（-2-1）号房*/ /*/ while(1) ma_7 =

10、 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. ); /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=

11、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. ); 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. ); /*机器

12、人停顿灭火时间*/ 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. ); /SetMotor(0x1111,-60,60,0,0); SetMotor(0x1111,(int)(-60*gf_1),(int)(60*gf_1),0,0); /*最后去2号房之前（即从一号去二号）*/ wait( 0. ); StopMotor(0x11

13、11); /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)

14、,(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)

15、| (gi_4 gi_10) /SetMotor(0x1111,50,80,0,0); SetMotor(0x1111,(int)(50*gf_1),(int)(80*gf_1),0,0); wait( 0. ); /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 gi_9) & (ma_2 ma_10) /SetMotor(0x111

16、1,100,100,0,0); SetMotor(0x1111,(int)(100*gf_1),(int)(100*gf_1),0,0); else if(ma_1 gi_9) & (ma_2 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);/*行走过程中转弯速度*/

17、/wait( 0. ); void SubRoutine_2( ) /*趋光*/ gi_3=AI(9)-AI(11); 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. ); /*/void SubRoutine_3( ) /*4号回家*/ /SetMot

18、or(0x1111,-100,-40,100,100); SetMotor(0x1111,(int)(-100*gf_1),(int)(-40*gf_1),100,100); wait( 0. ); DO( 0x, 0 ); ma_9 = 1023 ; resettime(); while(1) ma_8 = AI(7); if(ma_8 gi_8) wait( 0. ); ma_9 = AI(7); if(ma_9 = gi_8) StopMotor(0x1111); break; else tim_1 =seconds(); if(tim_1 gi_9) & (ma_5 ma_10) /S

19、etMotor(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 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

20、),0,0);/*行走过程中转弯速度*/ /wait( 0. ); void SubRoutine_5( ) /*趋光B*/ gi_3=AI(13)-AI(15); 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. ); /*/void SubRoutine_6( ) /*三号回家*/ /SetMotor(0x1111,-100,100,0,0); SetMotor(0x1111,(int)(-100*gf_1),(int)(100*gf_1),0,0); wait( 0. ); StopMotor(0x1111); ma_7 = AI(6); while(1) ma_7 = AI(6); if(ma_7 = 0.