action.cpp

东北大学机器人足球队决策系统设计.开发环境使用 VS.NET 2003 调试 DLL 程序方法： 启动 VS.Net 选择“工具”菜单

/****************************************************************************************************
action.h 艾数机器人角色动作头文件

Purpose:
	角色可以使用的动作

author:
	yy_cn
	
Created Time:
	2006-5-10
****************************************************************************************************/

#include "action.h"

////////////////////////////////////////////////////////////////////////////////////////////////////
//
// 动作系统，提供机器人的一系统动作支持
//

//
// actVelocity
//
void actVelocity (Robot &robot, int vl, int vr)
{
	if (vr > 125)
		vr = 125;
	if (vl > 125)
		vl = 125;
	if (vr < -125)
		vr = -125;
	if (vl < -125)
		vl = -125;

	robot.velocityLeft  = vl;
	robot.velocityRight = vr;
}

//
// move
//
//void move (Robot &robot, BallDirection direction)
//{
//
//}

//
// turn
//
void actTurn (Robot &robot, double angle)
{
	int theta_e, vl, vr;
	theta_e = (int) angle - (int) robot.rotation;	
	
	while (theta_e > 180)
		theta_e -= 360;

	while (theta_e < -180)
		theta_e += 360;

	if (theta_e < -90)
		theta_e += 180;
	else if (theta_e > 90)
		theta_e -= 180;

	if (abs(theta_e) > 50) {
		vl = (int) (-9./90.0 * (double)theta_e);
		vr = (int) (9./90.0 * (double)theta_e);
	}
	else if (abs(theta_e) > 20)  {
		vl = (int) (-11.0/90.0 * (double)theta_e);
		vr = (int) (11.0/90.0 * (double)theta_e);
	}

	if (vl < 0)
		vl = 0;
	if (vr < 0)
		vr = 0;

	actVelocity (robot, vl, vr);
}

//
// toPosition
//
void actToPosition (Robot &robot, double x, double y)
{
	double desired_angle = 0,
		   theta_e = 0,
		   d_angle = 0;
	int	vl,
		vr,
		vc = 200;

	double dx, dy, d_e, Ka = 1.0;//10.0/90.0;

	//
	// 计算到目标位置的直线距离
	//
	d_e = getDistance (robot.pos.x, robot.pos.y, x, y);

	//
	// 计算当前位置与目标的相对位移
	//
	dx = x - robot.pos.x;
	dy = y - robot.pos.y;

	//
	// 计算当前位置到目标点的角度
	//
	if (dx == 0 && dy == 0)
		desired_angle = 90;
	else
		desired_angle = 180. / PI * atan2((double)(dy), (double)(dx));

	//
	// 当前机器人与目标的夹角
	//
	theta_e = desired_angle - robot.rotation;
	
	while (theta_e > 180)
		theta_e -= 360;

	while (theta_e < -180)
		theta_e += 360;

	//if (d_e > 100.) 
	//	Ka = 17. / 90.;			// 0.19
	//else if (d_e > 50)
	//	Ka = 19. / 90.;			// 0.21
	//else if (d_e > 30)
	//	Ka = 21. / 90.;			// 0.23
	//else if (d_e > 20)
	//	Ka = 23. / 90.;			// 0.25
	//else 
	//	Ka = 25. / 90.;			// 0.27

	//if (d_e < 10.0 && d_e > 5.0) {
	//	Ka = 0.1;
	//	vc = 70;
	//}
	//else 
	if (d_e < 10.0 && d_e > 5.0) {
		vc = 40;
	}
	else if (d_e < 4.0 && d_e > 2.0) {
		Ka = 0.1;
		vc = 30;
	}
	else if (d_e < 2.0) {
		Ka = 0.1;
		vc = 20;
	}
	else if (d_e < 1.0) {
		Ka = 0.1;
		vc = 0;
	}
	//if (d_e < 3.0 && d_e > 2.0) {
	//	Ka = 0.1;
	//	vc /= 20;
	//}
	//else if (d_e < 2.0 && d_e > 1.0) {
	//	Ka = 0.1;
	//	vc /= 50;
	//}
	//else if (d_e < 1.0) {
	//	Ka = 0.1;
	//	vc /= 100;
	//}

	if (theta_e > 95 || theta_e < -95) {
		theta_e += 180;
		
		if (theta_e > 180) 
			theta_e -= 360;
		if (theta_e > 80)
			theta_e = 80;
		if (theta_e < -80)
			theta_e = -80;

		//if (d_e < 5.0 && abs(theta_e) < 40)
		//	Ka = 0.1;

		vr = (int) (-vc * (1.0 / (1.0 + exp(-3.0 * d_e)) - 0.3) + Ka * theta_e);
		vl = (int) (-vc * (1.0 / (1.0 + exp(-3.0 * d_e)) - 0.3) - Ka * theta_e);
	}	
	else if (theta_e < 85. && theta_e > -85.) {
		//if (d_e < 5.0 && abs(theta_e) < 40)
		//	Ka = 0.1;

		vr = (int) (vc * (1.0 / (1.0 + exp(-3.0 * d_e)) - 0.3) + Ka * theta_e);
		vl = (int) (vc * (1.0 / (1.0 + exp(-3.0 * d_e)) - 0.3) - Ka * theta_e);
	}
	else {
		vr = (int) (+.1 * theta_e);//(int) (+.17 * theta_e);
		vl = (int) (-.1 * theta_e);//(int) (-.17 * theta_e);
	}

	actVelocity (robot, vl, vr);
}

//
// shoot
//

bool canShoot (double robotX, double robotY, double ballX, double ballY)
{
	/*
		+---------------------------+
		|                           |
		|-            1  2  3      -|- GTOPY		
		| |           4  *  5     | | <- 假设敌方球门
		|-            6  7  8      -|- GBOTY
		|                           |
		+---------------------------+
		<-后                     前->

		1,2,3,4,5,6,7,8 机器人位置
		* 球的位置
	 */
	//
	// 如果机器人的 X 坐标比球的 X 坐标更接近对方球门说明机器人在射门的反方向
	// 不能直接射门，需要跑到球的 X 坐标更接近对方球门的坐标位置
	// 机器人在 3 5 8 位置的情况直接返回不能射门
	//
	if (analyseFieldInfo () == BF_IS_LEFT && robotX > ballX) {
		//
		// 我方在左半场，敌方球门在右半场
		// 机器人的 X 坐标要小于球的坐标才可以直接射门
		//
		return false;
	}
	else if (analyseFieldInfo () == BF_IS_RIGHT && robotX < ballX) {
		//
		// 我方在右半场，敌方球门在左半场
		// 机器人的 X 坐标要大于球的坐标才可以直接射门
		//
		return false;
	}

	//
	// 跟据机器人坐标，球的坐标两点形成的直线映射到对方球门 Y 位置的点
	// 是否在球门范围内来确定是否可以直接射门
	//
	double targetX = OPP_GOAL_CENTER_X;
	double targetY = getMappedTargetY (robotX, robotY, ballX, ballY, targetX);

	//
	// targetY 为跟据 机器人 与 球 之间角度求出 targetX 处 Y 值
	//
	if (targetY < GTOPY && targetY > GBOTY)
		return true;

	return false;
}

void actShoot (Robot &robot, const Environment &env)
{
	//
	// 射门 - 踢球的情况相同
	// 1. 机器人离球非常近，球的速度为中低速，则使用以下方法能够准确的射门
	//    直接以 机器人的中心点 与 球的点心点 形成的一条直线向对方球门区域内移动
	//    将移动的目标点设置为一个比较比较远的位置，此时机器人与球相撞产生的速度能让球快速移动
	//    如果机器人当前的坐标不适合射门，则将机器人移动到一个合适的最近的位置
	//
	// 2. 机器人离球有一定的距离，球的速度为快速，则使用以下方法能够准确的射门
	//    球的运动方向与机器人水平移动的方向能够产生一个交点
	//    且产生交点的机器人移动的方向为敌方方向
	//
	
	//
	// 暂时只考虑 1 的情况
	//
	

	//
	// 射门，先实现简单方法，直接以机器人与球，球门中心的角度踢
	//
	// 有时间的话进行精确的计算，检查机器人，球，球门中心角度的直线上的敌人
	// 射门角度以球所在的区域来击向敌方的死角
	//
	/*
		+------------------+
		|               6 7|
		|-            1   -|
		| |           2  | |
		|-            3   -|
		|               4 5|
		+------------------+
	 */
	double ballX = env.currentBall.pos.x;
	double ballY = env.currentBall.pos.y;

	double targetX, targetY;
	if (canShoot (robot.pos.x, robot.pos.y, ballX, ballY) == false) {
		//
		// 角度不正确，移动机器人到合适的位置 - 面向对方球门方向，在球的边上
		//
		if (analyseFieldInfo () == BF_IS_LEFT) {
			//
			// 我方在左半场，移动到球的左边的一个球位置
			//
			targetX = ballX - BALL_DIAMETER;
		}
		else {
			//
			// 我方在右半场，移动到球的右边的一个球位置
			//
			targetX = ballX + BALL_DIAMETER;
		}

		//
		// 移动的目标点为球门的边角
		//
		double shootX = OPP_GOAL_CENTER_X;
		double shootY = 0;//OPP_GOAL_CENTER_Y;
		if (robot.pos.y < ballY) {
			//
			// 机器人在球的下方
			//
			shootY = GTOPY - BALL_DIAMETER - BALL_DIAMETER;
		}
		else {
			//
			// 机器人在球的上方
			//
			shootY = GBOTY + BALL_DIAMETER - BALL_DIAMETER;
		}

		transform (shootX , shootY);

		//
		// 对方球门中心为第一个点，球为第二个点， targetX 为已知的 X 点，求出 targetY
		//
		targetY = getMappedTargetY (shootX, shootY, ballX, ballY, targetX);
		if (targetX < FLEFTX || targetX > FRIGHTX || targetY < FBOT || targetY > FTOP) {
			//
			// 球靠近场地边框，直接撞球
			//
			actToPosition (robot, ballX, ballY);
			//actBezier2Reach (robot, ballX, ballY, 2.5);
			return;
		}

		//
		// 移动到 targetX, targetY 点
		//
		actToPosition (robot, targetX, targetY);
		//actBezier2Reach (robot, targetX, targetY, 2.5);
		return;
	}

	//
	// 角度正确，获得一个非常远的映射点的为目标点移动达到射门的目的
	// 如果目标点很远，机器人离球越近则射门力度越大
	//
	targetX = FRIGHTX + FIELD_WIDTH / 2;
	transform (targetX, targetY);

	targetY = getMappedTargetY (robot.pos.x, robot.pos.y, ballX, ballY, targetX);
	actToPosition (robot, targetX, targetY);
	//actBezier2Reach (robot, targetX, targetY, 2.5);
}

//
// kickBall
//
/*
	  **********************************
****** 以下参数均以我方在左半场为参考值 *******
	  **********************************

	      A     B          C      D
	    |--|---------|----------|---|
		+---------------------------+
	    |            |              |
	   _|+++         |           +++|_
	  | |  +         |           +  | | ____ 中点 centerY
	  |_|  +         |           +  |_|
		|+++         |           +++|
	    |            |              |
		+---------------------------+
		<-我方球场         敌方球场->

		+---------------------------+
	    |            |              |
	   _|+++         | 1  2  3   +++|_ - GTOPY
	  | |  +         | 4  *  5   +  | | ____ 中点 centerY
	  |_|  +         | 6  7  8   +  |_|
		|+++         |           +++|  - GBOTY
	    |            |              |
		+---------------------------+
		<-后                     前->

		1,2,3,4,5,6,7,8 机器人位置
		* 球的位置

                          上
		+---------------------------+
	    |            |              |
	   _|+++         |  1  2  3  +++|_ 
   左 | |  +         |  4  *  5  +  | | 右
	  |_|  +         |  6  7  8  +  |_|
		|+++         |           +++|
	    |            |              |