/***前、后复眼分别接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);
}
}
}
}
}
}
|
QQ好友和群
QQ空间
腾讯微博
腾讯朋友
收藏
淘帖
顶
踩
