📄 decisionmakingx.cpp
字号:
{
if(ball.y > CENTER_Y + 10)
IsClockWise = 1;
else if(ball.y < CENTER_Y - 10)
IsClockWise = -1;
else if(pRobot->y < ball.y)
IsClockWise = 1;
else
IsClockWise = -1;
}
moveTheta = theta_rb + k*IsClockWise/dist*(pi/2);
if(dist > 50)
s_MoveOnAngle(pRobot,moveTheta,m_MoveParameter.V_max,pSpeed);
else
s_MoveOnAngle(pRobot,moveTheta,m_MoveParameter.V_max,pSpeed);
if(k==0)
s_Move2Pt(pRobot,ball,m_MoveParameter.V_max,pSpeed);*/
dbPOINT goal;
goal.x = wallright;
goal.y = walltop/2;
ToPositionNew(pRobot,ball,goal,70,2,pSpeed);
if(pRobot->x>ball.x)
{
dbPOINT pt;
double det;
det = 30;
pt.x =ball.x +100;
pt.y = 2*ball.y-pRobot->y;
if(pt.y>ball.y)
pt.y = ball.y + 5;
else
pt.y = ball.y - 5;
if(ball.y<det)
pt.y = ball.y - 3;
else if(ball.y>walltop-det)
pt.y = ball.y + 3;
ToPositionNew(pRobot,ball,pt,70,0,pSpeed);
}
return 1;
}
///////////////////////////////////////////////////////
//函数 DribleBall()
//函数名 带球至对方球门
///////////////////////////////////////////////////////
int CDecisionMakingx::DribleBall(dbROBOTPOSTURE *pRobot,ballInformation ball ,dbLRWheelVelocity *pSpeed)
{
return 1;
}
///////////////////////////////////////////////////////
//函数 Defence()
//函数名 防守
///////////////////////////////////////////////////////
int CDecisionMakingx::Defence(dbROBOTPOSTURE *pRobot,ballInformation ball ,dbLRWheelVelocity *pSpeed)
{
double theta_rb;
double dx,dy;
double dist;
int IsClockWise = 0;
dbPOINT target;
double moveTheta,theta_e;
double k = 1.0;
///////////////////////////////////////////
dx = ball.x - pRobot->x;
dy = ball.y - pRobot->y;
dist = sqrt(dx*dx + dy*dy);
target.x = ball.x;
target.y = ball.y;
theta_rb = atan2(dy,dx);
theta_rb = cn_AngleTrim2PI(theta_rb);
pRobot->theta = cn_AngleTrim2PI(pRobot->theta);
///////////////////////////////////////////
if(ball.x > pRobot->x + 5)
{
if((theta_rb < 65.0/180*pi && theta_rb >= 0 ) || (theta_rb < 2*pi && theta_rb > 300.0/180*pi)) // 55-- 305
{
theta_e = theta_rb - pRobot->theta;
theta_e = cn_AngleTrim2PI(theta_e);
if(theta_e > pi) theta_e -= pi;
if(theta_e > pi/2) theta_e = pi - theta_e;
if(theta_e < pi/6) // Deb %%% modify 30
{
Move2Pt(pRobot,target,m_MoveParameter.V_max,pSpeed);
return 0;
}
}
double tempC = 0.5*dist/cos(theta_rb);
if( tempC < 0 ) tempC =0; // should +
target.x = ball.x - tempC;
if(target.y < wallbottom + BOUND1)
target.y= wallbottom + BOUND1;
else if(target.y > walltop - BOUND1)
target.y= walltop - BOUND1;
else if(target.y < GOAL_UP_LINE + 2 && target.y > GOAL_DN_LINE - 2)
{
if(target.x < goal_x_widthM + 4)
target.x = goal_x_widthM + 4;//2
if(pRobot->x < goal_x_widthM + 5)
{
theta_e = pRobot->theta;
if(theta_e > pi) theta_e -= pi;
theta_e -= pi/2;
if(fabs(theta_e) > pi/6)
{
TurnToAnglePD(pRobot,pi/2,NOCLOCK,pSpeed);
return 0;
}
else
target.x = ball.x; // ????
}
}
else
{
if(target.x < wallleft + BOUND1)
target.x = wallleft + BOUND1;
}
ToPositionPD(pRobot,target,m_MoveParameter.V_max,10.0,pSpeed); // Vbase pt
return 0;
}
else
{
if(pRobot->x < goal_x_widthM + 4)
if( pRobot->y < GOAL_UP_LINE + 2 && pRobot->y > GOAL_DN_LINE - 2)
{
theta_e = pRobot->theta;
if(theta_e > pi) theta_e -= pi;
theta_e -= pi/2;
if(fabs(theta_e) > pi/6)
TurnToAnglePD(pRobot,pi/2,NOCLOCK,pSpeed);
else
{
target.x = goal_x_widthM;//+2
if(ball.y > GOAL_UP_LINE + 2)
target.y = GOAL_UP_LINE + 8;
else if(ball.y < GOAL_DN_LINE - 2)
target.y = GOAL_DN_LINE - 8;
else
target.y = ball.y;
// ToPositionPD(pRobot,target,m_MoveParameter.V_max,10.0,pSpeed); // Vbase pt
Move2Pt(pRobot,target,pSpeed);
}
return 0;
}
else if( ball.x < goal_x_widthM)
{
target.y = 0;
if(ball.y > GOAL_UP_LINE + 6)
target.y = GOAL_UP_LINE + 8;
else if(ball.y < GOAL_DN_LINE - 6)
target.y = GOAL_DN_LINE - 8;
//else // ????
// target.y= ball.y;
if(target.y != 0)
{
ToPositionPD(pRobot,target,m_MoveParameter.V_max,10.0,pSpeed); // Vbase pt
return 0;
}
}
if(dist > 40) k = 0;
else k = 15;//12
// according to the position of ball compared with robot;
if(ball.y > UP_LINE)
IsClockWise = 1;
else if(ball.y < DN_LINE)
IsClockWise = -1;
else
{
if(pRobot->y < ball.y )
IsClockWise = 1;
else
IsClockWise = -1;
}
moveTheta = theta_rb + k*IsClockWise/dist*(pi/2);
if(dist > 50)
MoveOnAngle(pRobot,moveTheta,m_MoveParameter.V_max,pSpeed);
else
MoveOnAngle(pRobot,moveTheta,m_MoveParameter.V_max/2,pSpeed);
}
return 0;
}
//末端处理(不能应用到球在车后的情况)
int CDecisionMakingx::EndProcess(int i,dbROBOTPOSTURE *pRobot,dbPOINT shoot_target,dbPOINT ballPt,dbLRWheelVelocity *pSpeed)
{
double r,dist,distE,anglerb2ball,anglerb2target,angle1,angle2,angle3,angle4,angleball2target,angle5,maxe,maxd,maxspeed;
dbPOINT rbPt,EGoal;
maxspeed = 60;//80
EGoal.x = wallright + 4;
EGoal.y = walltop/2;
maxd = 15;
maxe = 2.8;
r = 2.8;//2.8;
dist = distRobot2Pt(*pRobot,ballPt);//求出球和小车的距离
rbPt.x = pRobot->x;
rbPt.y = pRobot->y;
anglerb2ball = Getpt2ptAngle(rbPt,ballPt);//小车指向球的方向
anglerb2target = Getpt2ptAngle(rbPt,EGoal);//小车指向目标的方向
angleball2target = Getpt2ptAngle(ballPt,shoot_target);//球到目标方向
angle1 = cn_AngleTrim2PI(anglerb2ball - anglerb2target);
if(angle1>pi&&angle1<=2*pi)
angle1 =angle1 - 2*pi;
angle2 = cn_AngleTrim2PI(pRobot->theta);
angle3 = cn_AngleTrim2PI(angle2 - anglerb2ball);
//末端圆弧
double radiu;
double samespeed = 40.0;
// double theta_E,theta1,theta2;
LINEFORMULATION line1,line2,line3;
dbPOINT tempPt1,tempPt2,tempPt3;
tempPt1.x = pRobot->x;tempPt1.y = pRobot->y;
StdLineForm(tempPt1,pRobot->theta, &line1);//点斜式得出车的直线方程L1
cn_PointPerpendLine(tempPt1, &line1, &line2, &tempPt2);//过车作L1的垂线方程L2
StdLineForm(tempPt1, ballPt, &line1);//车球连线方程L3
tempPt2.x = (pRobot->x + ballPt.x)/2;
tempPt2.y = (pRobot->y + ballPt.y)/2;//车球中点c
cn_PointPerpendLine(tempPt2, &line1, &line3, &tempPt3);//过c作L3的垂线方程
cn_2LinesCrossPoint(&line2, &line3, &tempPt3);//L2和L3的交点即圆心
//xue
double angle8;
dbPOINT roundpt;
roundpt = tempPt3;
angle8 = cn_AngleTrim2PI(angle2 - Getpt2ptAngle(rbPt,ballPt));
// BOOL sign1;
radiu = cn_2PointsDist(tempPt3,ballPt);
StdLineForm(tempPt3, ball, &line1);//圆心球连线方程L4
cn_PointPerpendLine(ball, &line1, &line2, &tempPt2);//过球作L4的垂线方程L5,L5即为触球时的切线
///xue
angle4 = angle2 - anglerb2ball;
if(rbPt.y<-(line2.a*rbPt.x+line2.c)/line2.b)
{
if(angle8<=pi)
{
angle5 = cn_AngleTrim2PI(anglerb2ball - angle4);
}
else
{
angle5 = cn_AngleTrim2PI(anglerb2ball + pi - angle4);
}
}
else
{
if(angle8<=pi)
{
angle5 = cn_AngleTrim2PI(anglerb2ball + pi - angle4);
}
else
{
angle5 = cn_AngleTrim2PI(anglerb2ball - angle4);
}
}
if(angle5>pi)
angle5 -=2*pi;
dbPOINT pt1,pt2;
pt1.x =wallright;
pt1.y = CENTER_Y+20;
pt2.x = wallright;
pt2.y = CENTER_Y-20;
double angle6,angle7;
angle6 = Getpt2ptAngle(ballPt,pt1);
if(angle6>pi)
angle6 -=2*pi;
angle7 = Getpt2ptAngle(ballPt,pt2);
if(angle7>pi)
angle7 -=2*pi;
//xue
double y = -(line2.a*150 + line2.c)/line2.b;
if(angle5>angle7&&angle5<angle6&&angle2!=anglerb2ball&&line2.b!=0&&rbPt.x<ballPt.x&&ball.x<EGoal.x&&i>1)//角度差在一定范围时以圆轨迹前进
{
//xue4.25
samespeed = radiu/150*35+60;
if(samespeed > 110)
samespeed = 110;
//xue 4.26
double angleround;
angleround = cn_AngleTrim2PI(Getpt2ptAngle(roundpt,rbPt)-Getpt2ptAngle(roundpt,ballPt));
if(rbPt.y<-(line2.a*rbPt.x+line2.c)/line2.b&&angleround<pi*1.2)
{
if(angle8<=pi)
{
pSpeed->LeftValue = (radiu + r)/radiu*samespeed;
pSpeed->RightValue = (radiu - r)/radiu*samespeed;
}
else
{
pSpeed->RightValue = -(radiu + r)/radiu*samespeed;
pSpeed->LeftValue = -(radiu - r)/radiu*samespeed;
}
}
else if(rbPt.y>=-(line2.a*rbPt.x+line2.c)/line2.b&&(2*pi - angleround)<pi*1.2)//xue 4.26
{
if(angle8<=pi)
{
pSpeed->LeftValue = -(radiu + r)/radiu*samespeed;
pSpeed->RightValue = -(radiu - r)/radiu*samespeed;
}
else
{
pSpeed->RightValue = (radiu + r)/radiu*samespeed;
pSpeed->LeftValue = (radiu - r)/radiu*samespeed;
}
}
}
distE = fabs(dist*sin(angle3));
//球和车距离很近，车在对方门区附近，车在球后，车和球的连线与右边界的交点在对方球门附近
/* if(dist<=maxd&&pRobot->x>CENTER_X+wallright/4&&pRobot->x<ballPt.x&&fabs(angle1)<pi*0.45)
TurnTo(pRobot,ball,EGoal,124,pSpeed);*/
//球和车距离很近，球在车的轨迹范围内,射门角度不是很大
if(dist<=maxd&&distE<=maxe&&i>0)
{
//xue
double anglelimit,anglet1,anglet2,anglet3,kk;
kk = 0.85;/////////////////////////////0.9
anglelimit = (150-pRobot->x -5)/(150 - 5)*pi*.2 + pi*.0.2;/////0.2
if(anglelimit>pi*.4)
anglelimit = pi*.4;
dbPOINT pt1,pt2;
pt1.x = pt2.x = wallright;
pt1.y = CENTER_Y+20;
pt2.y = CENTER_Y-20;
//
//正面向球，
if(angle3<pi*0.2&&angle3>=0||angle3>pi*1.8&&angle3<2*pi)
{
double anglek;
anglek = cn_AngleTrim2PI(pRobot->theta - anglerb2target);
if(anglek>pi)
anglek -=2*pi;
//xue
anglet1 = cn_AngleTrim2PI(pRobot->theta - Getpt2ptAngle(rbPt,pt1));
if(anglet1>pi)
anglet1 -=2*pi;
anglet2 = cn_AngleTrim2PI(pRobot->theta - Getpt2ptAngle(rbPt,pt2));
if(anglet2>pi)
anglet2 -=2*pi;
if(fabs(anglet1)>fabs(anglet2))
anglet3 = fabs(anglet2);
else
anglet3 = fabs(anglet1);
//
if(anglet3<anglelimit||(anglet1*anglet2<0))//xue
{
if(anglek>0)
{
if(anglek<pi/2)
pSpeed->LeftValue = (maxspeed-60)*(pi/2-fabs(anglek))/pi*2+60;
else
pSpeed->LeftValue = 70;
pSpeed->RightValue = pSpeed->LeftValue*fabs(cos(anglek))*kk;
}
else
{
if(anglek>-pi/2)//(anglek<pi/2)
pSpeed->RightValue = (maxspeed-60)*(pi/2-fabs(anglek))/pi*2+60;
else
pSpeed->RightValue = 70;
//pSpeed->RightValue = maxspeed;
pSpeed->LeftValue = pSpeed->RightValue*fabs(cos(anglek))*kk;
}
}
}
//后面向球
else if(angle3>pi - pi*0.2&&angle3<pi+pi*0.2&&i>0)
{
double anglek;
anglek = cn_AngleTrim2PI(pRobot->theta+pi - anglerb2target);
if(anglek>pi)
anglek -=2*pi;
//xue
anglet1 = cn_AngleTrim2PI(pRobot->theta - Getpt2ptAngle(rbPt,pt1)+pi);
if(anglet1>pi)
anglet1 -=2*pi;
anglet2 = cn_AngleTrim2PI(pRobot->theta - Getpt2ptAngle(rbPt,pt2)+pi);
if(anglet2>pi)
anglet2 -=2*pi;
if(fabs(anglet1)>fabs(anglet2))
anglet3 = fabs(anglet2);
else
anglet3 = fabs(anglet1);
//
if(anglet3<anglelimit||(anglet1*anglet2<0))
{
if(anglek>0)
{
// pSpeed->RightValue = -maxspeed;
if(anglek<pi/2)
pSpeed->RightValue = (maxspeed-60)*(pi/2-fabs(anglek))/pi*2+60;
else
pSpeed->RightValue = 70;
pSpeed->RightValue = -pSpeed->RightValue;
pSpeed->LeftValue = pSpeed->RightValue*fabs(cos(anglek))*kk;
}
else
{
if(anglek>-pi/2)//(anglek<pi/2)
pSpeed->LeftValue = (maxspeed-60)*(pi/2-fabs(anglek))/pi*2+60;
else
pSpeed->LeftValue = 70;
pSpeed->LeftValue = -pSpeed->LeftValue;
// pSpeed->LeftValue = -maxspeed;
pSpeed->RightValue = pSpeed->LeftValue*fabs(cos(anglek))*kk;
}
}
}
}
//角度特别好的处理
//正面向球
//maxe应等于(7.5/2-4/2)/sin(angle3)
if((angle3<pi*0.2&&angle3>=0||angle3>pi*1.8&&angle3<2*pi)&&fabs(angle1)<pi*0.45&&distE<=maxe&&(angle2>=0&&angle2<=pi*0.45||angle2>=pi*1.55&&angle2<=2*pi))
//if((angle3<pi*0.4&&angle3>=0||angle3>pi*1.6&&angle3<2*pi)&&fabs(angle1)<pi*0.45&&distE<=maxe&&(angle2>=0&&angle2<=pi*0.45||angle2>=pi*1.55&&angle2<=2*pi))
{
double temp;
temp = pRobot->y+(wallright-pRobot->x)*tan(angle2);
if(temp>=GATE_DN_LINE+1&&temp<=GATE_UP_LINE-⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -