无火不进房间回家程序及说明(广茂达灭火机器人)

传感器接法
     红外传感器接法
       前红外： 数字9
       左红外： 数字15
       左45度角红外： 数字10
       右45度角红外： 数字8
       右红外： 数字14
     火焰传感器接法
       左火焰： 模拟3
       中火焰： 模拟5
       右火焰： 模拟4
     （底部）灰度传感器：  模拟2
     声控传感器：  模拟6
     　　
     程序说明
    
     #define p 120    //定义火焰传感器检测到火焰的返回值
     int k,j,i=0,n,b=1,c=1;   //程序控制变量，不必更改
     int m=i;
     int pro,end=1;
    
     void main()    //主程序
     {
         while(analog(6)>100)   //声控启动
           {
         }
        
         while(!(analog(2)>100))  //走出白色超始区
           {
             motor(0,80); 
             motor(1,80);
         }
        
         pro=start_process(test());  //启动地面标志线检测进程
        
         while(1)    //灭火与迷宫程序切换
           {
             if (analog(3)               {
                 fire();
             }
             else        //没有发现火焰，进入迷宫子程序
               {
                 migong();
             }
         }
     }
    
     void migong()    //迷宫子程序
     {   
         if(digital(8)==0 II digital(9)==0)     //如果前方 或 右45度角红外检测到障碍物，左转
           {
             motor(0,-70);       //根据情况，调节功率参数，以下雷同
             motor(1,70);
         }
         else if(digital(14)==0 && digital(9)==1 && digital(8)==1)  //如果只右方有障碍物，直行
             {
               motor(0,100);
               motor(1,100);
           }
           else        //如果没有障碍物，右转
             {               
               motor(0,100);
               motor(1,-100);
              
               motor(1,10);
               motor(0,90);
           }
         if((i>2)&&(i>m))       //如果标志线数大于2且标志线有变化(针对1、2、3号房间)
           {
             stop();
             while(analog(3)>150 && analog(5)>150 && analog(4)>150)   //如果没有检测到火焰
               {
                 motor(0,70);       //右转
                 motor(1,-70);
                 if(digital(14)==0)       //右红外检测到障碍物，停止转动
                   break;
             }
             m=i+1;         //更改标志线的对比变量
         }
         if(i==1 && b && (analog(3)>150 && analog(5)>150 && analog(4)>150)) //4号房间，检测到第1条标志线，且没有火焰
           {
             while(digital(9)==0 II digital(8)==0 II digital(14)==0)   //任意右手红外传感器有障碍物，右转
               {           
                 motor(0,70);
                 motor(1,-70);
             }
             while(digital(9)==1)       //前方传感器没有障碍物，右前进
               {
                 motor(0,100);
                 motor(1,70);
             }
             b=0;         //控制变量
         }   
     }
    
     void fire()    //灭火子程序
     {
         if((i>4 II i<8) && c)      //如果在2号房间
           {
             while(analog(3)               {
                 motor(0,-60);
                 motor(1,60);
             }
             motor(0,60);        //强制直行一段
             motor(1,60);       //主要是解决2号房间右手法则灭火时的碰墙的情况
             sleep(0.6);
             c=0;
         }
         while(analog(5)<10 && analog(2)<60)     //如果火焰距离合适 且 检测到地面标志线，灭火
           {
             motor(0,-10);       //后退一段
             motor(1,-10);
             sleep(0.15);
             stop();
             motor(2,100);       //灭火
             sleep(2.000);
             stop();
             motor(0,-70);       //灭完火，后退一段
             motor(1,-100);
             sleep(0.2);
            
             kill_process(pro);      //关掉地面标志线检测进程
             select();       //进入回家子程序选择子程序
         }
         while(analog(3)-analog(4)> 3)     //向右调整，如果调整较频繁，修改最后的3
           {
             motor(0,50);
             motor(1,-50);
         }
         while(analog(4)-analog(3)> 3)     //向左调整，同上
           {
             motor(0,-50);
             motor(1,50);
         }
         motor(0,40);       //直行
         motor(1,40);
     }
    
     void test()        //地面标志线检测子程序
     {    
         while(1)
           {
             k=analog(2);
             if(k<30 II k>150)      //30 和 150 分别为底部传感器检测到的的标志线和黑地面
               j=k;        //的值，请根据实际测试值适当修改
             if((j<30) && (n>150))      //如果更改，此处也一并更改
               {
                 i++;
                 beep();
             }
             n=j;
         }
     }
    
     void select()        //回家程序选择子程序
     {
         if(i<3)        //如果地面标志线条数小于3(即在4号房间)
           gohome_4();       //进入4号房间回家子程序，下同
         else if(i<5)
             gohome_3();
           else if(i<8)
               gohome_2();
             else
               gohome_1();
     }
    
     void gohome_4()     //4号房间回家子程序
     {
         start_process(endstop());   //开启终(起)点停止进程序，下同
         mg_right();     //右手法则回家
         stop();     //停止
         sleep(100.0);
     }
    
     void gohome_3()     //3号房间回家子程序
     {
         start_process(endstop());
         mg_left();     //左手法则回家
         stop();
         sleep(100.0);
     }
    
     void gohome_2()     //2号房间的特殊回家程序
     {   
         motor(0,60);    //灭完火后右转一段
         motor(1,-60);
         sleep(0.2);
         reset_system_time();   //系统时间复位
         while(mseconds()<2100L)   //运行左手法则走迷宫2.1秒(时间长短为可以走出房间门口且不到3号房间的走廊)
           {
             while(digital(9)==0 II digital(10)==0)
               {
                 motor(0,60);
                 motor(1,-50);
             }
             while(digital(15)==0 && digital(9)==1 && digital(10)==1)
               {
                 motor(0,50);
                 motor(1,50);
             }
             while(digital(15)==1 && digital(9)==1 && digital(10)==1)
               {
                 motor(0,-80);
                 motor(1,80);
                
                 motor(0,45);
                 motor(1,80);
             }
         }
         stop();
         sleep(0.1);
         while(digital(9)==0)   //如果前方有障碍物，右转(调整姿势)
           {
             motor(0,60);
             motor(1,-60);
         }
         while(digital(9) == 1)   //如果前方没有障碍物，左、右手法则并进，基本上直行到另一端(3号房间旁)
           {
             if(digital(8)==0 )   //右边45度有障碍物，左转
               {
                 motor(0,-50);
                 motor(1,50);
             }
             if(digital(14)==0 && digital(8)==1) //右边有障碍物且右45度角无障碍物，直行
               {
                 motor(0,50);
                 motor(1,50);
             }
             if(digital(10)==0)   //左边45度有障碍物，右转
               {
                 motor(0,50);
                 motor(1,-50);
             }
             if(digital(15)==0 && digital(10)==1) //左边有障碍物且左45度角无障碍物，直行
               {
                 motor(0,50);
                 motor(1,50);
             }
             motor(0,50);    //直行
             motor(1,50);
         }
         start_process(endstop());   //前方有障碍物时，退出循环，开启终点停止进程
         while(end)     //左手法则走迷宫
           {
             if(digital(9)==0 II digital(10)==0)
               {
                 motor(0,60);
                 motor(1,-50);
             }
             else if (digital(15)==0 && digital(9)==1 && digital(10)==1)
               {
                 motor(0,70);
                 motor(1,70);
             }
             else
               {
                 motor(0,-80);
                 motor(1,80);
                
                 motor(0,40);
                 motor(1,100);
             }
         }
         motor(0,-100);    //终点刹车
         motor(1,-100);
         sleep(0.1);
         stop();     //停止
         sleep(100.0);   
     }
    
     void gohome_1()     //1号房间回家
     {
         start_process(endstop());   //开启终点停止进程
         mg_right();     //右手法则走迷宫
         stop();
         sleep(100.0);
     }
    
     void mg_right()     //右手法则走迷宫子程序
     {
         while(end)
           {
             if(digital(8)==0 II digital(9)==0)
               {
                 motor(0,-70);
                 motor(1,70);
             }
             else if(digital(14)==0 && digital(9)==1 && digital(8)==1)
                 {
                   motor(0,100);
                   motor(1,100);
               }
               else
                 {               
                   motor(0,100);
                   motor(1,-100);
                  
                   motor(1,20);
                   motor(0,100);
               }
         }   
     }
    
     void mg_left()     //左手法则走迷宫子程序
     {
         while(end)
           {       
             if(digital(9)==0 II digital(10)==0 )
               {
                 motor(0,60);
                 motor(1,-50);
             }
             else if(digital(15)==0 && digital(9)==1 && digital(10)==1)
               {
                 motor(0,70);
                 motor(1,70);
             }
             else
               {
                 motor(0,-100);
                 motor(1,100);
                
                 motor(0,30);
                 motor(1,100);
             }
         }   
     }
    
     void endstop()     //终点停止进程子程序
     {
         while(1)
           {
             k=analog(2);    //检测1次
             sleep(0.02);    //间隔0.02秒，再检测 //可适当缩短或延长时间
             if(k<50 && analog(2)<50)  //如果2次都是白色，终点到，停止
               {
                 end=0;
                 stop();
                 sleep(1.0);           
             }
         }
     }
    
     //全部程序结束