📄 decisionmakingx.cpp
字号:
{
dbLRWheelVelocity* pSpeed = &rbV[nRobot];
dbPOINT shoot_target;
shoot_target.x = posTarget.GetX();
shoot_target.y = posTarget.GetY();
dbPOINT ballPt;
ballPt.x = posBall.GetX();
ballPt.y = posBall.GetY();
/************************************************************************/
/* 末端圆弧 */
/* 可调参数: r, samespeed */
/************************************************************************/
///////// ///////////////////////////////////////////////////////////////////////
double dist,distE,anglerb2ball,anglerb2target;
double angle1,angle2,angle3,angle4,angleball2target,angle5;
dbPOINT rbPt,EGoal;
EGoal.x = PITCH_LENGTH + 3;
EGoal.y = PITCH_WIDTH/2;
dist = distRobot2Pt(Robot[nRobot],ballPt);//求出球和小车的距离
rbPt.x = Robot[nRobot].x;
rbPt.y = Robot[nRobot].y;
anglerb2ball = Getpt2ptAngle(rbPt,ballPt);//小车指向球的方向
anglerb2target = Getpt2ptAngle(rbPt,EGoal);//小车指向目标的方向
angleball2target = Getpt2ptAngle(ballPt,shoot_target);//球到目标方向
angle1 = VecPosition::NormalizeAngle2PI(anglerb2ball - anglerb2target);
angle1 = VecPosition::NormalizeAngle(angle1);
angle2 = VecPosition::NormalizeAngle2PI(Robot[nRobot].theta);
angle3 = VecPosition::NormalizeAngle2PI(angle2 - anglerb2ball);
// r大，圆半径变小
double r = 18;//2.65;
double radiu;
LINEFORMULATION line1,line2,line3;
dbPOINT tempPt1,tempPt2,tempPt3;
tempPt1.x = Robot[nRobot].x;
tempPt1.y = Robot[nRobot].y;
StdLineForm(tempPt1,Robot[nRobot].theta, &line1);//点斜式得出车的直线方程L1
cn_PointPerpendLine(tempPt1, &line1, &line2, &tempPt2);//过车作L1的垂线方程L2
StdLineForm(tempPt1, ballPt, &line1);//车球连线方程L3
tempPt2.x = (Robot[nRobot].x + ballPt.x)/2;
tempPt2.y = (Robot[nRobot].y + ballPt.y)/2;//车球中点c
cn_PointPerpendLine(tempPt2, &line1, &line3, &tempPt3);//过c作L3的垂线方程
cn_2LinesCrossPoint(&line2, &line3, &tempPt3);//L2和L3的交点即圆心
double angle8;
dbPOINT roundpt;
roundpt = tempPt3;
angle8 = VecPosition::NormalizeAngle2PI(angle2 - Getpt2ptAngle(rbPt,ballPt));
radiu = cn_2PointsDist(tempPt3,ballPt);
StdLineForm(tempPt3, ballPt, &line1);//圆心球连线方程L4
cn_PointPerpendLine(ballPt, &line1, &line2, &tempPt2);//过球作L4的垂线方程L5,L5即为触球时的切线
angle4 = angle2 - anglerb2ball;
if(rbPt.y<-(line2.a*rbPt.x+line2.c)/line2.b)
{
if(angle8<=PI)
{
angle5 = VecPosition::NormalizeAngle2PI(anglerb2ball - angle4);
}
else
{
angle5 = VecPosition::NormalizeAngle2PI(anglerb2ball + PI - angle4);
}
}
else
{
if(angle8<=PI)
{
angle5 = VecPosition::NormalizeAngle2PI(anglerb2ball + PI - angle4);
}
else
{
angle5 = VecPosition::NormalizeAngle2PI(anglerb2ball - angle4);
}
}
if(angle5>PI)
angle5 -=2*PI;
dbPOINT pt1,pt2;
pt1.x = PITCH_LENGTH;
pt1.y = PITCH_WIDTH/2+GOAL_WIDTH/2;
pt2.x = PITCH_LENGTH;
pt2.y = PITCH_WIDTH/2-GOAL_WIDTH/2;
double angle6,angle7;
angle6 = Getpt2ptAngle(ballPt,pt1);
if(angle6>PI)
angle6 -=2*PI;
angle7 = Getpt2ptAngle(ballPt,pt2);
if(angle7>PI)
angle7 -=2*PI;
//角度差在一定范围时以圆轨迹前进
if(angle5>angle7
&& angle5<angle6
&& angle2!=anglerb2ball
&& line2.b!=0
&& rbPt.x<ballPt.x
&& ballPt.x<EGoal.x
&& IfEndprocess>2)
{
double samespeed = radiu/150*40+80;
if(samespeed > MAXSPEED)
samespeed = MAXSPEED;
// SendMsg(2, "E3 : %2.2f, %2.2f", radiu, samespeed);
double angleround;
angleround = VecPosition::NormalizeAngle2PI(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;
if(pSpeed->LeftValue>MAXSPEED)
{
pSpeed->RightValue = MAXSPEED - pSpeed->LeftValue + pSpeed->RightValue;
pSpeed->LeftValue = MAXSPEED;
}
}
else
{
pSpeed->RightValue = -(radiu + r)/radiu*samespeed;
pSpeed->LeftValue = -(radiu - r)/radiu*samespeed;
if(pSpeed->RightValue<-MAXSPEED)
{
pSpeed->LeftValue = -MAXSPEED + pSpeed->RightValue - pSpeed->LeftValue;
pSpeed->RightValue = -MAXSPEED;
}
}
}
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;
if(pSpeed->LeftValue<-MAXSPEED)
{
pSpeed->RightValue = -MAXSPEED + pSpeed->LeftValue - pSpeed->RightValue;
pSpeed->LeftValue = -MAXSPEED;
}
}
else
{
pSpeed->RightValue = (radiu + r)/radiu*samespeed;
pSpeed->LeftValue = (radiu - r)/radiu*samespeed;
if(pSpeed->RightValue>MAXSPEED)
{
pSpeed->LeftValue = MAXSPEED - pSpeed->RightValue + pSpeed->LeftValue;
pSpeed->RightValue = MAXSPEED;
}
}
}
}
/************************************************************************/
/* 末端余弦 */
/* 可调参数: maxd, maxe, kk, basespeed, kk */
/************************************************************************/
// samespeed
double basespeed = MAXSPEED;//100+(220-Robot[nRobot].x)/200*20;
basespeed = Maths::Limit(basespeed, 100, MAXSPEED);
double kk = 0.7; // 余弦参数
double maxd = 16; // 球和车距离范围
double maxe = 3.7; // 轨迹范围
distE = fabs(dist*sin(angle3));
//球和车距离很近，球在车的轨迹范围内,射门角度不是很大
if(dist<=maxd
&& distE<=maxe
&& IfEndprocess>1)
{
// SendMsg(2, " cos");
double anglelimit,anglet1,anglet2,anglet3;
anglelimit = (150-Robot[nRobot].x-5)/(150 - 5)*PI*.2 + PI*.2;
if(anglelimit>PI*.4)
anglelimit = PI*.4;
dbPOINT pt1,pt2;
pt1.x = pt2.x = PITCH_LENGTH;
pt1.y = PITCH_WIDTH/2+GOAL_WIDTH/2;
pt2.y = PITCH_WIDTH/2-GOAL_WIDTH/2;
//正面向球，
if(angle3<PI*0.2&&angle3>=0||angle3>PI*1.8&&angle3<2*PI)
{
double anglek;
anglek = VecPosition::NormalizeAngle2PI(Robot[nRobot].theta - anglerb2target);
if(anglek>PI)
anglek -=2*PI;
//xue
anglet1 = VecPosition::NormalizeAngle2PI(Robot[nRobot].theta - Getpt2ptAngle(rbPt,pt1));
if(anglet1>PI)
anglet1 -=2*PI;
anglet2 = VecPosition::NormalizeAngle2PI(Robot[nRobot].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
{
//带球标志
sym[2] = currentOrder[1];
sym[1] = 1;
sym[0] = 1;
if(anglek>0)
{
if(fabs(anglek)<PI/2)
pSpeed->LeftValue = (MAXSPEED-basespeed)*(PI/2-fabs(anglek))/PI*2+basespeed;
else
pSpeed->LeftValue = basespeed;
pSpeed->RightValue = pSpeed->LeftValue*fabs(cos(anglek))*kk;
}
else
{
if(fabs(anglek)<PI/2)
pSpeed->RightValue = (MAXSPEED-basespeed)*(PI/2-fabs(anglek))/PI*2+basespeed;
else
pSpeed->RightValue = basespeed;
pSpeed->LeftValue = pSpeed->RightValue*fabs(cos(anglek))*kk;
}
}
}
//后面向球
else if(angle3>PI - PI*0.2&&angle3<PI+PI*0.2)
{
double anglek;
anglek = VecPosition::NormalizeAngle2PI(Robot[nRobot].theta+PI - anglerb2target);
if(anglek>PI)
anglek -=2*PI;
//xue
anglet1 = VecPosition::NormalizeAngle2PI(Robot[nRobot].theta - Getpt2ptAngle(rbPt,pt1)+PI);
if(anglet1>PI)
anglet1 -=2*PI;
anglet2 = VecPosition::NormalizeAngle2PI(Robot[nRobot].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))
{
//带球标志
sym[2] = currentOrder[1];
sym[1] = 0;
sym[0] = 1;
if(anglek>0)
{
if(fabs(anglek)<PI/2)
pSpeed->RightValue = (MAXSPEED-basespeed)*(PI/2-fabs(anglek))/PI*2+basespeed;
else
pSpeed->RightValue = basespeed;
pSpeed->RightValue = -pSpeed->RightValue;
pSpeed->LeftValue = pSpeed->RightValue*fabs(cos(anglek))*kk;
}
else
{
if(fabs(anglek)<PI/2)
pSpeed->LeftValue = (MAXSPEED-basespeed)*(PI/2-fabs(anglek))/PI*2+basespeed;
else
pSpeed->LeftValue = basespeed;
pSpeed->LeftValue = -pSpeed->LeftValue;
pSpeed->RightValue = pSpeed->LeftValue*fabs(cos(anglek))*kk;
}
}
}
}
/************************************************************************/
/* 末端直线 */
/* 可调参数: maxe, angleLimit1,2 */
/************************************************************************/
//角度特别好的处理
if (IfEndprocess > 0)
{
maxe = 4;
double angleLimit1 = 0.2*PI;
double angleLimit2 = 0.45*PI;
//正面向球
if((angle3<angleLimit1&&angle3>=0||angle3>2*PI-angleLimit1&&angle3<2*PI)
&&fabs(angle1)<PI*0.45
&&distE<=maxe&&
(angle2>=0 &&angle2<=angleLimit2||angle2>=2*PI-angleLimit2))
{
double temp;
temp = Robot[nRobot].y+(PITCH_LENGTH-Robot[nRobot].x)*tan(angle2);
if(temp>= PITCH_WIDTH/2 - GOAL_WIDTH/2 +1
&&temp <= PITCH_WIDTH/2 + GOAL_WIDTH/2 -1)
{
// SendMsg(2, "line");
pSpeed->LeftValue = pSpeed->RightValue = MAXSPEED;
//带球标志
sym[2] = currentOrder[1];
sym[1] = 1;
sym[0] = 1;
}
}
//后面向球
else if((angle3>PI - angleLimit1&&angle3<PI+angleLimit1)
&&fabs(angle1)<PI*0.45
&&distE<=maxe
&&angle2>=PI-angleLimit2&&angle2<=2*PI-angleLimit2)
{
double temp;
temp = Robot[nRobot].y+(PITCH_LENGTH-Robot[nRobot].x)*tan(angle2);
if(temp>=PITCH_WIDTH/2 - GOAL_WIDTH/2+1
&&temp<=PITCH_WIDTH/2 + GOAL_WIDTH/2-1)
{
// SendMsg(2, "line");
pSpeed->LeftValue = pSpeed->RightValue = -MAXSPEED;
//带球标志
sym[2] = currentOrder[1];
sym[1] = 0;
sym[0] = 1;
}
}
}
}
////////////////// Basic Actions End /////////////////////////////////////////////////////
////////////////// Complex Actions ///////////////////////////////////////////////////////
/************************************************************************/
/* 射门函数 */
/************************************************************************/
void CDecisionMakingx::Vect_MidShoot(int nRobot)
{
VecPosition posGoal(PITCH_LENGTH, PITCH_WIDTH/2);
VecPosition posBall;
GetToPositionNewPerformance(11, nRobot, posGoal, posBall);
ToPositionNew(nRobot, posGoal, posBall, MAXSPEED, 0);
return;
if(Robot[nRobot].x > m_posBall.GetX())
{
VecPosition pos;
double det = 30;
pos.SetX(m_posBall.GetX() +100);
if(m_posBall.GetY() > Robot[nRobot].y)
pos.SetY(m_posBall.GetY() + 5);
else
pos.SetY(m_posBall.GetY() - 5);
if(m_posBall.GetY() < det)
pos.SetY(m_posBall.GetY() - 3);
else if(m_posBall.GetY() > PITCH_WIDTH-det)
pos.SetY(m_posBall.GetY() + 3);
ToPositionNew(nRobot, pos, m_posBall, 110, 2);
}
}
/************************************************************************/
/* 中分线射门 */
/************************************************************************/
void CDecisionMakingx::Vect_MidShoot1(int nRobot)
{
VecPosition posRobot(Robot[nRobot].x, Robot[nRobot].y);
VecPosition posGoal(PITCH_LENGTH, PITCH_WIDTH/2);
VecPosition posBall = m_posBall;
Line lineBall2Goal = Line::MakeLineFromTwoPoints(posGoal, posBall);
double L = 0, R = 30;
VecPosition posShoot = lineBall2Goal.GetPointInLine(posBall, -L);
Line linePerp = lineBall2Goal.GetPerpendicularLine(posShoot);
VecPosition posCenter = linePerp.GetPointInLine(posShoot, -R);
Circle c(posCenter, R);
Line lineRobot2Shoot = Line::MakeLineFromTwoPoints(posRobot, posShoot);
VecPosition posMid = (posRobot + posShoot)/2;
Line linePerp2 = lineRobot2Shoot.GetPerpendicularLine(posMid);
VecPosition posSolu[2], posTarget;
linePerp2.GetCircleIntersectionPoints(c, &posSolu[0], &posSolu[1]);
if ( posSolu[0].GetDistanceTo(posRobot) > posSolu[1].GetDistanceTo(posRobot))
posTarget = posSolu[1];
else
posTarget = posSolu[0];
ToPositionPD(nRobot, posTarget, 100);
SendMsg(3, "%2.2f, %2.2f", posTarget.GetX(), posTarget.GetY());
}
// 圆弧射门
void CDecisionMakingx::Vect_MidShoot2(int nRobot)
{
VecPosition posRobot(Robot[nRobot].x, Robot[nRobot].y);
VecPosition posGoal(PITCH_LENGTH, PITCH_WIDTH/2);
VecPosition posBall = m_posBall;
double L = 5;
Line lineBall2Goal = Line::MakeLineFromTwoPoints(posBall, posGoal);
posBall = lineBall2Goal.GetPointInLine(posBall, L*
( (posBall.GetX() < posGoal.GetX()) ? -1 : 1 ));
Line linePerBall2Goal = lineBall2Goal.GetPerpendicularLine(posBall);
Line lineRobot2Ball = Line::MakeLineFromTwoPoints(posRobot, posBall);
Line lineMidPerRobot2Ball = lineRobot2Ball.GetPerpendicularLine((posRobot+posBall)/2);
VecPosition posCenter = linePerBall2Goal.GetIntersection(lineMidPerRobot2Ball);
double dRadius = (posCenter-posBall).GetMagnitude();
Circle circle(posCenter, dRadius);
// double d = (posCenter - posRobot).GetMagnitude();
rbV[nRobot].LeftValue = (dRadius-4.0);//-(dRadius + 4.1)/dRadius * 100;
rbV[nRobot].RightValue = (dRadius+4.0);//-(dRadius - 4.1)/dRadius * 100;
// EndProcess(1, nRobot, posGoal, m_posBall);
// rbV[nRobot].LeftValue = -12;//-(dRadius + 4.1)/dRadius * 100;
// rbV[nRobot].RightValue = 0;//-(dRadius - 4.1)/dRadius * 100;
}
int CDecisionMakingx::Vect_MidShootli(dbROBOTPOSTURE pRobotInford, BallInformation &ball,dbLRWheelVelocity *pSpeed)
{
// pSpeed->LeftValue = pSpeed->RightValue = 0;
// return 1;
dbPOINT goal,ballPt;
double delta,angletemp;
delta = 10;
ballPt = ball;
goal.x = PITCH_LENGTH + 5;
goal.y = PITCH_WIDTH/2;
angletemp = Getpt2ptAngle(ballPt,goal);
//ballPt.x = ballPt.x - delta*sin(angletemp);
//ballPt.y = ballPt.y - delta*cos(angletemp);
double deltax;
deltax = 10;
if((ball.x>PITCH_LENGTH - deltax)&&(ball.y>PITCH_LENGTH/2 - GOAL_AREA_WIDTH/2-deltax&&ball.y<PITCH_LENGTH/2 + GOAL_AREA_WIDTH/2 + deltax))
{
if(ball.y<=PITCH_LENGTH/2)
goal.y = ball.y + deltax;
else
goal.y = ball.y - deltax;
goal.x = PITCH_LENGTH + 8;
ToPositionNewli(&pRobotInford,ballPt,goal,120,2,pSpeed);
return 1;
}
ToPositionNewli(&pRobotInford,ballPt,goal,120,2,pSpeed);
/*if(pRobotInford.x>ball.x)
Vect_MidShoot2(pRobotInford,ball,goal,0,pSpeed);*/
return 1;
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -