📄 action.cpp
字号:
distance = Distance(p->robot[robot].pos , pos);
posangle = Atan(dy,dx);
turnangle = p->robot[robot].rotation - posangle; //转动角度
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; //速度的方向和robot正面的夹角
RegulateAngle(vangle); //主要用于最后控制减速度的大小
if( vangle <-90 || vangle > 90 ) //同时判断v的正负
v=-v;
if(face)
{//forward 跑位，如果后退的话 就v=0
//设vl,vr=0 还是vl,vr=125 有一个条件有一个临界条件那就是
//v = SPEED_ZERO
if(v < -SPEED_ZERO)
{
Velocity(env,robot,0,0);
return ;
}
}
else if(v > SPEED_ZERO)
{//back 跑位，如果后退的话 就v=0
Velocity(env,robot,0,0);
return ;
}
v1=v; //v1 is changing while program running
//whlie, v is not
if(distance > Limiteddis )
{//it is too early to count the steps
//but the Limiteddis should be tested!! to do...
if(turnangle > Limitedangle || turnangle < -Limitedangle)
{//adjust angle
/////////////////测试这一段
//对于goalie这一段应该特别注意
//发生变向 1.knock the robot,especially the opponent
// 2.knock the wall
// so the anglespeedmax is allowed ++ more!!
if(turnangle > 50 || turnangle < -50)
anglespeedmax = 0;
else if(turnangle > 30 || turnangle < -30)
anglespeedmax = 80;
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!!!!!!
//radious of robot is about 1.5 ,so the distance is very short
if(turnangle > Limitedangle || turnangle < -Limitedangle)
{
Angle(env,robot,posangle);
return ;
}
}
if(distance < 0.4)
{ //停止并转向 return!!!!!!
//radious of robot is about 1.5 ,so the distance is very short
if( v<0.1 && v>-0.1)
{ //the range of v shoud be tested
Angle(env,robot,bestangle);
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);
}
}
}
}
}
}
//
/*PositionAndStopX 让robot 跑到pos，并且停下来原地旋转，
Xangle 旋转的角速度
limit 控制停在pos附近的距离
*/
//
void PositionAndStopX(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 )
limit =2;
double Limiteddis=limit;
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 ;
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -