📄 decisionmakingx.cpp
字号:
goToken = 1;
}
else
{
if(target.y < GATE_DN_LINE)
target.y = GATE_DN_LINE - gtError;
else if(target.y > GATE_UP_LINE)
target.y = GATE_UP_LINE + gtError;
}
}
///////////////////////////////////
if(target.y > GATE_UP_LINE + 3)target.y = GATE_UP_LINE + 3;
else if(target.y < GATE_DN_LINE - 3)target.y = GATE_DN_LINE - 3;
double e1;
static double e2 = 0;
static double turn_E = 0;
double ex;
double k = 0;
double Kd = 1.0; //.08;
double v;
int sign;
e1 = target.y - pRobotInford->y;
if(e1 < 0) sign = -1; //目标点在守门员下方
else sign = 1;
ex = target.x - pRobotInford->x;
e1 = sqrt(e1*e1 + ex*ex); //test
if( e1 > 10 ) k = 2; // GGGK prara// 3 6 9
else if(e1 > 5) k = 4;
else k = 6;
if(fabs(target.y - pRobotInford->y) > 10.0)
goToken = 1; // GGGK
e1 = e1*sign;
v = k*e1 + Kd*(e2 - e1);
e2 = e1;
if (v > gVMAX) v = gVMAX;
if (v < -gVMAX) v = -gVMAX;
if((ex > -xError && ex < xError) && (goToken == 0))
{
if(fabs(cos(pRobotInford->theta)) < cos(4.0/9.0*Pi)) //10degree
{
if(pRobotInford->theta < pi)
{
pSpeed->LeftValue = v;
pSpeed->RightValue = v;
}
else
{
pSpeed->LeftValue = -v;
pSpeed->RightValue = -v;
}
}
else
TurnToAnglePD(pRobotInford,pi/2,NOCLOCK, pSpeed);
}
else
{
if(goToken==2)
{
v = fabs(v);
Move2Pt(pRobotInford, target,m_MoveParameter.V_max,pSpeed);
}
else if(fabs(target.y - pRobotInford->y) < 2) // beyond ex or goToken=1;
TurnToAnglePD(pRobotInford,pi/2,NOCLOCK, pSpeed);// GGGK
else
{
v = fabs(v);
double dx = pRobotInford->x - ball.x;
double dy = pRobotInford->y - target.y;
if(sqrt(dx*dx +dy*dy) > 20.0)
Move2Pt(pRobotInford, target,v,pSpeed);
else
ToPositionPDGoal(pRobotInford,target,m_MoveParameter.V_max,0.0,pSpeed);
}
}
}
return 1;
}
/************************************************************************************
函数名 ：AvoidPt2Pt(robotInformation &robot, dbPOINT &obsPt, dbPOINT &target)
创建日期：19/4/2001
作者 ：
功能 ：避障到定点
参数 ： robot 机器人位姿
obsPt 障碍点
targer 目标点
**************************************************************************/
int CDecisionMakingx::AvoidPt2Pt(dbROBOTPOSTURE* pRobotInford, dbPOINT &obsPt, dbPOINT &target,dbLRWheelVelocity *pSpeed)
{
double dx,dy;
double theta_ro;
double theta_rt;
double moveTheta;
int IsClockWise;
double speed;
double dist;
double k;
dy = obsPt.y - pRobotInford->y;
dx = obsPt.x - pRobotInford->x;
dist = sqrt(dx*dx + dy*dy);
if(dist < 5.0) dist = 5.0;
// if(dist > 50)
speed = m_MoveParameter.V_max;
/* else if(dist > 30)
speed = m_MoveParameter.V_max-25;
else
speed = m_MoveParameter.V_max/2;
*/
theta_ro = atan2(dy,dx);
theta_ro= cn_AngleTrim2PI(theta_ro);
dy = target.y - pRobotInford->y;
dx = target.x - pRobotInford->x;
theta_rt = atan2(dy,dx);
theta_rt= cn_AngleTrim2PI(theta_rt);
double thetaBase = theta_rt + pi;
double theta_RO = theta_ro - thetaBase;
theta_RO = cn_AngleTrim2PI(theta_RO);
if( pi/2 < theta_RO && theta_RO < 3*pi/2 )
{
if(theta_RO < pi)
IsClockWise = 1;
else
IsClockWise = -1;
if(dist > 50)
k = 1;
else if(dist > 30)
k = 15;
else if(150.0*pi/180 < theta_RO && theta_RO < 210.0*pi/180)
k = 16;
else
k = 4;
moveTheta = theta_ro + k*IsClockWise/dist*(pi/2);
theta_RO = moveTheta - thetaBase;
theta_RO = cn_AngleTrim2PI(theta_RO);
if(IsClockWise == 1)
{ if(theta_RO < pi)
{
moveTheta = theta_rt;
speed = m_MoveParameter.V_max;
}
}
else
{
if(theta_RO > pi)
{
moveTheta = theta_rt;
speed = m_MoveParameter.V_max;
}
}
}
else
moveTheta = theta_rt;
MoveOnAngle(pRobotInford, moveTheta,speed,pSpeed);
return 1;
}
/************************************************************************************
函数名 ：Vect_MidShoot1(dbROBOTPOSTURE* pRobotInford, BallInformation &ball,dbLRWheelVelocity *pSpeed)
创建日期：19/4/2001
作者 ：
功能 ：射门
参数 ： robot 机器人位姿
ball 球
**************************************************************************/
int CDecisionMakingx::Vect_MidShoot1(dbROBOTPOSTURE pRobotInford, BallInformation &ball,dbLRWheelVelocity *pSpeed)
{
dbPOINT goal,ballPt;
double delta,angletemp;
delta = 10;
ballPt = ball;
goal.x = wallright + 3;
goal.y = walltop/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>wallright - deltax)&&(ball.y>CENTER_Y - goal_y_widthM/2-deltax&&ball.y<CENTER_Y + goal_y_widthM/2 + deltax))
{
if(ball.y<=CENTER_Y)
goal.y = ball.y + deltax;
else
goal.y = ball.y - deltax;
goal.x = wallright + 8;
ToPositionNew(&pRobotInford,ballPt,goal,100,1,pSpeed);
return 1;
}*/
ToPositionNew(&pRobotInford,ballPt,goal,65,2,pSpeed);
if(pRobotInford.x>ball.x)
{
dbPOINT pt;
double det;
det = 30;
pt.x =ballPt.x +30;
pt.y = 2*ballPt.y-pRobotInford.y;
if(pt.y>ballPt.y)
pt.y = ballPt.y + 5;
else
pt.y = ballPt.y - 5;
if(ballPt.y<det)
pt.y = ballPt.y - 3;
else if(ballPt.y>walltop-det)
pt.y = ballPt.y + 3;
ToPositionNew(&pRobotInford,ballPt,pt,55,0,pSpeed);
}
return 1;
}
/************************************************************************************
函数名 ：Vect_MidShoot(robotInformation &robot, ballInformation &ball)
创建日期：19/4/2001
作者 ：
功能 ：射门
参数 ： robot 机器人位姿
ball 球
**************************************************************************/
int CDecisionMakingx::Vect_MidShoot(dbROBOTPOSTURE* pRobotInford, BallInformation &ball,dbLRWheelVelocity *pSpeed)
{
double dx,dy;
double theta;
dbPOINT goal, midPoint, target;
dbPOINT top_goalcorner,down_goalcorner;
double thetaR2Ball,thetaR2goal1,thetaR2goal2;
goal.x = RGATE_X+1;
goal.y = RGATE_Y;
////////////////////////////////////////////////////
top_goalcorner.x = wallright;
top_goalcorner.y = GATE_UP_LINE;
down_goalcorner.x = wallright;
down_goalcorner.y = GATE_DN_LINE;
///////////////////////////////////////////////////
// modify in hrb
if(pRobotInford->x < ball.x - 5 && pRobotInford->x < wallright - 3) // -3 add 5.1
{
dx = ball.x - pRobotInford->x;
dy = ball.y - pRobotInford->y;
thetaR2Ball = atan2(dy,dx);
thetaR2Ball = cn_AngleTrim2PI(thetaR2Ball);
thetaR2Ball -= pi;
if(thetaR2Ball < 0) thetaR2Ball += 2*pi;
// rotate change: 180 degree as the compared line; from -<- to ->-
// compare the angle(Rb to Ball) with theta(Rb to GateupLine and GateDownLine)
dx = top_goalcorner.x - pRobotInford->x;
dy = top_goalcorner.y - pRobotInford->y;
thetaR2goal1 = atan2(dy,dx);
thetaR2goal1 = cn_AngleTrim2PI(thetaR2goal1);
thetaR2goal1 -= pi;
if(thetaR2goal1 < 0) thetaR2goal1 += 2*pi;
// with the point of gate down line.
dx = down_goalcorner.x - pRobotInford->x;
dy = down_goalcorner.y - pRobotInford->y;
thetaR2goal2 = atan2(dy,dx);
thetaR2goal2 = cn_AngleTrim2PI(thetaR2goal2);
thetaR2goal2 -= pi;
if(thetaR2goal2 < 0) thetaR2goal2 += 2*pi;
//////////////-----------------------------/
if( thetaR2Ball > thetaR2goal2 && thetaR2Ball < thetaR2goal1)
{
target.x = ball.x;target.y = ball.y;
////////// the follow is add on 5.1
theta = pRobotInford->theta;
theta = cn_AngleTrim2PI(theta);
theta -= thetaR2Ball;
if(theta > pi/2) theta -= pi;
if(theta < -pi/2) theta += pi;
if(fabs(theta) > 50.0*pi/180.0) //20)
{
TurnToPointPD(pRobotInford,target,NOCLOCK,pSpeed); // mmm
return 1;
}
else if(fabs(theta) < 20.0*pi/180.0)//15)
{
Move2Pt(pRobotInford,target,m_MoveParameter.V_MAX,pSpeed); // mmmm 80
return 1;
}
}
}
// -----have need to modi DEB k, dist deta_y ------//
double deta_y = 0;
if(ball.x < wallright - 20)
{
if(pRobotInford->y > CENTER_Y)
{
if(pRobotInford->y > ball.y + 2)
deta_y = 10;
}
else
if( pRobotInford->y < ball.y - 2)
deta_y = -10;
if(pRobotInford->y < CENTER_Y)
deta_y = 20;
}
if(pRobotInford->x > wallright - 15)
{
if((pRobotInford->y > CENTER_Y - 15)&&(pRobotInford->y < CENTER_Y + 15))
{
deta_y = ball.y - RGATE_Y;
}
}
BallInformation mball;
mball.x=ball.x - 4*(wallright - ball.x)/sqrt((wallright - ball.x)*(wallright - ball.x) + (ball.y - CENTER_Y)*(ball.y - CENTER_Y) + 0.0001);
mball.y=ball.y - 4*(ball.y - CENTER_Y)/sqrt((wallright - ball.x)*(wallright - ball.x) + (ball.y - CENTER_Y)*(ball.y - CENTER_Y) + 0.0001);
midPoint = GetMidPoint(pRobotInford,mball,deta_y);
if(midPoint.x == -1)
{
dbPOINT target,obsPt,goal;
obsPt.x = ball.x; obsPt.y = ball.y;
goal.x = wallright;goal.y = CENTER_Y;
Formulation bg_line;
StdLineForm(ball, goal, &bg_line);//aX+bY+c=0
target.x = ball.x - 20;
if(bg_line.b != 0)
target.y = -(bg_line.a*target.x + bg_line.c)/bg_line.b;
else
target.y = ball.y;
if(target.y > walltop - BOUND1) target.y = walltop - BOUND1;
else if(target.y < wallbottom + BOUND1) target.y = wallbottom +BOUND1;
// if(ball.y < CENTER_Y)
if(pRobotInford->y < ball.y)
target.y = ball.y - 10;
else
target.y = ball.y + 10;
AvoidPt2Pt(pRobotInford, obsPt, target,pSpeed);
// ToPositionPD(pRobotInford,target,10.0,pSpeed);
}
else
// ToPositionPD(pRobotInford,midPoint,m_MoveParameter.V_max,10.0,pSpeed);
Move2Pt(pRobotInford,midPoint,pSpeed);
return 1;
}
///////////////////////////////////////////////////////
//函数 Attack()
//函数名 进攻
///////////////////////////////////////////////////////
int CDecisionMakingx::AttackAction(dbROBOTPOSTURE *pRobotInford,BallInformation ball ,dbLRWheelVelocity *pSpeed)
{
double dx,dy;
double theta;
dbPOINT target,top_goalcorner,down_goalcorner;
double thetaR2Ball,thetaR2goal1,thetaR2goal2;
////////////////////////////////////////////////////
top_goalcorner.x = wallright;
top_goalcorner.y = GATE_UP_LINE;
down_goalcorner.x = wallright;
down_goalcorner.y = GATE_DN_LINE;
///////////////////////////////////////////////////
// modify in hrb
dx = ball.x - pRobotInford->x;
dy = ball.y - pRobotInford->y;
thetaR2Ball = atan2(dy,dx);
thetaR2Ball = cn_AngleTrim2PI(thetaR2Ball);
thetaR2Ball -= pi;
if(thetaR2Ball < 0) thetaR2Ball += 2*pi;
// rotate change: 180 degree as the compared line; from -<- to ->-
// compare the angle(Rb to Ball) with theta(Rb to GateupLine and GateDownLine)
dx = top_goalcorner.x - pRobotInford->x;
dy = top_goalcorner.y - pRobotInford->y;
thetaR2goal1 = atan2(dy,dx);
thetaR2goal1 = cn_AngleTrim2PI(thetaR2goal1);
thetaR2goal1 -= pi;
if(thetaR2goal1 < 0) thetaR2goal1 += 2*pi;
// with the point of gate down line.
dx = down_goalcorner.x - pRobotInford->x;
dy = down_goalcorner.y - pRobotInford->y;
thetaR2goal2 = atan2(dy,dx);
thetaR2goal2 = cn_AngleTrim2PI(thetaR2goal2);
thetaR2goal2 -= pi;
if(thetaR2goal2 < 0) thetaR2goal2 += 2*pi;
target.x = ball.x;target.y = ball.y;
if( thetaR2Ball > thetaR2goal2 && thetaR2Ball < thetaR2goal1)
{
////////// the follow is add on 5.1
theta = pRobotInford->theta;
theta = cn_AngleTrim2PI(theta);
theta -= thetaR2Ball;
if(theta > pi/2) theta -= pi;
if(theta < -pi/2) theta += pi;
if(fabs(theta) > 40.0*pi/180.0) //20)
{
TurnToPointPD(pRobotInford,target,NOCLOCK,pSpeed);
return 1;
}
else if(fabs(theta) < 40.0*pi/180.0)
{
Move2Pt(pRobotInford,target,m_MoveParameter.V_MAX,pSpeed);
return 1;
}
}
else
Move2Pt(pRobotInford,target,pSpeed);
return 1;
}
///////////////////////////////////////////////////////
//函数 ClearBall()
//函数名 扫球
///////////////////////////////////////////////////////
int CDecisionMakingx::ClearBall(dbROBOTPOSTURE *pRobot,ballInformation ball ,dbLRWheelVelocity *pSpeed)
{/*
double theta_rb;
double dx,dy;
double dist;
int IsClockWise = 0;
double moveTheta;
int k = 1;
///////////////////////////////////////////
dx = ball.x - pRobot->x;
dy = ball.y - pRobot->y;
dist = sqrt(dx*dx + dy*dy);
theta_rb = atan2(dy,dx);
theta_rb = cn_AngleTrim2PI(theta_rb);
pRobot->theta = cn_AngleTrim2PI(pRobot->theta);
if(dist > 40) k = 10;
else k = 15;//12
if(pRobot->x < ball.x-3) k=0;
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -