strategysystem.cpp
来自「足球机器人仿真组SimuroSot11vs11的源程序。」· C++ 代码 · 共 857 行 · 第 1/2 页
CPP
857 行
ball.position.x=1032-bal.position.x;
ball.position.y=818-bal.position.y;
home1.position.x=1032-ho1.position.x;
home1.position.y=818-ho1.position.y;
///*
if(home1.angle>0)
home1.angle=ho1.angle-180;
else
home1.angle =180+ho1.angle;
//*/
home2.position.x=1032-ho2.position.x;
home2.position.y=818-ho2.position.y;
///*
if(home2.angle>0)
home2.angle=ho2.angle-180;
else
home2.angle=180+ho2.angle;
//*/
home3.position.x=1032-ho3.position.x;
home3.position.y=818-ho3.position.y;
///*
if(home3.angle>0)
home3.angle=ho3.angle-180;
else
home3.angle=180+ho3.angle;
//*/
home4.position.x=1032-ho4.position.x;
home4.position.y=818-ho4.position.y;
///*
if(home4.angle>0)
home4.angle=ho4.angle-180;
else
home4.angle=180+ho4.angle;
//*/
home5.position.x=1032-ho5.position.x;
home5.position.y=818-ho5.position.y;
///*
if(home5.angle>0)
home5.angle=ho5.angle-180;
else
home5.angle =180+ho5.angle;
//*/
home6.position.x=1032-ho6.position.x;
home6.position.y=818-ho6.position.y;
///*
if(home6.angle>0)
home6.angle=ho6.angle-180;
else
home6.angle =180+ho6.angle;
//*/
home7.position.x=1032-ho7.position.x;
home7.position.y=818-ho7.position.y;
///*
if(home7.angle>0)
home7.angle=ho7.angle-180;
else
home7.angle =180+ho7.angle;
//*/
home8.position.x=1032-ho8.position.x;
home8.position.y=818-ho8.position.y;
///*
if(home8.angle>0)
home8.angle=ho8.angle-180;
else
home8.angle =180+ho8.angle;
//*/
home9.position.x=1032-ho9.position.x;
home9.position.y=818-ho9.position.y;
///*
if(home9.angle>0)
home9.angle=ho9.angle-180;
else
home9.angle =180+ho9.angle;
//*/
home10.position.x=1032-ho10.position.x;
home10.position.y=818-ho10.position.y;
///*
if(home10.angle>0)
home10.angle=ho10.angle-180;
else
home10.angle =180+ho10.angle;
//*/
hgoalie.position.x=1032-hgo.position.x;
hgoalie.position.y=818-hgo.position.y;
///*
if(hgoalie.angle>0)
hgoalie.angle=hgo.angle-180;
else
hgoalie.angle=180+hgo.angle;
//*/
opponent.position1.x=1032-opp.position1.x;
opponent.position2.x=1032-opp.position2.x;
opponent.position3.x=1032-opp.position3.x;
opponent.position4.x=1032-opp.position4.x;
opponent.position5.x=1032-opp.position5.x;
opponent.position6.x=1032-opp.position6.x;
opponent.position7.x=1032-opp.position7.x;
opponent.position8.x=1032-opp.position8.x;
opponent.position9.x=1032-opp.position9.x;
opponent.position10.x=1032-opp.position10.x;
opponent.position11.x=1032-opp.position11.x;
opponent.position1.y=818-opp.position1.y;
opponent.position2.y=818-opp.position2.y;
opponent.position3.y=818-opp.position3.y;
opponent.position4.y=818-opp.position4.y;
opponent.position5.y=818-opp.position5.y;
opponent.position6.y=818-opp.position6.y;
opponent.position7.y=818-opp.position7.y;
opponent.position8.y=818-opp.position8.y;
opponent.position9.y=818-opp.position9.y;
opponent.position10.y=818-opp.position10.y;
opponent.position11.y=818-opp.position11.y;
}
}
void CStrategySystem::Stop(int which)
{
int vL,vR;
vL=vR=0;
Velocity(which,vL,vR);
}
void CStrategySystem::GoaliePosition(int which, CPoint point)
{
OurRobot *robot;
double distance_e;
int dx, dy, desired_angle, theta_e, vL, vR;
switch(which){
case HOME1:
robot = &home1;
break;
case HOME2:
robot = &home2;
break;
case HGOALIE:
robot = &hgoalie;
break;
}
// TRACE("\nin strategy yh1.x=%d,yh1.y=%d",robot->position.x,robot->position.y);
// TRACE("\nin strategy point.x=%d,point.y=%d",point.x,point.y);
dx = point.x - robot->position.x;
dy = point.y - robot->position.y;
distance_e = sqrt(1.0*dx*dx+1.0*dy*dy);
if(dx == 0 && dy == 0)
desired_angle = 90;
else
desired_angle = (int)(180.0/M_PI*atan2((double)(dy), (double)(dx)));
theta_e = desired_angle - robot->angle;
while(theta_e > 180)
theta_e -= 360;
while(theta_e < -180)
theta_e += 360;
//若夹角在（90，180）或（-180，-90）之间，旋转其补角
//倒退计算出的距离
if(theta_e < -90){
theta_e += 180;
distance_e = -distance_e;
}
else if(theta_e > 90){
theta_e -= 180;
distance_e = -distance_e;
}
vL = (int)(20.*(100.0/400.0*distance_e+40.0/90.0*theta_e));
vR = (int)(20.*(100.0/400.0*distance_e-40.0/90.0*theta_e));
// TRACE("\nin strategy vL=%d,vR=%d",vL,vR);
Velocity(which, vL, vR);
}
int CStrategySystem::C_CheckBallPos()
{
if(ball.position.x < (boundRect.left+(boundRect.right-boundRect.left)/4))
{
if(ball.position.y < (boundRect.top+(boundRect.bottom-boundRect.top)/2))
return 1;
else
return 2;
}
else if(ball.position.x > (boundRect.left+(boundRect.right-boundRect.left)/4) && ball.position.x < (boundRect.left+(boundRect.right-boundRect.left)/2))
{
if(ball.position.y < (boundRect.top+(boundRect.bottom-boundRect.top)/2))
return 3;
else
return 4;
}
else if(ball.position.x > (boundRect.left+(boundRect.right-boundRect.left)/2) && ball.position.x < (boundRect.left+3*(boundRect.right-boundRect.left)/4))
{
if(ball.position.y < (boundRect.top+(boundRect.bottom-boundRect.top)/2))
return 5;
else
return 6;
}
else
{
if(ball.position.y < (boundRect.top+(boundRect.bottom-boundRect.top)/2))
return 7;
else
return 8;
}
}
void CStrategySystem::C_KickTo(int which, CPoint pos)
{
OurRobot *robot;
CPoint target, mball;
int dx, dy, theta_d, alpha;
switch(which){
case HOME1:
robot = &home1;
break;
case HOME2:
robot = &home2;
break;
case HOME3:
robot = &home3;
break;
case HOME4:
robot = &home4;
break;
case HOME5:
robot = &home5;
break;
case HOME6:
robot = &home6;
break;
case HOME7:
robot = &home7;
break;
case HOME8:
robot = &home8;
break;
case HOME9:
robot = &home9;
break;
case HOME10:
robot = &home10;
break;
case HGOALIE:
robot = &hgoalie;
break;
}
mball.x = ball.position.x;
mball.y = ball.position.y;
switch(nKick){
case 0: //状态0：处在一般位置，跑到好的位置去射门,定位
dx = mball.x - pos.x;
dy = mball.y - pos.y;//(boundRect.top+boundRect.bottom)/2;
target.x = mball.x + (int)(35.0*dx/sqrt(dx*dx+dy*dy+0.01));//原为25
target.y = mball.y + (int)(35.0*dy/sqrt(dx*dx+dy*dy+0.01));//原为25
Position(which, target); //跑到好的位置
dx = robot->position.x - target.x;
dy = robot->position.y - target.y;
//跑到好的位置，距目标点半径为5的圆内
if(dx*dx+dy*dy < 25)
nKick = 1; //转到状态1
break;
case 1: //状态1：机器人转动一定的角度准备射门
dx = mball.x - robot->position.x; //dx<0
dy = mball.y - robot->position.y; //dy没限制
if(dx == 0 && dy == 0)
alpha = 90;
else
alpha = (int)(180.0/M_PI*atan2((double)dy, (double)dx));
alpha -= robot->angle;
while(alpha > 180)
alpha -= 360;
while(alpha < -180)
alpha += 360;
alpha = abs(alpha);
//如果夹角在预定范围内
if(alpha < 15 || alpha > 180-15){//原为15,后改为8,后改为20
nKick = 2; //转到状态2，准备射门
//ShootVar存放球的位置，是成员变量
ShootVar.x = ball.position.x;
ShootVar.y = ball.position.y;
//ShootLen存放人与球的距离
ShootLen=leng(ShootVar.x,ShootVar.y,robot->position.x,robot->position.y);
}
dx = mball.x - pos.x;
dy = mball.y - pos.y;//(boundRect.top+boundRect.bottom)/2;
if(dx == 0 && dy == 0)
theta_d = 90;
else
theta_d = (int)(180.0/M_PI*atan2((double)(-dy), (double)(-dx)));
Angle(which, theta_d); //what if theta_d>180
/*
//若机器人跑到球的左边（跑过了头）
if(robot->position.x<mball.x)
nKick=6; //转到状态6
*/
break;
case 2: //状态2：射门,shooting the ball
dx = mball.x - robot->position.x;
dy = mball.y - robot->position.y;
if(dx == 0 && dy == 0)
alpha = 90;
else
alpha = (int)(180.0/M_PI*atan2((double)dy, (double)dx));
alpha -= robot->angle;
while(alpha > 180)
alpha -= 360;
while(alpha < -180)
alpha += 360;
alpha = abs(alpha);
//shooting
if(alpha < 90)
Velocity(which, 10, 10);//110
else
Velocity(which, -12, -12);
//若踢球后距离大于踢之前的值，转2，即停止
if(ShootLen<leng(ShootVar.x,ShootVar.y,robot->position.x,robot->position.y))
{
nKick=3;
}
else //更新距离，下一个时刻再踢
ShootLen=leng(ShootVar.x,ShootVar.y,robot->position.x,robot->position.y);
break;
case 3: //状态3：停止
Velocity(which,0,0);
break;
}
}
CPoint CStrategySystem::C_NearestOppFromBall()
{
int minidistance,distance;
CPoint position;
minidistance=CountDistance(opponent.position1,ball.position);
position=opponent.position1;
distance=CountDistance(opponent.position2,ball.position);
if(distance<minidistance)
{
minidistance=distance;
position=opponent.position2;
}
distance=CountDistance(opponent.position3,ball.position);
if(distance<minidistance)
{
minidistance=distance;
position=opponent.position2;
}
distance=CountDistance(opponent.position4,ball.position);
if(distance<minidistance)
{
minidistance=distance;
position=opponent.position2;
}
distance=CountDistance(opponent.position5,ball.position);
if(distance<minidistance)
{
minidistance=distance;
position=opponent.position2;
}
distance=CountDistance(opponent.position6,ball.position);
if(distance<minidistance)
{
minidistance=distance;
position=opponent.position2;
}
distance=CountDistance(opponent.position7,ball.position);
if(distance<minidistance)
{
minidistance=distance;
position=opponent.position2;
}
distance=CountDistance(opponent.position8,ball.position);
if(distance<minidistance)
{
minidistance=distance;
position=opponent.position2;
}
distance=CountDistance(opponent.position9,ball.position);
if(distance<minidistance)
{
minidistance=distance;
position=opponent.position2;
}
distance=CountDistance(opponent.position10,ball.position);
if(distance<minidistance)
{
minidistance=distance;
position=opponent.position2;
}
distance=CountDistance(opponent.position11,ball.position);
if(distance<minidistance)
{
minidistance=distance;
position=opponent.position2;
}
return position;
}
int CStrategySystem::CountDistance(CPoint point1, CPoint point2)
{
int distance;
distance=(int)sqrt(pow(abs(point1.x-point2.x),2)+pow(abs(point1.y-point2.y),2));
return distance;
}
⌨️ 快捷键说明
复制代码Ctrl + C
搜索代码Ctrl + F
全屏模式F11
增大字号Ctrl + =
减小字号Ctrl + -
显示快捷键?