标题: 篮球防守程序 [打印本页]

---

作者: dlersson    时间: 2016-8-30 07:44
标题: 篮球防守程序
/***前、后复眼分别接EYE0、EYE1*******************/
/***地面灰度 左前 左后 右前 右后 分别接AI0、AI1、AI2、AI3********/
/***前、后左右PSD分别接AI4、AI5、AI6、AI7********/

#include "RobotLib.h"
unsigned int gi_1=0;  /*球相对于机器人的方位标记*/
unsigned int eye_ch_all=0; /*复眼通道*/
unsigned int gi_2=0;  /*拨球时间计数*/
unsigned int gi_3=0;  /*防后退卡死计数1*/
unsigned int gi_4=0;   /*防后退卡死计数2*/
unsigned int gi_5=0;  /*防后退卡死计数3*/
unsigned int gi_6=0;  /*防后退卡死计数4*/

unsigned int ball_light_d=0; /*球的远近比较域值*/    /*正前或正后方火焰值与之比较的值：得出球的远近：是否带上球或碰上球*/
unsigned int backTime_cnt=0;   /**后退时计数比较的域值（相当于时间控制）**/
unsigned int BoQiuTime_cnt=0; /*拨球计数（时间控制）参数*/
unsigned int ball_light_jb=0;/*拨球亮度要求*/
unsigned int psdf=0;  /*前红外测距*/
unsigned int psdb=0;  /*后红外测距*/
unsigned int psdl=0;  /*左红外测距*/
unsigned int psdr=0;  /*右红外测距*/

unsigned int back_psd_distance=0;  /*后红外测距B*/
unsigned int ball_distance=0;  /*球的远近*/  /*正前或正后方火焰值*/

int left_right_psddifferrence=0;  /*左右红外测距之差值*/
unsigned int right_psd=0;   /*右侧红外测距*/
unsigned int left_psd=0;  /*左侧红外测距*/
unsigned int front_eye_2ch,front_eye_4ch,back_eye_9ch,back_eye_11ch;/*前偏左、前偏右、反偏左、反偏右复眼单路*/
unsigned int compass_1=0;  /*指南针*/
unsigned int eye_1=0;  /*复眼亮度最亮值*/
unsigned int psd_ff=0;  /*前红外测距域值*/
unsigned int psd_bb=0;  /*后红外测距域值*/
float time_1=0.0;/*防乌龙转身时间控制*/

float power_speed=0;
unsigned int ball=0;/*有没有球*/
unsigned int frontball=400;/*前拨球单敏探头亮度要求*/
unsigned int backball=400;/*后拨球单敏探头亮度要求*/
unsigned int psd_back_a;
unsigned int psd_back_b;
unsigned int psd_front_a;
unsigned int psd_front_b;


unsigned int psd_back_cum;
unsigned int eye0,eye1,eye2;
unsigned int eye_mnv;//复眼在球处于中近距离阀值 450
int my_EyeChMax()
{
    int ch=0;
    ch=EyeChMax();   
    return ch;
}

int time=0;
int key=0;
float fT;
void Run(int sl,int sr)   //可设置功率,范围-100--100   可通过左下和右下按钮控制电机的停、转
{
    SetMoto(0,sl*power_speed);
    SetMoto(1,sr*power_speed);
    SetMoto(2,-(sl+sr)*power_speed/2);
}
void RunB(int sl,int sr)   //可设置功率,范围-100--100   可通过左下和右下按钮控制电机的停、转
{
    SetMoto(1,-sl*power_speed);
    SetMoto(0,-sr*power_speed);
    SetMoto(2,(sl+sr)*power_speed/2);
}
#define A_GG_FL    0//AI(0)
#define A_GG_FR    1//AI(1)
#define A_GG_BL    2//AI(2)
#define A_GG_BR    3//AI(3)
#define WHITE 0
#define GREEN 1
#define BLACK 2
typedef struct{
  int a;
  int b;  
}GC;
GC gc[2];
int gs;
int g_v[4];
int g_fl_l;
int g_fl_h;
int g_fr_l;
int g_fr_h;
int g_bl_l;
int g_bl_h;
int g_br_l;
int g_br_h;
int a[10];
void GetGG(void)
{
  int i;
  if(AI(A_GG_FL)<g_fl_l)g_v[0]=WHITE;
  else if(AI(A_GG_FL)<g_fl_h)g_v[0]=GREEN;
  else g_v[0]=BLACK;  
   
  if(AI(A_GG_FR)<g_fr_l)g_v[1]=WHITE;
  else if(AI(A_GG_FR)<g_fr_h)g_v[1]=GREEN;
  else g_v[1]=BLACK;  
      
  if(AI(A_GG_BL)<g_bl_l)g_v[2]=WHITE;
  else if(AI(A_GG_BL)<g_bl_h)g_v[2]=GREEN;
  else g_v[2]=BLACK;
         
  if(AI(A_GG_BR)<g_br_l)g_v[3]=WHITE;
  else if(AI(A_GG_BR)<g_br_h)g_v[3]=GREEN;
  else g_v[3]=BLACK;
}
void YueJie(void)
{
  compass_1=Compass_Degree();
  if(compass_1<45||compass_1>315) Run(-70,-70);     
  else if(compass_1<225&&compass_1>135) {Run(70,70);}  
  else if(compass_1>270) Run(50,-80);
  else if(compass_1<90) Run(-80,50);  
  else if(compass_1>=90&&compass_1<=135) {Run(-40,40);}      
  else if(compass_1>=225&&compass_1<=270) {Run(40,-40);}
  else Run(-60,60);
  wait(0.01);
}
void Read_Config(void)
{
    int i;
    for(i=0;i<10;i++)//读存储器0到9编号的值作为参数
    {
        a[i]=FLASH_Read(i);//a[0]为0是表示白色进攻 1是黑色进攻
    }
}

void findball_f( )   /*正方向测球并标记**/
{   
    eye_ch_all=my_EyeChMax();
    if((eye_ch_all<3)&&(eye_ch_all>0))
    {gi_1=2;}
    else if((eye_ch_all>3)&&(eye_ch_all<6))
    {gi_1=1;}
    else if((eye_ch_all>11)||(eye_ch_all<1))
    {gi_1=4;}
    else if((eye_ch_all>5)&&(eye_ch_all<9))
    {gi_1=3;}
    else if((eye_ch_all>8)&&(eye_ch_all<12))
    {gi_1=5;}
    else
    {gi_1=0;}   
}

void findball_b( )   /*反方向测球并标记**/
{   
    eye_ch_all=my_EyeChMax();
    if((eye_ch_all<10)&&(eye_ch_all>7))
    {gi_1=2;}
    else if((eye_ch_all<13)&&(eye_ch_all>10))
    {gi_1=1;}
    else if((eye_ch_all>4)&&(eye_ch_all<8))
    {gi_1=4;}
    else if((eye_ch_all>12)||(eye_ch_all<2))
    {gi_1=3;}
    else if((eye_ch_all>1)&&(eye_ch_all<5))
    {gi_1=5;}
    else
    {gi_1=0;}   
}

int bq_ln;
int bq_rn;
void goal_aa( )     /*正方向进攻*/
{
  //LCDControl(1);
    compass_1=Compass_Degree();
    if(compass_1<30||compass_1>330) power_speed=0.5;
    else power_speed=0.35;
    ball_distance = AI(17);
    psdf = AI(0);
    psdl = AI(2);   
    psdr = AI(3);   
    if((compass_1 <= 90) || (compass_1 >= 270))
    {
      if((compass_1<=90)&&(compass_1>=30)) bq_ln=0;
      else if((compass_1>=270)&&(compass_1<=330)) bq_rn=0;        
        if(psdf>psd_ff && ball_distance > ball_light_jb )      
        {
            if(compass_1 <= 60)
            {
            Run(80,100);
            wait( 0.03);                 
            }                                
            else if(compass_1 >= 300)
            {         
                Run(100,80);
                wait( 0.03);                                                
            }
            else
            {
              Run(100,100);
              wait(0.01);
            }

        }
        else
        {   
            if((ball_distance > ball_light_d)&&((compass_1<20)||(compass_1>340)))
            {
                left_psd = AI(2+4);/*左测距*/
                right_psd = AI(3+4);/*右测距*/
              front_eye_2ch =AI(16);
              front_eye_4ch =AI(18);   
              if(compass_1<10)
              {
                if(left_psd>right_psd)
                {
                  if(front_eye_4ch-front_eye_2ch>0)
                  {
                    if(left_psd>340) Run(100,75);
                    else Run(100,90);
                  }
                  else
                  {
                    Run(95,100);
                  }
                }
          else
          {
                  if(front_eye_2ch-front_eye_4ch>0)
                  {
                    Run(75,100);
                  }
                  else
                  {
                    Run(100,100);
                  }                                 
          }                 
              }
              else if(compass_1>350)
              {
                if(left_psd>right_psd)
                {
                  if(front_eye_2ch-front_eye_4ch>0)
                  {
              Run(100,100);
                  }
                  else
                  {
                    Run(100,75);
                  }
                }
          else
          {
                  if(front_eye_4ch-front_eye_2ch>0)
                  {
              Run(100,95);
                  }
                  else
                  {
                    Run(90,100);
                  }                                 
          }                 
              }
              else if(compass_1>=10&&compass_1<20)   
              {
                if(front_eye_4ch-front_eye_2ch>30)
                {
                  Run(100,95);
                }
                else if(front_eye_2ch-front_eye_4ch>30)
                {
                  Run(80,100);
                }               
              }
              else
              {
                  Run(100,100);               
              }                          
            }
            else
            {
                ////////////////////////////////////////////
              front_eye_2ch =AI(16);
              front_eye_4ch =AI(18);
              if(front_eye_2ch-front_eye_4ch>20)Run(90,100);
              else if(front_eye_4ch-front_eye_2ch>20)Run(100,90);
              else Run(100,100);                                                         
                ////////////////////////////////////////////               
            }
        }
    }
}

void goal_bb( )     /*正方向进攻*/
{
    compass_1=Compass_Degree();   
   
    if(compass_1<210&&compass_1>150) power_speed=0.5;
    else power_speed=0.35;
      
    ball_distance = AI(24);
    psdf = AI(1+4);
    psdl = AI(3+4);   
    psdr = AI(2+4);   
    if((compass_1 <= 270) && (compass_1 >= 90))
    {
      if((compass_1<=270)&&(compass_1>=210)) bq_ln=0;
      else if((compass_1>=90)&&(compass_1<=150)) bq_rn=0;        
        if(psdf>psd_ff && ball_distance > ball_light_jb )      
        {
            if((compass_1 >= 180)&&(compass_1 <= 240))
            {      
                    RunB(80,100);
                    wait( 0.03);                                                
            }
            else if((compass_1 <= 180)&&(compass_1>=120))
            {         
                    RunB(100,80);
                    wait( 0.03);                                               
            }
            else
            {
              RunB(100,100);
              wait(0.01);
            }

        }
        else
        {        
            if((ball_distance > ball_light_d)&&((compass_1<200)&&(compass_1>160)))
            {
                left_psd = AI(3+4);/*左测距*/
                right_psd = AI(2+4);/*右测距*/
              front_eye_2ch =AI(23);
              front_eye_4ch =AI(25);   
              if((compass_1<190)&&(compass_1>=180))
              {
                if(left_psd>right_psd)
                {
                  if(front_eye_4ch-front_eye_2ch>0)
                  {
                    RunB(100,90);
                  }
                  else
                  {
                    RunB(95,100);
                  }
                }
          else
          {
                  if(front_eye_2ch-front_eye_4ch>0)
                  {
                    RunB(75,100);
                  }
                  else
                  {
                    RunB(100,100);
                  }                                 
          }                 
              }
              else if((compass_1>170)&&(compass_1<=180))
              {
                if(left_psd>right_psd)
                {
                  if(front_eye_2ch-front_eye_4ch>0)
                  {
              RunB(100,100);
                  }
                  else
                  {
                    RunB(100,75);
                  }
                }
          else
          {
                  if(front_eye_4ch-front_eye_2ch>0)
                  {
              RunB(100,95);
                  }
                  else
                  {
                    RunB(90,100);
                  }                                 
          }                 
              }
              else if(compass_1>=10&&compass_1<20)   
              {
                  RunB(80,100);              
              }
              else
              {
                  RunB(100,80);                 
              }                          
            }
            else
            {
                ////////////////////////////////////////////
              front_eye_2ch =AI(23);
              front_eye_4ch =AI(25);
              if(front_eye_2ch-front_eye_4ch>20)RunB(90,100);
              else if(front_eye_4ch-front_eye_2ch>20)RunB(100,90);
              else RunB(100,100);                                                         
                ////////////////////////////////////////////               
            }
        }
    }
    else
    {
          bq_ln=0;
          bq_rn=0;     
    }
//  LCDControl(0);
}

void part_1()   /*0~90角度一*/
{
  int n;
    findball_f ();
   
    if(gi_1 == 0)
    {
        goal_aa();   
    }
    else
    {
      power_speed=0.45;
      eye0=AI(16);eye1=AI(17);eye2=AI(18);
        if(gi_1 == 1)
        {
            if(eye_ch_all==4)   
            {
                Run(100,25);   
            }
            else
            {
                Run(100,-20);   
            }
        }
        else
        {
            if(gi_1 == 2)
            {
                if(eye_ch_all==2)   
                {
                Run(25,100);     
                }
                else
                {
                    Run(-20,100);   
                }                 
            }
            else
            {
                if((gi_1 == 3) || (gi_1 == 4))
                {
                    if(EyeInMax()<eye_mnv+70)                  
                    {
                      compass_1=Compass_Degree();
                      psdf = AI(0+4);
            psdb = AI(1+4);   

            if(compass_1<15&&DI(1)==0&&psdb>150)
            {
              if(gi_1==3)Run(80,-80);
              else Run(-80,80);
              fT=seconds();
               while(1)
               {
                 compass_1=Compass_Degree();
                 if(compass_1>45&&compass_1<315) break;
                 if(seconds()>fT+1.5) break;
               }
            }
                      if((compass_1<15)&&(psdf>psd_front_a)&&(psdf<psd_front_b)&&psdb<150)                  
                      {
                        psd_back_cum++;
                        if(psd_back_cum>5)
                        {
                          //printf("\n\n\n\n\nrun -50,50");
                          Run(-40,40);
                          wait(0.1);
                          //psd_back_cum=0;            
                          return;                    
                        }
                      }                                                                                                                           
                      else
                      {
                        psd_back_cum=0;
                      }               
                        if(gi_1 == 3)
                        {
                          compass_1=Compass_Degree();
                          //if(eye_ch_all==6) power_speed=0.9;
                            if((compass_1 >=45) && (compass_1 <= 90))
                            {
                                Run(-100,-70);      
                                wait( 0.0100 );
                            }
                            else if(eye_ch_all <8)
                            {
                                Run(-100,-90);      
                                wait( 0.0100 );
                            }
                            else
                            {
                                Run(-100,-100);   
                                wait( 0.0100 );
                            }
                            power_speed=0.98;
                           
                        }
                        else
                        {
                          compass_1=Compass_Degree();
   
                          Run(-100,-100);                    
                            wait( 0.0100 );
                            power_speed=0.98;
                        }
                    }               
                    else
                    {
                      if(gi_1 == 3)
                      {
                          Run(25,-25);   
                          wait(0.080000);                        
                      }
                      else
                      {
                          Run(-25,25);   
                          wait( 0.080000 );                        
                      }
                    }
                }                        
                else          //5////////////////////////
                {               //21 22 23 24 25 26 27
                                //7   8  9 10 11 12 13
                  power_speed=0.55;           
                    eye_1 = EyeInMax();
                    if(eye_1 < ball_light_d)
                     {
                        //if(AI(5)>frontball){Run(80,-100);}
                        //else Run(-100,-50);
                        if((eye_1<eye_mnv-100))
                        {
                          if((AI(26)>AI(24))) Run(-100,-75);
                          else Run(-75,-100);                     
                        }
                        else
                        {
                          if((AI(26)>AI(24))) Run(-100,-50);
                          else Run(-50,-100);        
                        }
                        wait(0.01);
                    }
                    else
                    {
                        Run(80,-100);   
                        wait( 0.03000 );
                    }
                }
            }
        }
    }
//    LCDControl(0);
}

void part_4()/*270~359角度四*/
{
  int n;
    findball_f ();
   
    if(gi_1 == 0)
    {
        goal_aa();
    }
    else
    {
        power_speed=0.45;
      eye0=AI(16);eye1=AI(17);eye2=AI(18);
        if(gi_1 == 1)
        {                                 
            if(eye_ch_all==4)   
            {
              Run(100,25);   
            }
            else
            {
                Run(100,-20);   
            }                       
        }
        else
        {
            if(gi_1 == 2)
            {
                if(eye_ch_all==2)   
                {
                  Run(25,100);   
                }
                else
                {
                    Run(-20,100);   
                }
            }
            else
            {
                if((gi_1 == 3) || (gi_1 == 4))
                {
                    if(EyeInMax()<eye_mnv+70)                  
                    {
                      compass_1=Compass_Degree();
                      psdf = AI(0+4);
            psdb = AI(1+4);   
            
            if(compass_1>345&&DI(1)==0&&psdb>150)
            {
               if(gi_1==3)Run(80,-80);
               else Run(-80,80);
               fT=seconds();
               while(1)
               {
                 compass_1=Compass_Degree();
                 if(compass_1>45&&compass_1<315) break;
                 if(seconds()>fT+1.5) break;
               }            
            }
                     
                      if((compass_1>345)&&(psdf>psd_front_a)&&(psdf<psd_front_b)&&psdb<150)                  
                      {
                        psd_back_cum++;
                        if(psd_back_cum>5)
                        {
                          //printf("\n\n\n\n\nrun -50,50");
                          Run(-40,40);
                          wait(0.1);
                          //psd_back_cum=0;            
                          return;                    
                        }
                      }                                                                                            
                      else
                      {
                        psd_back_cum=0;
                      }               
                        if(gi_1 == 4)
                        {
                          compass_1=Compass_Degree();
                          //if(eye_ch_all==6) power_speed=0.9;
                            if((compass_1 >=270) && (compass_1 <= 315))
                            {
                                Run(-70,-100);      
                                wait( 0.0100 );
                            }
                            else if(eye_ch_all==0 || eye_ch_all==13)
                            {
                                Run(-90,-100);      
                                wait( 0.0100 );
                            }
                            else
                            {
                                Run(-100,-100);   
                                wait( 0.0100 );
                            }
                            power_speed=0.98;
                           
                        }
                        else
                        {
                          compass_1=Compass_Degree();
                          //eye_1 = EyeInMax();
                         // if(eye_ch_all==0) power_speed=0.8;
                          if( AI(20)>eye_mnv && compass_1<330 )
                          {
                            Run(-70,-100);                        
                          }
                          else
                          {
                            Run(-100,-100);
                          }                           
                            wait( 0.0100 );
                            power_speed=0.98;
                        }
                    }               
                    else
                    {
                      if(gi_1 == 3)
                      {
                          Run(25,-25);   
                          wait(0.080000);                        
                      }
                      else
                      {
                          Run(-25,25);   
                          wait( 0.080000 );                        
                      }
                    }
                }                                 
                else
                {
//21 22 23 24 25 26 27
//7   8  9 10 11 12 13            
          power_speed=0.55;      
                    eye_1 = EyeInMax();
                    if(eye_1 < ball_light_d)
                     {
                        if((eye_1<eye_mnv-100))
                        {
                          Run(-75,-100);                     
                        }
                        else
                        {
                          if((AI(26)>AI(24))) Run(-100,-50);
                          else Run(-50,-100);        
                        }
                        wait(0.01);
                    }
                    else
                    {
                        Run(80,-100);   
                        wait( 0.03000 );
                    }
                }
            }
        }
    }
//LCDControl(0);
}

void part_2()    /**90~180角度二**/
{
  int n;
    findball_b();
   
    if(gi_1 == 0)
    {
        goal_bb();
    }
    else
    {
    power_speed=0.45;
      eye0=AI(23);eye1=AI(24);eye2=AI(25);
        if(gi_1 == 1)
        {
            if(eye_ch_all==11)  //if(AI(24)>AI(25))
            {
                Run(-25,-100);   
            }
            else
            {
                Run(20,-100);   
            }  
        }
        else
        {
            if(gi_1 ==2)
            {
                if(eye_ch_all==9)   //if(AI(22)>AI(21))
                {            
                  Run(-100,-25);  
                }
                else
                {
                    Run(-100,20);   
                }
            }
            else
            {
                if((gi_1 == 3) || (gi_1 == 4))
                {
                    if(EyeInMax()<eye_mnv+70)                  
                    {
                      compass_1=Compass_Degree();
                      psdf = AI(0+4);
            psdb = AI(1+4);   
            if(compass_1>165&&DI(0)==0&&psdf>150)
            {           
               if(gi_1==3)Run(0,-90);
               else Run(-90,0);
               fT=seconds();
               while(1)
               {
                 compass_1=Compass_Degree();
                 if(compass_1>225||compass_1<135) break;
                 if(seconds()>fT+1.5) break;
               }
            }
                      if((compass_1>165)&&(psdb>psd_back_a)&&(psdb<psd_back_b)&&psdf<150)                  
                      {
                        psd_back_cum++;
                        if(psd_back_cum>5)
                        {
                          //printf("\n\n\n\n\nrun -50,50");
                          RunB(-40,40);
                          wait(0.05);
                          //psd_back_cum=0;            
                          return;                    
                        }
                      }                                                                                                  
                      else
                      {
                        psd_back_cum=0;
                      }               
                        if(gi_1 == 4)
                        {
                          compass_1=Compass_Degree();
                            if((compass_1 >=90) && (compass_1 <= 135))
                            {
                                RunB(-70,-100);      
                                wait( 0.0100 );
                            }
                            else if(eye_ch_all == 6 || eye_ch_all == 7)
                            {
                                RunB(-90,-100);      
                                wait( 0.0100 );
                            }
                            else
                            {
                                RunB(-100,-100);   
                                wait( 0.0100 );
                            }
                            power_speed=0.98;
                           
                        }
                        else
                        {
                          compass_1=Compass_Degree();
                          //eye_1 = EyeInMax();
                         // if(eye_ch_all==0) power_speed=0.8;
                          if( AI(27)>eye_mnv && compass_1<150)
                          {
                            RunB(-70,-100);                        
                          }
                          else
                          {
                            RunB(-100,-100);
                          }                           
                            wait( 0.0100 );
                            power_speed=0.98;
                        }
                    }               
                    else
                    {
                      if(gi_1 == 3)
                      {
                          RunB(25,-25);   
                          wait(0.080000);                        
                      }
                      else
                      {
                          RunB(-25,25);   
                          wait( 0.080000 );                        
                      }
                    }
                }                        
                else          //5////////////////////////
                {               //21 22 23 24 25 26 27
                                //7   8  9 10 11 12 13
                  power_speed=0.55;
                    eye_1 = EyeInMax();
                    if(eye_1 < ball_light_d)
                     {
                        //if(AI(5)>frontball){Run(80,-100);}
                        //else Run(-100,-50);
                        if((eye_1<eye_mnv-100))
                        {
                          if((AI(18)>AI(16))) RunB(-100,-75);
                          else RunB(-75,-100);                     
                        }
                        else
                        {
                          if((AI(18)>AI(16))) RunB(-100,-50);
                          else RunB(-50,-100);        
                        }
                        wait(0.01);
                    }
                    else
                    {
                        RunB(80,-100);   
                        wait( 0.03000 );
                    }
                }
            }
        }
    }
//    LCDControl(0);
}

void part_3()    /**180~270角度三**/
{
  int n;
    findball_b();
   
    if(gi_1 == 0)
    {
        goal_bb();   
    }
    else
    {
        power_speed=0.5;
      eye0=AI(23);eye1=AI(24);eye2=AI(25);
        if(gi_1 == 1)
        {
            if(eye_ch_all==11)  //if(AI(24)>AI(25))
            {
              Run(-25,-100);  
            }
            else
            {
                Run(20,-100);   
            }
        }
        else
        {
            if(gi_1 == 2)
            {
                if(eye_ch_all==9)   //if(AI(22)>AI(21))
                {
                  Run(-100,-25);  
                }
                else
                {
                    Run(-100,20);   
                }
            }
            else
            {
                if((gi_1 == 3) || (gi_1 == 4))
                {
                    if(EyeInMax()<eye_mnv+70)                  
                    {
                      compass_1=Compass_Degree();
                      psdf = AI(0+4);
            psdb = AI(1+4);   
            if(compass_1<195&&DI(0)==0&&psdf>150)
            {           
               if(gi_1==3)Run(0,-90);
               else Run(-90,0);
               fT=seconds();
               while(1)
               {
                 compass_1=Compass_Degree();
                 if(compass_1>225||compass_1<135) break;
                 if(seconds()>fT+1.5) break;
               }
            }         
            //printf("%d %d %d %d\n",compass_1,psdf,psdb,psd_back_cum);
                      if((compass_1<195)&&(psdb>psd_back_a)&&(psdb<psd_back_b)&&psdf<150)                  
                      {
                        psd_back_cum++;
                        if(psd_back_cum>5)
                        {
                          //printf("\n\n\n\n\nrun -50,50");
                          RunB(-80,80);
                          wait(0.3);
                          //psd_back_cum=0;            
                          return;                    
                        }
                      }                                                                                                      
                      else
                      {
                        psd_back_cum=0;
                      }               
                        if(gi_1 == 3)
                        {
                          compass_1=Compass_Degree();
                            if((compass_1 >=225) && (compass_1 <= 270))
                            {
                                RunB(-100,-70);      
                                wait( 0.0100 );
                            }
                            else if(eye_ch_all ==0 || eye_ch_all == 13)
                            {
                                RunB(-100,-90);      
                                wait( 0.0100 );
                            }
                            else
                            {
                                RunB(-100,-100);   
                                wait( 0.0100 );
                            }
                            power_speed=0.98;
                           
                        }
                        else
                        {
                          compass_1=Compass_Degree();
                          if( AI(21)>eye_mnv && compass_1>210)
                          {
                            RunB(-100,-70);                        
                          }
                          else
                          {
                            RunB(-100,-100);
                          }                           
                            wait( 0.0100 );
                            power_speed=0.98;
                        }
                    }               
                    else
                    {
                      if(gi_1 == 3)
                      {
                          RunB(25,-25);   
                          wait(0.080000);                        
                      }
                      else
                      {
                          RunB(-25,25);   
                          wait( 0.080000 );                        
                      }
                    }
                }                        
                else          //5////////////////////////
                {               //21 22 23 24 25 26 27
                                //7   8  9 10 11 12 13
                  power_speed=0.55;           
                    eye_1 = EyeInMax();
                    if(eye_1 < ball_light_d)
                     {
                        if((eye_1<eye_mnv-100))
                        {
                          RunB(-75,-100);                     
                        }
                        else
                        {
                          if((AI(18)>AI(16))) RunB(-100,-50);
                          else RunB(-50,-100);        
                        }
                        wait(0.01);
                    }
                    else
                    {
                        RunB(80,-100);   
                        wait( 0.03000 );
                    }
                }
            }
        }
    }
//  LCDControl(0);
}


void Run_back()
{
    compass_1=Compass_Degree();
    if((compass_1 <20)||(compass_1 > 340))
     {psdb=AI(1+4);if(psdb<120)Run(-90,-90);else if(psdb<180)Run(-40,-40);else if(psdb>300)Run(20,20);else Run(0,0);}
    else if((compass_1 >160)&&(compass_1 < 200))
    {psdf=AI(0+4);if(psdf<120)Run(90,90);else if(psdf<180)Run(40,40);else if(psdf>300)Run(-20,-20);else Run(0,0);}
   
    else if((compass_1 >= 20) && (compass_1 < 30)){Run(-16,16);}               
    else if((compass_1 >=330) && (compass_1 <= 340)){Run(16,-16);}
    else if((compass_1 >= 30) && (compass_1 < 60)){Run(-30,30);}
    else if((compass_1 >= 300) && (compass_1 < 330)){Run(30,-30);}   
    else if((compass_1 >= 60) && (compass_1 < 90)){Run(-50,50);}
    else if((compass_1 >= 270) && (compass_1 <300)){Run(50,-50);}   
     
    else if((compass_1 >= 90) && (compass_1 < 120)){Run(50,-50);}
    else if((compass_1 >= 240) && (compass_1 <270)){Run(-50,50);}   
    else if((compass_1 >= 120) && (compass_1 < 150)){Run(30,-30); }
    else if((compass_1 >= 210) && (compass_1 <240)){Run(-30,30);}   
    else if((compass_1 >= 150) && (compass_1 < 160)){Run(16,-16);}   
    else if((compass_1 >200) && (compass_1 <210)){Run(-16,16);}   
}
void GetDelay(void)
{
  float t;
  t=seconds();
  while(1)
  {
    if(seconds()>t+5) {break;}
    if(DI(13)==1)break;  
  }
}




void InitParam(void)
{  
  int a0,a1,b0,b1,c0,c1,d0,d1;
  Read_Config();  
  ///////////////////////>>>>>>>>>>新场地需手动输入
  a0=150;//左前地面灰度白色值
  a1=475;//左前地面灰度黑色值
  b0=220;//右前地面灰度白色值
  b1=760;//右前地面灰度黑色值  
  c0=140;//左后地面灰度白色值
  c1=410;//左后地面灰度黑色值
  d0=165;//右后地面灰度白色值
  d1=520;//右后地面灰度黑色值
  ////////////////////////<<<<<<<<<<
  
  g_fl_l=a0+60;
  g_fl_h=a1-60;
  g_fr_l=b0+60;
  g_fr_h=b1-60;
  g_bl_l=c0+60;
  g_bl_h=c1-60;
  g_br_l=d0+60;
  g_br_h=d1-60;  

  if(a[0]%2==0) gs=0;//白色进攻 //调整系统存储器0号的值为双数则白色防守
  else gs=1;         //黑色进攻 //调整系统存储器0号的值为单数则白色防守
  gc[0].a=WHITE;
  gc[0].b=BLACK;  
  gc[1].a=BLACK;
  gc[1].b=WHITE;  
  
  LCDControl(1);
  if(gs==0)
  printf("\n\n\n\n-当前设定为白色场地-\n--进入防守程序--\n\n");
  else
  printf("\n\n\n\n-当前设定为黑色场地-\n--进入防守程序--\n\n");      
  
}


void main()
{        
    InitParam();  
    ball=150;   /*有没有球*/
    ball_light_d =500 ;   /*球很近近比较域值:防乌龙距离、*/
    psd_ff=400;    /*前红外测距域值,300*/
    psd_bb=400;    /*后红外测距域值,300*/
    ball_light_jb=550;/*拨球亮度要求*/
  
    power_speed=0.98;/*功率值，范围0.0~1.0*/   
   
    psd_front_a=160;//280
    psd_front_b=250;   
    psd_back_a=480;//365
    psd_back_b=550;
    psd_back_cum=0;
    eye_mnv=450;
    gi_1=0;
    GetDelay();
    LCDControl(0);   
    /************************/   
    while(1)
    {
        while(EyeInMax()<ball)
        {Run_back();}
        compass_1=Compass_Degree();
        GetGG();
    //printf("c=%d g=%d \nAI(5)=%d DI(1)=%d\n%d %d\n",compass_1,gi_1,AI(5),DI(1),g_v[2],g_v[3]);  
    if(g_v[2]==gc[gs].b||g_v[3]==gc[gs].b) YueJie();  
        else if((compass_1 >= 0) && (compass_1 <= 90))     
        {
            part_1();
        }
        else
         {
            if((compass_1 >=270) && (compass_1 <= 359))      
            {
                part_4();
            }
            else
            {
                if((compass_1 > 90) && (compass_1 <= 180))        
                {
                    part_2();
                }
                else
                {
                    if((compass_1 > 180) && (compass_1 < 270))        
                    {
                        part_3() ;
                    }
                    else
                    {
                        Run(0,0);
                    }
                }
            }
        }      
    }
}

---

欢迎光临 (http://www.51hei.com/bbs/)

Powered by Discuz! X3.1