📄 vsv.cpp
字号:
double distance;
double turnangle,posangle,vangle;
double dx,dy;
double a=SPEED_A;
double b=SPEED_B;
double v,v1;
double f=vmax;
double s=0;
int n=0;
bool face=true; //判断小车是否是正面前进
v= sqrt(p->myspeed[robot].x * p->myspeed[robot].x + p->myspeed[robot].y*p->myspeed[robot].y);
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);
turnangle = p->robot[robot].rotation - posangle; //think more!!
RegulateAngle(turnangle);
if(turnangle > 90)
{
face=false;
turnangle-=180;
}
else if(turnangle < -90)
{
face=false;
turnangle+=180;
}
else
{
face=true;
}
vangle = p->myspeed[robot].z - p->robot[robot].rotation;
RegulateAngle(vangle);
if( vangle <-90 || vangle > 90 )
v=-v;
v1=v;
if(distance > Limiteddis )
{//it is too early to count the steps
if(turnangle > Limitedangle || turnangle < -Limitedangle)
{//adjust angle
/////////////////测试这一段
if(turnangle > 20 || turnangle < -20)
anglespeedmax = 0;
else if(turnangle > 10 || turnangle < -10)
anglespeedmax = 125;
else if(turnangle > 5 || turnangle < -5)
anglespeedmax = 180;
else
anglespeedmax = 200;
///////////////测试这一段
PAngle(env,robot,posangle,anglespeedmax);
}
else
{
if(face)
Velocity(env,robot,f,f);
else
Velocity(env,robot,-f,-f);
}//it is time to rush
}
else
{
if(distance > 1)
{ //调整角度 return!!!!!!
if(turnangle > Limitedangle || turnangle < -Limitedangle)
{
Angle(env,robot,posangle);
return ;
}
}
if(distance < 1)
{ //停止并转向 return!!!!!!
if( v<0.5 && v>-0.5)
{
Velocity(env,robot,-Xangle,Xangle);
return ;
}
}
if(true)
{
vmax=125;
if(face)
{
f=-vmax; //减速度 为 0000000
v1=VelocityOne(v1,-f,-f); //加速一步
s=v1;
do{//whether to reduce
if(v1 > SPEED_ZERO) //as i said,this is limited
v1=VelocityOne(v1,0,0);
else
v1=VelocityOne(v1,f,f);
s+=v1;
}while( v1 > 0 );
s-=v1;
if(s < distance)
{//不满足减速条件加速
Velocity(env,robot,-f,-f);
}
else
{
if(v > SPEED_ZERO)
Velocity(env,robot,0,0);
else
{
v1=VelocityOne(v,f,f); //减速一步
if( v1 < 0 )
{
do{//该降低功率了
f++; //f=-vmax;
v1 = VelocityOne(v,f,f);
}while( v1 < distance && f < vmax);
}
Velocity(env,robot,f,f);
}
}
}
else
{
f=vmax; //减速度!!!!!
v1=VelocityOne(v1,-f,-f);
s=v1;
do{//whether to reduce
if(v1 < -SPEED_ZERO) //as i said,this is limited
v1=VelocityOne(v1,0,0);
else
v1=VelocityOne(v1,f,f);
s+=v1;
}while( v1 < -0.1 );
s-=v1;
if(s > -distance)
{//不满足减速条件加速
Velocity(env,robot,-f,-f);
}
else
{
if(v < -SPEED_ZERO)
Velocity(env,robot,0,0);
else
{
v1=VelocityOne(v,f,f); //减速一步
if( v1 > 0 )
{
do{//该降低功率了
f--; //f=-vmax;
v1 = VelocityOne(v,f,f);
}while( v1 > -distance && f > -vmax);
}
Velocity(env,robot,f,f);
}
}
}
}
}
}
//
/*PositionBallX 让robot 跑到pos，并且停下来原地旋转，
Xangle 旋转的角速度
limit 控制球和robot的距离
如果球和队员的距离大于limit则不旋转
*/
//
void PositionBallX(Environment *env,int robot,Vector3D pos ,double Xangle,double limit)
{
Mydata * p;
p=(Mydata *)env->userData;
double anglespeedmax=0; //控制转交速度的变量
double vmax=125;
double Limitedangle=2;
if( limit <2.8 )
limit =2.8;
double Limiteddis=limit;
double distance;
double turnangle,posangle,vangle;
double dx,dy;
double a=SPEED_A;
double b=SPEED_B;
double v;
double f=vmax;
bool face=true; //判断小车是否是正面前进
bool turnornot=false ; //是否旋转,临时变量
v= sqrt(p->myspeed[robot].x * p->myspeed[robot].x + p->myspeed[robot].y*p->myspeed[robot].y);
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);
turnangle = p->robot[robot].rotation - posangle; //think more!!
RegulateAngle(turnangle);
if(turnangle > 90)
{
face=false;
turnangle-=180;
}
else if(turnangle < -90)
{
face=false;
turnangle+=180;
}
else
{
face=true;
}
vangle = p->myspeed[robot].z - p->robot[robot].rotation;
RegulateAngle(vangle);
if(distance < 3.2)
turnornot = true;
else if(distance < 3.5 && v > 0.5 )
turnornot = true ;
else if(distance < 4.5 && v > 0.8)
turnornot = true ;
if(distance > Limiteddis ) //不在旋转范围内 则不转
turnornot = false ;
if(turnornot)
{//满足条件 转!!!
Velocity(env,robot,-Xangle,Xangle);
}//否则跑位
else if(turnangle > Limitedangle || turnangle < -Limitedangle)
{//adjust angle
/////////////////测试这一段
if(turnangle > 60 || turnangle < -60)
anglespeedmax = 0;
else if(turnangle > 30 || turnangle < -30)
anglespeedmax = 100;
else if(turnangle > 10 || turnangle < -10)
anglespeedmax = 150;
else
anglespeedmax = 200;
///////////////测试这一段
PAngle(env,robot,posangle,anglespeedmax);
}
else
{
if(face)
Velocity(env,robot,f,f);
else
Velocity(env,robot,-f,-f);
}//it is time to rush
}
//
/*PositionAndThrough 让robot以最快MAX 冲向pos，中间没有减速控制
*/
//
void PositionAndThrough(Environment *env,int robot,Vector3D pos ,double MAX)
{
Mydata * p;
p=(Mydata *)env->userData;
double anglespeedmax=0; //控制转交速度的变量
double max=MAX;
double Limitedangle=2;
double Limiteddis=0;
double distance;
double turnangle,posangle,vangle;
double dx,dy;
double a=SPEED_A;
double b=SPEED_B;
double v,v1;
double f;
double s=0;
int n=0;
bool face=true; //判断小车是否是正面前进
v= sqrt(p->myspeed[robot].x * p->myspeed[robot].x + p->myspeed[robot].y*p->myspeed[robot].y);
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);
turnangle = posangle - p->robot[robot].rotation; //think more!!
RegulateAngle(turnangle);
if(turnangle > 90)
{
face=false;
turnangle-=180;
}
else if(turnangle < -90)
{
face=false;
turnangle+=180;
}
else
{
face=true;
}
vangle = p->myspeed[robot].z - posangle;
RegulateAngle(vangle);
if( vangle <-90 || vangle > 90 )
v=-v;
v1=v;
if(distance > Limiteddis)
{//it is too early to count the steps
if(turnangle > Limitedangle || turnangle < -Limitedangle)
{//adjust angle
/////////////////测试这一段
if(turnangle > 20 || turnangle < -20)
anglespeedmax = 0;
else if(turnangle > 10 || turnangle < -10)
anglespeedmax = 125;
else if(turnangle > 5 || turnangle < -5)
anglespeedmax = 180;
else
anglespeedmax = 200;
///////////////测试这一段
PAngle(env,robot,posangle,anglespeedmax);
}
else
{
f=max;
if(face)
Velocity(env,robot,f,f);
else
Velocity(env,robot,-f,-f);
}//it is time to rush
}
else
{
}//abserlutely count
}
//
/*AngleOne 计算在当前角速度omiga的基础上以左右轮速vl,vr控制，下一个周期达到的角速度
返回 下一个周期达到的角速度
*/
//
double AngleOne(double omiga,double vl,double vr)
{
// omiga = p->robot[i].rotation - p->myoldpos[i].z ;
// 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;
}
//////
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -