goodvanguard.c

机器人足球赛RoboCup中型组,一个比较好的前锋的算法

#include <stdio.h>
#include "ingenious.h"
int receivedata1=0;
int eagle = 0;
int AD_1 = 0;
int AD_2 = 0;
int AD_3 = 0;
int AD_4 = 0;
int AD_5 = 0;
int AD_6 = 0;
int AD_7 = 0;
int AD_8 = 0;
void init();
void read();
void find_ball();
void tiqiu();
void raoqiu();
void main()
{
     while(1)
     {   
          AD_1 = AD(1);
          AD_2 = AD(2);
          AD_3 = AD(3);
          AD_4 = AD(4);
          AD_5 = AD(5);
          AD_6 = AD(6);
          AD_7 = AD(7);
          AD_8 = AD(8);
         Mprintf(1,"       AD1=%d",AD_1);
          Mprintf(1,"AD3=%d",AD_3);
          Mprintf(3,"       AD5=%d",AD_5);
          Mprintf(7,"       AD4=%d",AD_4);
          Mprintf(5,"       AD7=%d",AD_7);
          Mprintf(5,"AD6=%d",AD_6);
          Mprintf(7,"AD8=%d",AD_8);
        find_ball();
        
          
     }
}
void find_ball()
{
	if(AD_1<75&& AD_4<60&& AD_3<60 &&AD_7<60&&AD_8<60)
	{
		move(100,-100,0);
		
	}
	if(AD_3>60||AD_4>60)
	{       /*init();
            read();*/
           if(eagle>20||eagle<310)
	        {
	         tiqiu();
	        }
	     else
	        {
	          raoqiu();
	        }
        }
        else
            {
		if(AD_1>75)
		{
			move(250,-250,0);
			sleep(500);
			stop();
			sleep(500);
		}
		if(AD_7>60)
		{
			move(-250,250,0);
			sleep(500);
			stop();
			sleep(500);
		}
		if(AD_8>60)
		{
			move(250,-250,0);
			sleep(800);
			stop();
			sleep(500);
		}
		if(AD_5>60)
		{
			move(-250,250,0);
			sleep(300);
			stop();
			sleep(500);
		}
		if(AD_6>60)	
		{
			move(250,-250,0);
			sleep(300);
			stop();
			sleep(500);
		}
        }
}

void tiqiu()
{
 if(AD_3>500&&AD_4>500)
   {
    move(400,400,0);
   }
	
   else
       {
       if(AD_3<500||AD_4<500)
          {
	       if(AD_3-AD_4>50)
	         {
	          move(400,350,0);
	         }
	          else
	             {
	             if(AD_4-AD_3>50)
	               {
		            move(350,400,0);
	                }
	              else
		               { move(400,400,0);}
				
	            }
		
          }
        }
}
void raoqiu()
{
 if(AD_3>500&&AD_4>500)
   {
    move(400,250,0);
    sleep(1500);
    move(250,400,0);
   sleep(2000);
   }
	
   else
       {
       if(AD_3<500||AD_4<500)
        {
	      if(AD_3-AD_4>50)
	      {
	       move(400,350,0);
	      }
	       else
	           {
	           if(AD_4-AD_3>50)
	              {
		           move(350,400,0);
	               }
	                else
		            { move(400,400,0);
		            }
				
	          }
		
        }
       }
 }
 void init()
{
	serial_init(9600);
	serial_send(2,0x00C0);
	receivedata1=serial_receive(2);
	while(receivedata1!=0x0043)
    {
	   receivedata1=serial_receive(2);
	}
	     
	serial_send(2,0x00C5);
	receivedata1=serial_receive(2);
	while(receivedata1!=0x0045)
	{
	   receivedata1=serial_receive(2);
	}
}
void read()
{
		serial_send(2,0x0091);
		receivedata1=serial_receive(2);
		while(receivedata1==0x0100)
		{
			receivedata1=serial_receive(2);
		}
		eagle=receivedata1;
		eagle <<=8;
		receivedata1=serial_receive(2);
		while(receivedata1==0x0100)
		{
			receivedata1=serial_receive(2);
		}
		receivedata1 &= 0x00FF;
		eagle &= 0x0FF00;
		eagle |= receivedata1;
		Mprintf(7,"receive=%d",eagle);
		sleep(80);
		Clr_Screen();
}