小车源代码.nqc

Legao机器人识别道路行走的代码

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//变量预定义
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
#define black1  32   //预定义1号光传感器的黑色感光值
#define black3  36   //预定义3号光传感器的黑色感光值
#define white1 51    //预定义1号光传感器的黑色感光值
#define white3 49    //预定义3号光传感器的白色感光值  
#define lineTime 40     //预定义小车直线行走调整时的等待时间
#define cornTime 10	   //预定义小车转弯时的调整等待时间
#define corn2Time 20    //预定义小车后退时的调整等待时

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//定义全局变量
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
int counter;//纪录小车在所在的纵向路线上已经走过的“十字交叉”的数目
int state1;
//state1 = 0: 小车已经搜索完，当前小车的所在路线
//state1 = 1: 小车已经到达过当前小车所在路线的一端
//state1 = 2: 小车没有到达过当前小车所在路线的任意一端
int state2;
//state2 = 0:小车在纵线路线上                              state2 = 1;小车向右转后，在横向路线上


//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//功能：主任务main()
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
task main()
{
     SetSensor(SENSOR_1,SENSOR_LIGHT);//设置1号光传感器
     SetSensor(SENSOR_3,SENSOR_LIGHT);//设置3号光传感器
     SetSensor(SENSOR_2,SENSOR_TOUCH);//设置2号触碰传感器
     OnFwd(OUT_A+OUT_C);
     //设置初始值
     counter=0;
     state1=2;
     state2=0;
     //启动任务
     start line;
     start turn_corn1;
     start turn_corn2;
     start reverse;
}


///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//功能：当小车走到一条跑道的末尾时，控制小车后退
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
task reverse()
{
    while(true)
   {
        if(counter==4)//counter = 4:小车走到第四个“十字交叉”处
        {
	     //停止任务line,turn_corn1,turn_corn2
            stop line;
            stop turn_corn1;
            stop turn_corn2;
	     //小车后退50 / 100秒
            OnRev(OUT_A+OUT_C);
            Wait(50);
	     //控制小车向后转，直到车轮A和C调转方向后，再次位于黑色跑道两侧
            OnFwd(OUT_A);
            until(SENSOR_3 <= black3 + 5);
            until(SENSOR_3 > black3 + 7 && SENSOR_1 > black1 + 7);
	     //将counter置零重新计数
            counter=0;
            state1--;
	     //将重新启动任务line,turn_corn1,turn_corn2
            start line;
            start turn_corn1;
            start turn_corn2;
        }
   }
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//功能：控制小车沿着黑色路线直线行走
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
task line()
{
    while(true)
    {
        OnFwd(OUT_A+OUT_C); 
	 //轮子A进入了黑色路线区域
        if(SENSOR_1<=black1+5 && SENSOR_3>black3+7)
        {
             Wait(lineTime);
	      //控制小车向左转，直到小车轮子A走出黑色路线区域
             OnRev(OUT_A);
             until(SENSOR_1>black1+5);
	      //调整好方向后，小车直线前进
             OnFwd(OUT_A);
        }
	 //轮子C进入了黑色路线区域
	 if(SENSOR_3<=black3+5 && SENSOR_1>black3+7)
        {
             Wait(lineTime);
	      //控制小车向右转，直到小车轮子C走出黑色路线区域
             OnRev(OUT_C);
             until(SENSOR_3>black3+5);
	      //调整好方向后，小车直线前进
             OnFwd(OUT_C);
        }
     }
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//功能：小车搜索完一条路线后，控制小车在“十字交叉”处，向右转，进入下一条路线继续搜索
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
task turn_corn1()
{
    while(true)
    {
	 //小车进入“十字交叉”处
        if(SENSOR_1 <= black1 + 5 && SENSOR_3 <= black3 + 5)
        {
	    //停止任务line,turn_corn2,reverse
           stop line;
           stop turn_corn2;
           stop reverse;
	    
           OnFwd(OUT_A+OUT_C);
           if(counter!=3 || (counter == 3 && state1 == 0))
                     Wait(lineTime);

	    //小车已经搜索完当前小车所在的纵向路线
           if(state1 == 0)
           {
		   PlayTone(262,40);
		   //小车在纵向路线上
                 if(state2 == 0)
                 {
                     Wait(cornTime);
			//小车向右转，直到小车两个车轮在横向方向的路线上
                     OnRev(OUT_C);
			counter++;//小车所走的纵向方向路线的“十字交叉”个数加一
                     until(SENSOR_3 > black3 + 7);
                     until(SENSOR_3 <= black3 + 5);
                     until(SENSOR_3 > black3 + 7 && SENSOR_1 > black1 + 7);
                     //设置小车在横向方向路线上的标记
                     state2=1;
                 }
                 else
                 {//小车在横向方向的路线上
                     Wait(cornTime);
			//小车向左转，直到小车的两个车轮A和C在新的纵向方向的路线上
                     OnRev(OUT_A);
			//设置小车在纵向方向的标记和搜索状态的
                     state2=0;
                     state1=2;
                     until(SENSOR_1 >  black1 + 7);
                     until(SENSOR_1 <= black1 + 5);
                     until(SENSOR_3 > black3 + 7 && SENSOR_1 > black1 + 7);
                 }
		   //启动任务line,turn_corn2,reverse
                 start line;
                 start turn_corn2;
                 start reverse;
           }//end of if
           else
           {
		   //启动任务line,turn_corn2,reverse
                start line;
                start turn_corn2;
                start reverse;
                counter++;//小车所走的纵向方向路线的“十字交叉”个数加一
	         //设置小车在通过“十字交叉”处发出声音
                PlayTone(262,40);
           }//end of else
        }//end of if
    }//end of while
}

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//功能：控制小车在横向路线上碰到“障碍”时，按原路线返回
//小车在纵向路线上碰到“障碍”任务完成，停止搜索
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
task turn_corn2()
{
    while(true)
    {
	 //小车在横向路线上碰到“障碍”时，按原路线返回
        if(SENSOR_2 == 1 && state1==0)
        {
		//停止任务line,turn_corn1,reverse
             stop line;
             stop turn_corn1;
             stop reverse;
	      //小车向后退，直到退到纵向方向上的“十字交叉”处
             OnRev(OUT_A + OUT_C);
      	      until(SENSOR_1 <= black1 + 5 && SENSOR_3 <= black3 + 5);
             OnFwd(OUT_A + OUT_C);
             Wait(corn2Time);
	      //小车向左转，重新调整到原来纵向方向的路线和方向，继续前进
             OnRev(OUT_A);
             Wait(20);
             until(SENSOR_1 <= black1 + 5);
             until(SENSOR_3 > black3 + 7 && SENSOR_1 > black1 + 7);
             //设置小车在纵向方向上的标记
             state2 = 0;
	      //启动任务line,turn_corn1,reverse
             start line;
             start turn_corn1;
             start reverse;
        }
	 //小车在纵向路线上碰到“障碍”任务完成，停止搜索
        if(SENSOR_2 == 1 && state1>0)
        {
	      //停止任务line,turn_corn1,reverse
             stop line;
             stop turn_corn1;
             stop reverse;
             //关闭发动机
             Off(OUT_A+OUT_C);
             break;
        }
   }
}