📄 action.cpp
字号:
// RegulateAngle(omiga);
if(vl>125)vl=125;
if(vl<-125)vl=-125;
if(vr>125)vr=125;
if(vr<-125)vr=-125;
double angle = (vr - vl)/2;
RegulateAngle(omiga); //应该没有大于180 的角速度罢
omiga += ANGLE_A*(ANGLE_B* angle-omiga);
if( vr > vl )
{
if( vl >= 0 || vr <=0 )
{
omiga -= 4 * ANGLE_K * angle * angle;
}
}
else if( vr < vl )
{
if( vr >= 0 || vl <=0 )
{
omiga += 4 * ANGLE_K * angle * angle;
}
}
RegulateAngle(omiga); //应该没有大于180 的角速度罢
return omiga;
}
//
/*VelocityOne 计算在当前速度speed的基础上以左右轮速vl,vr控制，下一个周期达到的速度
返回 下一个周期达到的速度
*/
//
double VelocityOne(double speed,double vl,double vr)
{
if(vl>125)vl=125;
if(vl<-125)vl=-125;
if(vr>125)vr=125;
if(vr<-125)vr=-125;
if(speed > 3 || speed < -3)
speed =0;
if( vl==0 && vr==0 )
speed += -SPEED_ODD * speed;
else
speed += SPEED_A*( SPEED_B*(vl +vr)/2-speed);
return speed;
}
//
/*Count 计算robot跑道pos位置的周期数
*/
//
int Count( Environment *env, int robot,Vector3D pos)
{
Mydata * p;
p=(Mydata *)env->userData;
double v=0,s=0;
double distance=0;
int n=0;
double turnangle=0,vangle=0;
double posangle=0,sangle=0;
double dx,dy;
dx = pos.x - p->robot[robot].pos.x ;
dy = pos.y - p->robot[robot].pos.y ;
distance = Distance(p->robot[robot].pos , pos);
posangle = Atan(dy,dx);
//omiga
turnangle = posangle - p->robot[robot].rotation;
RegulateAngle(turnangle);
if(turnangle > 90)
{
turnangle-=180;
}
else if(turnangle < -90)
{
turnangle+=180;
}
double omiga=p->robot[robot].rotation - p->myoldpos[robot].z ;
RegulateAngle(omiga);
if( turnangle <0 )
{
turnangle = - turnangle;
if( omiga < 0)
omiga= -omiga;
}
do{
omiga=AngleOne(omiga,-120,120);
sangle+=omiga;
n++;
}while(sangle < turnangle);
n--;
//omiga
v= sqrt(p->myspeed[robot].x * p->myspeed[robot].x + p->myspeed[robot].y*p->myspeed[robot].y);
vangle = p->myspeed[robot].z - posangle;
RegulateAngle(vangle);
if( vangle <-90 || vangle > 90 )
v=-v;
do{//跑位的步数
//还没有考虑反向 是怎么办呢
v=VelocityOne(v,110,110);
s+=v;
n++;
}while(s < distance);
n--;
return n;
}
//
/*Goalie 让robot守门
*/
//
void Goalie ( Environment *env, int robot )
{//先校正姿态，再去拦球
Mydata * p;
p=(Mydata *)env->userData;
Vector3D go;
double OX= FLEFT - (GTOP - GBOT); // 该点为球门中心 向后移动半个球门
double OY= (FTOP + FBOT)/2; //球门中心
double ballx=p->curball.x ;
double bally=p->curball.y ;
double gx = p->robot[robot].pos.x ;
double gx_outline = FLEFT + 2.2; //对x坐标的限定，防止离球门线太远了
double gx_inline = FLEFT + 1.8;
double gy = p->robot[robot].pos.y; //跑位点,初值为当前位置
double goalline = FLEFT + 3;
bool notout= true; //不出击
bool standby = true; //限制x 坐标
bool XX=false; //是否旋转
if( XX && ballx < gx + 0.5 )
{//旋转
if(bally < gy && bally > gy-4 )
{
Velocity(env,robot,-125,125);
return ;
}
else if(bally > gy && bally < gy+4)
{
Velocity(env,robot,125,-125);
return ;
}
}
gy = OY + ( goalline - OX ) * (bally - OY)/(ballx - OX);
if(notout)
{
if(gy > GTOP+3)
gy = GTOP+3;
else if(gy < GBOT-3)
gy = GBOT-3;
}
go.y = gy ;
if(standby)
{
if(gx > gx_outline )
gx = gx_outline;
else if(gx < gx_inline)
gx = gx_inline;
}
go.x = gx ;
GoaliePosition(env,robot,go,90,1.5);
}
void Goalie ( Environment *env, int robot,Vector3D pos)
{//先校正姿态，再去拦球
Mydata * p;
p=(Mydata *)env->userData;
Vector3D go;
PredictBall(env,2);
double ballx=p->preball.x;
double bally=p->preball.y;
double gx =pos.x;
double gx_outline = pos.x+ 2.2;
double gx_inline = pos.x+ 1.8;
double gy =p->preball.y;
double goalline = pos.x+ 3;
bool standby = true; //限制x 坐标
if(ballx < gx +3&&fabs(p->robot[robot].pos.y-bally)<4){
Kick(env,robot,CONSTGATE);
return ;
}
go.y = gy ;
if(standby)
{
if(gx > gx_outline )
gx = gx_outline;
else if(gx < gx_inline)
gx = gx_inline;
}
go.x = gx ;
GoaliePosition(env,robot,go,90,1.5);
}
//////
/*Meetball_p 求出robot追到球的最佳位置
*/
//
Vector3D Meetball_p( Environment *env, int robot)
{//求出robot追到球的位置
Mydata * p;
p=(Mydata *)env->userData;
Vector3D meetpoint={0,0,-1};
double dis=Distance(p->curball,p->robot[robot].pos);
double t =0 ;
double vb=0;
double v=1.9; //按照最大速度计算
double pos_angle,b_sp_angle;
pos_angle = Atan(p->curball.y - p->robot[robot].pos.y , p->curball.x - p->robot[robot].pos.x);
b_sp_angle = p->ballspeed.z ;
vb = (p->ballspeed.y * p->ballspeed.y + p->ballspeed.x * p->ballspeed.x);
t = sin((b_sp_angle - pos_angle) * PI /180);
t = vb* t*t;
v=v*v;
if( v > t )
{
v = sqrt( v - t) + sqrt( vb ) * cos((b_sp_angle - pos_angle) * PI /180);
if( v > 0.1)
{
t = dis /v; //得到步数
meetpoint.x = p->ballspeed.x *t + p->curball.x ;
meetpoint.y = p->ballspeed.y *t + p->curball.y ;
meetpoint.z = t;
}
}
return meetpoint;
}
//////
/*Meetball_y 求出该队员和求相遇的 y 坐标
maxn用于限制追到球的最长的时间（周期数）
*/
//
Vector3D Meetball_y( Environment *env ,double X,double maxn)
{//求出该队员和求相遇的 y 坐标
Mydata * p;
p=(Mydata *)env->userData;
double dx = X -p->curball.x;
Vector3D meetpoint={0,0,-1};
if( dx > 0 )
{//球在 左方,目标在右边
if(p->ballspeed.x > 0.1)
meetpoint.z = dx / p->ballspeed.x;
}
else
{//球在 右边 ,目标在左方 //dx < 0
if(p->ballspeed.x < -0.1)
meetpoint.z = dx / p->ballspeed.x;
}
if( meetpoint.z > 0 )
{
meetpoint.x = X;
meetpoint.y = p->ballspeed.y * meetpoint.z + p->curball.y;
}
return meetpoint;
}
//////
/*Meetball_x 求出该队员和求相遇的 x 坐标
maxn用于限制追到球的最长的时间（周期数）
*/
//
Vector3D Meetball_x( Environment *env ,double Y,double maxn)
{//求出该队员和求相遇的 x 坐标
Mydata * p;
p=(Mydata *)env->userData;
double dy = Y -p->curball.y;
Vector3D meetpoint={0,0,-1};
if( dy > 0 )
{//球在 下方,目标在上
if(p->ballspeed.y > 0.01)
meetpoint.z = dy / p->ballspeed.y;
}
else
{//球在 上方 ,目标在下 //dx < 0
if(p->ballspeed.y < -0.01)
meetpoint.z = dy / p->ballspeed.y;
}
if( meetpoint.z > 0 )
{
meetpoint.y = Y;
meetpoint.x = p->ballspeed.x * meetpoint.z + p->curball.x ;
}
return meetpoint;
}
//////
/*Within 判断robot队员和球的距离是否再LENGTH规定的范围内
返回true or false
*/
//
bool Within(Environment* env,int robot,double LENGTH)
{
Mydata* p;
p=(Mydata*)env->userData;
const double steps=50;
int who=robot;
double dis;
Vector3D ballgo={0,0,0};
Vector3D robotgo={0,0,0};
Vector3D ball=p->curball;
ballgo.x = ball.x + steps * p->myspeed[who].x;
ballgo.y = ball.y + steps * p->myspeed[who].y;
dis=Distance(ballgo,p->robot[robot].pos);
if(dis < LENGTH)
{
return true;
}
return false;
}
bool oppWithin(Environment* env,int robot,double LENGTH)
{
Mydata* p;
p=(Mydata*)env->userData;
const double steps=50;
int who=robot;
double dis;
Vector3D ballgo={0,0,0};
Vector3D robotgo={0,0,0};
Vector3D ball=p->curball;
ballgo.x = ball.x + steps * p->opspeed[who].x;
ballgo.y = ball.y + steps * p->opspeed[who].y;
dis=Distance(ballgo,p->opp[robot].pos);
if(dis < LENGTH)
{
return true;
}
return false;
}
/*
prdictopp 预测经过steps个周期之后对方机器人的位置
函数返回结果：经过steps个周期之后球的位置
*/
Vector3D PredictOpp(Environment *env,int robot,int steps)
{
Mydata * p;
p=(Mydata *)env->userData;
Vector3D predictopp;
Vector3D oppspeed;
int i=0;
predictopp.x = p->opp[robot].pos.x; //赋初值
predictopp.y = p->opp[robot].pos .y;
oppspeed.x = p->opspeed[robot].x ;
oppspeed.y = p->opspeed[robot].y ;
for(i=0;i<steps;i++)
{
predictopp.x += oppspeed.x ;
predictopp.y += oppspeed.y ;
}
return predictopp;
}
////////////
/* PredictBall 预测经过 steps 个周期之后球的位置
函数返回结果：经过 steps 个周期之后球的位置
考虑撞墙的过程
*/
////////////
void PredictBall(Environment *env,int steps)
{
Mydata * p;
p=(Mydata *)env->userData;
Vector3D predictball;
Vector3D ballspeed;
int i=0;
predictball.x = p->curball.x; //赋初值
predictball.y = p->curball.y;
ballspeed.x = p->ballspeed.x ;
ballspeed.y = p->ballspeed.y ;
ballspeed.z = p->ballspeed.z ;
for(i=0;i<steps;i++)
{
predictball.x += ballspeed.x ;
predictball.y += ballspeed.y ;
//处理撞墙
if( predictball.x > FRIGHT )
{
predictball.x -= ballspeed.x ; //retern
predictball.y -= ballspeed.y ;
ballspeed.x *=-SPEED_NORMAL; //loose
ballspeed.y *= SPEED_TANGENT;
predictball.x += ballspeed.x ; //go on
predictball.y += ballspeed.y ;
}
else if( predictball.x < FLEFT )
{
predictball.x -= ballspeed.x ; //retern
predictball.y -= ballspeed.y ;
ballspeed.x *=-SPEED_NORMAL; //loose
ballspeed.y *= SPEED_TANGENT;
predictball.x += ballspeed.x ; //go on
predictball.y += ballspeed.y ;
}
else if( predictball.y < FBOT )
{
predictball.x -= ballspeed.x ; //retern
predictball.y -= ballspeed.y ;
ballspeed.x *= SPEED_TANGENT; //loose
ballspeed.y *=-SPEED_NORMAL;
predictball.x += ballspeed.x ; //go on
predictball.y += ballspeed.y ;
}
else if( predictball.y > FTOP )
{
predictball.x -= ballspeed.x ; //retern
predictball.y -= ballspeed.y ;
ballspeed.x *= SPEED_TANGENT; //loose
ballspeed.y *=-SPEED_NORMAL;
predictball.x += ballspeed.x ; //go on
predictball.y += ballspeed.y ;
}
if( predictball.x + predictball.y > FRIGHT +FTOP -CORNER)
{//右上
double vx,vy;
vy=0.7071*ballspeed.y + 0.7071*ballspeed.x; //变换1
vx=-0.7071*ballspeed.y + 0.7071*ballspeed.x;
predictball.x -= ballspeed.x ; //retern
predictball.y -= ballspeed.y ;
vx *= SPEED_TANGENT; //loose
vy *=-SPEED_NORMAL;
ballspeed.y = 0.7071 * vy - 0.7071 * vx; //变换2
ballspeed.x = 0.7071 * vy + 0.7071 * vx;
predictball.x += ballspeed.x ; //go on
predictball.y += ballspeed.y ;
}
else if( predictball.x + predictball.y < FLEFTX +FBOT+CORNER)
{//左下
double vx,vy;
vy=0.7071*ballspeed.y + 0.7071*ballspeed.x; //变换1
vx=-0.7071*ballspeed.y + 0.7071*ballspeed.x;
predictball.x -= ballspeed.x ; //retern
predictball.y -= ballspeed.y ;
vx *= SPEED_TANGENT; //loose
vy *=-SPEED_NORMAL;
ballspeed.y = 0.7071 * vy - 0.7071 * vx; //变换2
ballspeed.x = 0.7071 * vy + 0.7071 * vx;
predictball.x += ballspeed.x ; //go on
predictball.y += ballspeed.y ;
}
else if( predictball.x - predictball.y > FRIGHT -FBOT -CORNER)
{//右下
double vx,vy;
vy=0.7071*ballspeed.y - 0.7071*ballspeed.x; //变换1
vx=0.7071*ballspeed.y + 0.7071*ballspeed.x;
predictball.x -= ballspeed.x ; //retern
predictball.y -= ballspeed.y ;
vx *= SPEED_TANGENT; //loose
vy *=-SPEED_NORMAL;
ballspeed.y = 0.7071 * vy + 0.7071 * vx; //变换2
ballspeed.x = -0.7071 * vy + 0.7071 * vx;
predictball.x += ballspeed.x ; //go on
predictball.y += ballspeed.y ;
}
else if( predictball.y - predictball.x > FTOP - FLEFT-CORNER)
{//左上
double vx,vy;
vy=0.7071*ballspeed.y - 0.7071*ballspeed.x; //变换1
vx=0.7071*ballspeed.y + 0.7071*ballspeed.x;
predictball.x -= ballspeed.x ; //retern
predictball.y -= ballspeed.y ;
vx *= SPEED_TANGENT; //loose
vy *=-SPEED_NORMAL;
ballspeed.y = 0.7071 * vy + 0.7071 * vx; //变换2
ballspeed.x = -0.7071 * vy + 0.7071 * vx;
predictball.x += ballspeed.x ; //go on
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -