corenalia.jc

机器人守门员程序,能够左右调整并回到中间

/*改动*/
int white=120/*130*/,black=204;

int grand=0;
int dui=white;
int ball=230;
int z_flag=0;
/*改动结束*/



int hui,north,zuo_ir,you_ir,qian_ir,a,b,c,d,e,f,i,j,r=30,k=0;
/*int zuo1=7,you1=4,z_qian1=1  ,y_qian1=0,zhong1=6,y_hou1=5,you_ir1=0,zuo_ir1=1,qian_ir1=2;*/
int zuo,you,z_qian,y_qian,z_hou,y_hou,zhong,dai_line=40;

int sx=0;

int a1=360-grand;
/*塑轮定义*/
int zuo1=1,you1=7,z_qian1=5,gr=2,y_qian1=6,zhong1=0,you_ir1=1,zuo_ir1=2,qian_ir1=0;
/*塑轮定义结束*/
/*
前左1
前右4
左前7
右前0
中6
右后5
*/

void main()
{
 printf("I am running.......");
 
 while(1)
  {
	find();
		
	if(dir())
	{
      
	  while(zhong<ball)
	  {
	 	daiqiu(100);/**/
	 	ball_det();
	 
	  }
	  ball_det(); 
	  if(zhong<30)
	     {
	        kaobian();
	     }
	     ball_det();
	     hui=gr_det();
	     if(((dui=black&&hui>=black)||(dui=white&&hui<=white))&&zhong<30)
	     {
	  
	      dixian();
	     }
	  }
	  
	
	else
	{
	/*自己球门*/
	beep();
	ball_det();
	ball_det1();
	a=0;
	while(zhong<=ball&&zhong>20&&zhong<y_qian&&zhong<z_qian)/**/
	{
		a=1;
		daiqiu(30);
		ball_det();
	}
	if(a==1)
	{
	/*drive(-dai_line,0);
	wait(0.1);*/
	/*stop();*/
	a=0;
	}
	zhuanxiang();
	
	}

  }



}
int ir_detect_pa(int port)
{ 
	int val1, val2, val3;
	/* port值的允许范围是[1,2]  */

		
    /* 检测接收到的背景红外信号 */
    val1 = digitalport(port);	            
	/* 打开左红外发射传感器, PD2口 */
  	bit_set(0x1008,0b0100);       
    msleep(1L);                  
    /* 检测接收到的红外信号 */
    val3 = digitalport(port);               
    /* 关闭左红外发射传感器 */
    bit_clear(0x1008,0b0100);          
    
   	/* 打开右红外发射传感器, PD3口 */   
	bit_set(0x1008,0b1000);            
	msleep(1L);                 
	/* 检测接收到的红外信号 */
    val2 = digitalport(port);             
    /* 关闭所有红外发射传感器 */
   	bit_clear(0x1008,0b1100);	      
   
	return ((val1 & ~val2) << 1) | (val1 & ~val3);
}




int gr_det()
{
  return analogport(gr);
}



int north_det()
{
  return (a1+2*read(0x4000))%360;
}

int dir()

{
    north=north_det();/**/
  
     if(north>=90&&north<=270)
     {
        return 0;
     }
  
     else
     {
     return 1;
     
     }
  
  
 
}

void ball_det()
{
  zuo=analogport(zuo1);
  you=analogport(you1);
  zhong=analogport(zhong1);
/* zhong=min(zuo,you);*/
}
void daiqiu(int dai_line1)
{
   int flag=0;
  drive(dai_line1,0);
   ball_det();
   ball_det1();
   if(zhong>z_qian||zhong>y_qian)
   {
     if(zhong>y_qian)
     {
        ball_det();
        ball_det1();
        
         while(zhong>y_qian)
         {
          motor(2,-20);/*0*/
          motor(1,dai_line1+30);
          ball_det();
          ball_det1();
         }
          drive(dai_line1,0);
     
     }
     if(zhong>z_qian)
     {
     	ball_det();
        ball_det1();
        while(zhong>z_qian)
        {
        	motor(1,-20);/*0*/
        	motor(2,dai_line1+30);
        	ball_det();
            ball_det1();
        
        
        
        }
     	drive(dai_line1,0);
     
     
     }
   }
   
  if(zhong<=ball&&zuo<ball&&you<ball&&(zuo>20&&you>20))
     {
        if((zuo>you)&&abs(zuo-you)>10)
     		{
     			motor(2,0);
     			motor(1,dai_line1+20);
     			wait(0.1);
     			drive(dai_line1,0);
     		}
     	if((zuo<you)&&abs(zuo-you)>10)
     	    {
     	        motor(1,0);
     			motor(2,dai_line1+20);
     			wait(0.1);
     			drive(dai_line1,0);
     	    }
   
   		
     }
   

      
   
}
 
int ir_det(int port)
{
  if(port==0)
  {
  		return ir_detector();
  }
  else
  {
        return ir_detect_pa(port);
  }

}
void kaobian()
{
   drive(20,0);
   wait(0.5);
   
      north=north_det();
      if(north<180)
      {
   		ni();
   		resettime();
   		while(north>20)
   		{
   		if (seconds()>0.5)
   		{
   		break;
   		}
   		north=north_det();
   		}
   		stop();
   		find_s1();
   		   
      }
      
      else
      {
        shun();
        qian_ir=ir_det(qian_ir1);
         resettime();
         while((north<330))
         {
         	if(seconds()>0.5)
         	{
         	break;
         	
         	}
         	north=north_det();
         	
                  
         }
         stop();
         find_n1();
         
      
      }
   
   

}
void ni()
{
drive(0,-50);

}
void shun()
{
drive(0,50);

}
void find_n()
 {
   int z_flag=0;
   ball_det();
   ball_det1();
   qian_ir=ir_det(qian_ir1);
   while((zhong>ball||zhong>z_qian)&&qian_ir==0)
   
  {
   
   drive(0,-20);
   ball_det();
   ball_det1();
   qian_ir=ir_det(qian_ir1);
   z_flag=1;
  
  }
 /* if(z_flag)
  {
  drive(0,20);
  wait(0.1);
  stop();
  z_flag=0;
  }*/
  drive(50,0);
}
void find1()
 {
   int z_flag=0;
   ball_det();
   ball_det1();
   resettime();
   while(zhong>ball||zhong>y_qian)
   
     { 
       zuo_ir=ir_det(zuo_ir1);
       you_ir=ir_det(you_ir1);
       qian_ir=ir_det(qian_ir1);
      /* if(zuo_ir)
       {*/
       drive(0,50);
	   /*}
	   else
	   {
	   drive(20,20);
	   
	   }*/
	   ball_det1();
       ball_det();
       z_flag=1;
       
       /*if(seconds()>0.4);
       {
        resettime();
         qian_ir=ir_det(qian_ir1);
         if(qian_ir)
        {
         
         drive(-60,0);
         wait(0.1);
        }
        
       }*/
       
     }
  if(z_flag)
  {
  drive(0,-20);
  wait(0.2);
  stop();
  z_flag=0;
  }
  drive(30,0);
}
/******************************************************/
void find()
{
int z_flag1=0,y_flag1=0;
   ball_det();
   ball_det1();
   resettime();
if(z_flag==0)
{
  while(zhong>ball||zhong>(z_qian+10))
   
     { 
 
       drive(0,-50);
	   ball_det1();
       ball_det();
       y_flag1=1;
   
     }
   if(y_flag1)
     {
        drive(0,-20);
        wait(0.2);
        stop();
        y_flag1=0;
  }
  z_flag=1;
  drive(30,0);
  

}
else
{
   while(zhong>ball||zhong>(y_qian+10))
   
     { 

       drive(0,50);
	   ball_det1();
       ball_det();
       z_flag1=1;
   
     }
   if(z_flag1)
     {
        drive(0,20);
        wait(0.2);
        stop();
        z_flag1=0;
  }
  z_flag=0;
  drive(30,0);
  
}




}
/******************************************************/
void find_s()
 {
   int z_flag=0;
   ball_det();
   ball_det1();
  qian_ir=ir_det(qian_ir1);
   while((zhong>ball||zhong>y_qian)&&qian_ir==0)
   
     { 
       drive(0,40);
	   ball_det1();
       ball_det();
       z_flag=1;
       qian_ir=ir_det(qian_ir1);
       
       
     }
  /*if(z_flag)
  {
  drive(0,-20);
  wait(0.1);
  stop();
  z_flag=0;
  }*/
  drive(50,0);
}

/**************************************************/
void find_n1()
 {
   int z_flag=0;
   ball_det();
   ball_det1();
   qian_ir=ir_det(qian_ir1);
   while((zhong>ball||zhong>z_qian)&&qian_ir==0)
   
  {
   
   drive(0,-40);/**/
   ball_det();
   ball_det1();
   z_flag=1;
  qian_ir=ir_det(qian_ir1);
  }
 /* if(z_flag)
  {
  drive(0,20);
  wait(0.1);
  stop();
  z_flag=0;
  }
  stop();*/
  drive(50,0);
}

void find_s1()
 {
   int z_flag=0;
   ball_det();
   ball_det1();
  qian_ir=ir_det(qian_ir1);
   while((zhong>ball||zhong>y_qian)&&qian_ir==0)
   
     { 
       drive(0,40);/**/
	   ball_det1();
       ball_det();
       z_flag=1;
       
       qian_ir=ir_det(qian_ir1);
       
     }
 
  drive(50,0);
}



/*****************************************************/




void ball_det1()
{
z_qian=analogport(z_qian1);
y_qian=analogport(y_qian1);
}

void dixian()
{
 int var_d=20,ir_z,ir_y,ir_zh;
 drive(20,0);
   wait(0.5);
/* drive(100,0);
 wait(0.5);
 stop();
 drive(-20,0);
 wait(0.1);
 stop();*/
   
   
      north=north_det();
      if(north>270)
   		  {
   		  
   		      resettime();
   		      while(north<350)
   		      {
   		         if(seconds()>0.5)
   		          {
   		            
   		            
   		            resettime();
   		            
   		            var_d=var_d+20;
   		 				
   		          }
   		          
   		          motor(2,-var_d);/*40*/
   		          motor(1,var_d);
   		         
   		          north=north_det();
   		       }
   		       stop();
   		       sx=1;
   		       ir_z=ir_det(zuo_ir1);
   		       ir_y=ir_det(you_ir1);			
   		    }	
      else
   		{
   		  resettime();
   		   while(north>10)
   		   {
   		     if(seconds()>0.5)
   		      {
   		        
   		        
   		        resettime();
   		        
   		        var_d=var_d+20;
   		 		
   		       }
   		       drive(0,-var_d);
   		       north=north_det();
   		   }
   		   stop();
   		   sx=2;
               ir_z=ir_det(zuo_ir1);
   		       ir_y=ir_det(you_ir1);
   		}
   		
   		
   	/*	  north=north_det();
   		
   		  
   		  
   		  stop();
   		  wait(0.2);*/
   		  
   		 
   		
   		ir_z=ir_det(zuo_ir1);
   		ir_y=ir_det(you_ir1);
   		if((ir_z&&(ir_y==0))||sx==1)
   		{
   		  
   		   resettime();
   		   while(north>270||north<60)/**/
   		   {
   		     if(seconds()>1.0)/*1.0*/
   		     {
   		       
   		        resettime();
   		        var_d=var_d+20;
   		 
   		     }
   		     drive(0,var_d);
   		     north=north_det();
   		
   		  } 
   		   find_n1();
   		   motor(1,60);/*drive(90,-10)*/
   		   motor(2,100);/**/
   		   wait(0.5);
   		}
   		else if(((ir_z==0)&&ir_y)||sx==2)
   		{
   		
   		   resettime();
   		   while(north<90||north>300)/**/
   		   {
   		     if(seconds()>0.5)
   		     {
   		       
   		        resettime();
   		        var_d=var_d+20;
   		 
   		     }
   		     motor(1,-var_d);/*10*/
   		     motor(2,var_d);
   		     north=north_det();
   		
   		    } 
   		  find_s1();
   		  motor(1,100);/**/
   		  motor(2,60);/*drive(90,10)*/
   		  wait(0.5);
   		
   		
   		}
 
   
    




}
void zhuanxiang()
{

   north=north_det();
   if(north<180)
   {
   		a=0;
   		resettime();
   		ball_det();
   		ball_det1();
   		while(north<225&&zhong<z_qian)
   		{
   			if(seconds()>0.5)
   			{
   				break;
   			
   			}
   			shun();
   			north=north_det();
   			ball_det();
   			ball_det1();
   			a=1;
   		}
   		ball_det();
   		ball_det1();
   		
   		if(zhong>z_qian)/**/
   		{
   		motor(1,47);/**/
   		motor(2,53);
   		wait(0.5);/**/
   		find_n1();
   		}
   		else
   		{
   		find();
   		}
   }
	else
	{
	
	a=0;
   		resettime();
   		ball_det();
   		ball_det1();
   		while(north>135&&zhong<y_qian)
   		{
   			if(seconds()>0.5)
   			{
   				break;
   			
   			}
   			ni();
   			north=north_det();
   			ball_det();
   			ball_det1();
   			a=1;
   		}
   		ball_det();
   		ball_det1();
   		
   		if(zhong>y_qian)/**/
   		{
   		motor(1,53);/**/
   		motor(2,47);
   		wait(0.5);/**/
   		find_s1();
   		}
   		else
   		{
   		find();
   		}
	
	
	}
	
}