📄 decisionmakingx.cpp
字号:
//计算机器人到目标点的角度
Dx = directionpt.x - robot->x;Dy = directionpt.y - robot->y;
dist = sqrt(Dx*Dx+Dy*Dy);
if(distRobot2Pt(*robot,directionpt)<20)
samespeed = 40*distRobot2Pt(*robot,directionpt)/20;
double kp4new;
kp4new = 15.0;
if(samespeed<=40)
kp4new = 15;// kp4new*samespeed/vemax;
//计算targetpt到directionpt的角度
Anglerb2target = atan2(Dy ,Dx);
Anglerb2target = cn_AngleTrim2PI(Anglerb2target);
//计算下个周期的目标角度
double disiredAngle;
disiredAngle = Anglerb2target;
disiredAngle = cn_AngleTrim2PI(disiredAngle);
//计算角度偏差
double Angle_e;
Angle_e = disiredAngle - robot->theta;
Angle_e = cn_AngleTrim2PI(Angle_e);
//计算左右轮速度差并计算出左右轮速度
double ka,limitation,a;
a = pi*0.3;
limitation = 100;
ka=samespeed;
double speed_e;
if(Angle_e <= pi/2)//角度偏差在第一象限
{
speed_e = kp4new*Angle_e;
speed_e = Limit(speed_e,limitation);
if(a-fabs(Angle_e)>0)
samespeed = ka*(a-fabs(Angle_e))/a;
else samespeed = 0;
pSpeed->LeftValue = samespeed + speed_e/2;
pSpeed->RightValue = samespeed - speed_e/2;
}
else if(Angle_e <= pi)
{
speed_e = kp4new*(pi - Angle_e);
speed_e = Limit(speed_e,limitation);
if(a-fabs(pi-Angle_e)>0)
samespeed = ka*(a-fabs(pi-Angle_e))/a;
else samespeed = 0;
pSpeed->LeftValue = samespeed + speed_e/2;
pSpeed->RightValue = samespeed - speed_e/2;
pSpeed->LeftValue =- pSpeed->LeftValue;
pSpeed->RightValue =- pSpeed->RightValue;
}
else if(Angle_e<3*pi/2)
{
speed_e = kp4new*(Angle_e - pi);
speed_e = Limit(speed_e,limitation);
if(a-fabs(Angle_e - pi)>0)
samespeed = ka*(a-fabs(Angle_e - pi))/a;
else samespeed = 0;
pSpeed->LeftValue = samespeed - speed_e/2;
pSpeed->RightValue = samespeed + speed_e/2;
pSpeed->LeftValue =- pSpeed->LeftValue;
pSpeed->RightValue =- pSpeed->RightValue;
}
else
{
speed_e = kp4new*(2*pi - Angle_e);
speed_e = Limit(speed_e,limitation);
if(a-fabs(2*pi - Angle_e)>0)
samespeed = ka*(a-fabs(2*pi - Angle_e))/a;
else samespeed = 0;
pSpeed->LeftValue = samespeed - speed_e/2;
pSpeed->RightValue = samespeed + speed_e/2;
}
if(pSpeed->LeftValue>vemax)
{
pSpeed->LeftValue = vemax;
pSpeed->RightValue = pSpeed->LeftValue - fabs(speed_e);
}
if(pSpeed->LeftValue<-vemax)
{
pSpeed->LeftValue = -vemax;
pSpeed->RightValue = pSpeed->LeftValue + fabs(speed_e);
}
if(pSpeed->RightValue<-vemax)
{
pSpeed->RightValue = -vemax;
pSpeed->LeftValue = pSpeed->RightValue + fabs(speed_e);
}
if(pSpeed->RightValue>vemax)
{
pSpeed->RightValue = vemax;
pSpeed->LeftValue = pSpeed->RightValue - fabs(speed_e);
}
return 1;
}
//守门员用
int CDecisionMakingx::ToPositionPDGoal(dbROBOTPOSTURE* pROBOTPOSTURE,dbPOINT Target,double startspeed,double endspeed,dbLRWheelVelocity* pLRWheelVelocity)
{
if(startspeed > 45)startspeed = 45;
int clock_sign,move_sign;
double theta,theta_e1;//e1为当前角度误差
static double theta_e2 = 0;//e2为上一周期角度误差
double dx,dy,dx1,dy1,distance;
double same_speed;
//坐标变换，以小车中心为原点，小车朝向为y轴
dx1 = Target.x - pROBOTPOSTURE->x;
dy1 = Target.y - pROBOTPOSTURE->y;
dx = dx1*cos(pROBOTPOSTURE->theta - pi/2) + dy1*sin(pROBOTPOSTURE->theta - pi/2);
dy = -dx1*sin(pROBOTPOSTURE->theta - pi/2) + dy1*cos(pROBOTPOSTURE->theta - pi/2);
distance = sqrt(dx*dx + dy*dy);
if(distance > m_MoveParameter.max_distanceG)
same_speed = startspeed;
else
{
same_speed = distance/m_MoveParameter.max_distanceG*startspeed;
if (same_speed>startspeed)
same_speed=startspeed;
}
if(same_speed < endspeed) same_speed = endspeed;
theta = atan2(dy,dx);
if(fabs(fabs(theta) - pi/2) > m_AngleParameter.MaxAngle)
{
TurnToPointPD(pROBOTPOSTURE,Target,NOCLOCK,pLRWheelVelocity);
pLRWheelVelocity->LeftValue /= 3;
pLRWheelVelocity->RightValue /= 3;
return 0;
}
theta = cn_AngleTrim2PI(theta);
if(theta <= pi/2)//第一象限
{
move_sign = 1;
clock_sign = 1;
theta_e1 = pi/2 - theta;
}
else if(theta <= pi)//第二象限
{
move_sign = 1;
clock_sign = -1;
theta_e1 = theta - pi/2;
}
else if(theta <= 3*pi/2)//第三象限
{
move_sign = -1;
clock_sign = 1;
theta_e1 = 3*pi/2 - theta;
}
else//第四象限
{
move_sign = -1;
clock_sign = -1;
theta_e1 = theta - 3*pi/2;
}
if(theta_e1 > 30.0/180.0*pi)
same_speed = same_speed*(1-theta_e1/(pi/2));
double kp,kd;
kp = m_MoveParameter.kp4pospdG;
kd = m_MoveParameter.kd4pospdG;
if(distance < 20 && fabs(theta_e1) < pi/6)
{
kp *= 0.4;
kd *= 0.4;
}
pLRWheelVelocity->LeftValue = same_speed*move_sign + clock_sign*(kp*theta_e1 + kd*(theta_e1- theta_e2));
pLRWheelVelocity->RightValue = same_speed*move_sign - clock_sign*(kp*theta_e1 + kd*(theta_e1- theta_e2));
//保存本周期角度误差，下一周期作微分用
theta_e2 = theta_e1;
return 0;
}
//基本动作函数结束
//////////////////////////////////////////////////////////////
// 技术动作函数 //
//////////////////////////////////////////////////////////////
/************************************************************************************
函数名 ：BoundProcess(int which, ballInformation &ball)
创建日期：19/4/2001
作者 ：
功能 ：边界处理
参数 ： which 机器人号码
ball 球
**************************************************************************/
int CDecisionMakingx::BoundProcess(dbROBOTPOSTURE *pRobot, BallInformation Ball,int field,dbLRWheelVelocity *pSpeed)
//本函数用于处理边界球，成功返回1
//pRobot为小车的位置信息，pBall为球的位置信息，field为球所在的边界区号，pSpeed为返回的左右轮速
// |--------------|-------------------------------------------|-------------|
// |LUpCorner=5 | UpBound=1 |RUpCorner=3 |
// |---|--------------|--------------|---------------|------------|-------------|---|
// | |LZone=10 |BackField=7 |MidField=8 |FrontField=9| RZone=11 | |
// | | | | | | | |
// |---|--------------|--------------|---------------|------------|-------------|---|
// |LDownCorner=6 | DownBound=2 |RDownCorner=4|
// |--------------|-------------------------------------------|-------------|
{
dbPOINT point,point1,target;
double RtTheta;
point.x = Ball.x;
point.y = Ball.y;
point1.x = pRobot->x;
point1.y = pRobot->y;
RtTheta = pRobot->theta;
RtTheta = cn_AngleTrim2PI(pRobot->theta);
switch (field)
{
case 1:
{
if(RtTheta<pi/2)
{
target.x = point1.x + 5;
target.y = point1.y + 2;
}
else if(RtTheta<pi)
{
target.x = point1.x + 5;
target.y = point1.y - 2;
}
else if(RtTheta<3*pi/2)
{
target.x = point1.x + 5;
target.y = point1.y + 2;
}
else
{
target.x = point1.x + 5;
target.y = point1.y - 2;
}
break;
}
case 2:
{
if(RtTheta<pi/2)
{
target.x = point1.x - 2;
target.y = point1.y - 5;
}
else if(RtTheta<pi)
{
target.x = point1.x + 2;
target.y = point1.y - 5;
}
else if(RtTheta<3*pi/2)
{
target.x = point1.x - 2;
target.y = point1.y - 5;
}
else
{
target.x = point1.x + 2;
target.y = point1.y - 5;
}
break;
}
case 3:
{
if(RtTheta<pi/2)
{
target.x = point1.x - 5;
target.y = point1.y - 5;
}
else if(RtTheta<pi)
{
target.x = point1.x - 5;
target.y = point1.y + 2;
}
else if(RtTheta<3*pi/2)
{
target.x = point1.x - 5;
target.y = point1.y - 5;
}
else
{
target.x = point1.x - 5;
target.y = point1.y + 2;
}
break;
}
case 4:
{
if(RtTheta<pi/2)
{
target.x = point1.x + 5;
target.y = point1.y + 2;
}
else if(RtTheta<pi)
{
target.x = point1.x + 5;
target.y = point1.y - 2;
}
else if(RtTheta<3*pi/2)
{
target.x = point1.x + 5;
target.y = point1.y + 2;
}
else
{
target.x = point1.x + 5;
target.y = point1.y - 2;
}
break;
}
case 5:
{
if(RtTheta<pi/2)
{
target.x = point1.x + 2;
target.y = point1.y + 5;
}
else if(RtTheta<pi)
{
target.x = point1.x - 2;
target.y = point1.y + 5;
}
else if(RtTheta<3*pi/2)
{
target.x = point1.x + 2;
target.y = point1.y + 5;
}
else
{
target.x = point1.x - 2;
target.y = point1.y + 5;
}
break;
}
case 6:
{
if(RtTheta<pi/2)
{
target.x = point1.x - 5;
target.y = point1.y - 2;
}
else if(RtTheta<pi)
{
target.x = point1.x - 5;
target.y = point1.y + 5;
}
else if(RtTheta<3*pi/2)
{
target.x = point1.x - 5;
target.y = point1.y - 2;
}
else
{
target.x = point1.x - 5;
target.y = point1.y + 2;
}
break;
}
}
ToPositionN(pRobot,target,60,pSpeed);
return 1;
}
/************************************************************************************
函数名 ：GoalieAction(robotInformation &robot, ballInformation &ball, dbPOINT &proball)
创建日期：19/4/2001
作者 ：
功能 ：守门
参数 ： robot 机器人位姿
ball 球
**************************************************************************/
int CDecisionMakingx::GoalieAction1(dbROBOTPOSTURE* pRobotInford, BallInformation ball,dbLRWheelVelocity *pSpeed)
{
//int homeline =8; G_OFFSET
int Error_y = 2; //block goal area up and down corner
int xError = 3; // homePt.x+xError and homePt.x-xError;
int gtError = 2, glError = 2; // gate corner and goal corner +-
dbPOINT target;
dbPOINT homePt;
double dx,dy;
static double turn_E2 = 0;
int goToken=0; // go first then turn;
homePt.x = G_OFFSET;
homePt.y = CENTER_Y;
target = homePt;
///////////////////////////////////
pRobotInford->theta = cn_AngleTrim2PI(pRobotInford->theta);
if(ball.x > CENTER_X + 40)
{
dx = pRobotInford->x - homePt.x;
dy = pRobotInford->y - homePt.y;
if(dx*dx + dy*dy < 2*2)
if(pRobotInford->theta < pi)
TurnToAnglePD(pRobotInford,pi/2,NOCLOCK,pSpeed);
else
TurnToAnglePD(pRobotInford,3*pi/2,NOCLOCK,pSpeed);
else
ToPositionPD(pRobotInford,homePt,m_MoveParameter.V_max,0.0,pSpeed);
}
else // ball in process area;
{ // Forcast ball next postition
if(m_decisionparamter.oldBallPt[0].x > m_decisionparamter.oldBallPt[3].x && m_decisionparamter.oldBallPt[3].x > m_decisionparamter.oldBallPt[6].x) // GG new add
{
target.y = GetCrossPtWithGLLine(G_OFFSET); //update oldball previous
}
else
target.y = 0;
if(target.y > GATE_UP_LINE || target.y < GATE_DN_LINE)//交点出了门区
{
target.y = ball.y;
if(ball.x < homePt.x + 5)//x方向离门很近，目标点在禁区里2厘米处
{
if(target.y < GOAL_DN_LINE)
{
target.y = GOAL_DN_LINE + glError;
if(pRobotInford->y < GATE_DN_LINE - glError ) //跑出门区
goToken = 2;//回目标点
}
else if(target.y > GOAL_UP_LINE)
{
target.y = GOAL_UP_LINE - glError;
if(pRobotInford->y > GATE_UP_LINE + glError )
goToken = 2;
}
}
else if(ball.x > homePt.x + 20)//离门>25厘米
{
if(target.y < GATE_DN_LINE)
target.y = GATE_DN_LINE + 5; // GGG
else if(target.y > GATE_UP_LINE)
target.y = GATE_UP_LINE - 5; // GGG
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -