strategy jinjianban.cpp

足球机器人比赛程序

// Strategy.cpp : Defines the entry point for the DLL application.
#include "stdafx.h"
#include "Strategy.h"
#include <math.h>


#define square(a) ((a)*(a))
#define leng(a,b,c,d) sqrt(square((a)-(c))+square((b)-(d)))

BOOL APIENTRY DllMain( HANDLE hModule, 
                       DWORD  ul_reason_for_call, 
                       LPVOID lpReserved )
{
    switch (ul_reason_for_call)
	{
		case DLL_PROCESS_ATTACH:
		case DLL_THREAD_ATTACH:
		case DLL_THREAD_DETACH:
		case DLL_PROCESS_DETACH:
			break;
    }
    return TRUE;
}

const double PI = 3.1415923;

char myMessage[200]; //big enough???



void PredictBall ( Environment *env );
void Velocity ( Robot *robot, int vl, int vr );
void Angle ( Robot *robot, int desired_angle);
void Position( Robot *robot, double x, double y );



void MoonAttack (Robot *robot, Environment *env );
void MoonFollowOpponent (  Robot *robot, OpponentRobot *opponent );



//以下是我加的
void NormalGame_Right( Environment *env );
void Attack0( Robot *robot , Environment *env);



extern "C" STRATEGY_API void Create ( Environment *env )
//void Create ( Environment *env )
{
	
	// allocate user data and assign to env->userData
	// eg. env->userData = ( void * ) new MyVariables ();
}

extern "C" STRATEGY_API void Destroy ( Environment *env )
//void Destroy ( Environment *env )
{

}

extern "C" STRATEGY_API void Strategy ( Environment *env )
{

	
	switch (env->gameState)
	{
		case 0:
		
			
			break;

		case FREE_BALL:
			NormalGame_Right(env);
			break;

		case PLACE_KICK:
			NormalGame_Right(env);
			break;	
			
		case PENALTY_KICK:
//			PenaltyKick_Right(&env->home[4],env);
			break;

		case FREE_KICK:
			NormalGame_Right(env);
			break;

		case GOAL_KICK:
			NormalGame_Right(env);
			break;
	}
}


void Attack0( Robot *robot , Environment *env)
{
    double vl=0,double vr=0; 
	Vector3D t=env->currentBall.pos;
	double r=robot->rotation;

	MoonAttack(&env->home[1], env);
	 if(r<0)r+=360;
	 if(r>360)r-=360;
	 


//	 if(t.y>env->fieldBounds.top-)

}



void NormalGame_Right( Environment *env )//右队总策略
{	
	

//	if(env->currentBall.pos.x>(FRIGHTX+FLEFTX)/2)
//	{
//		if(env->whosBall==YELLOW_BALL || (env->whosBall==ANYONES_BALL && env->currentBall.pos.x>env->lastBall.pos.x) || env->currentBall.pos.x>FRIGHTX-23)
//			Defender_Right(env);
//		else
//			Attack_Right(env);
//	}
//	else
//		Attack_Right(env);
}


void MoonAttack ( Robot *robot, Environment *env )
{
	PredictBall ( env );
	Position(robot, env->predictedBall.pos.x, env->predictedBall.pos.y);
}

void MoonFollowOpponent ( Robot *robot, OpponentRobot *opponent )
{
	Position(robot, opponent->pos.x, opponent->pos.y);
}

void Velocity ( Robot *robot, int vl, int vr )
{
	robot->velocityLeft = vl;
	robot->velocityRight = vr;
}

// robot soccer system p329
void Angle ( Robot *robot, int desired_angle)//原始Angle函数,有问题
{
	int theta_e, vl, vr;
	theta_e = desired_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);//20度以下没有处理
	}
	Velocity (robot, vl, vr);
}

void PredictBall ( Environment *env )
{

}

void Position( Robot *robot, double x, double y )//原始Position函数,两方向前进,有问题,不能精确定位,但能保证直线速度
{
	int desired_angle = 0, theta_e = 0, d_angle = 0, vl, vr, vc = 70;

	double dx, dy, d_e, Ka = 10.0/90.0;
	dx = x - robot->pos.x;
	dy = y - robot->pos.y;

	d_e = sqrt(dx * dx + dy * dy);
	if (dx == 0 && dy == 0)
		desired_angle = 90;
	else
		desired_angle = (int)(180. / PI * atan2((double)(dy), (double)(dx)));
	theta_e = desired_angle - (int)robot->rotation;
	
	while (theta_e > 180) theta_e -= 360;
	while (theta_e < -180) theta_e += 360;

	if (d_e > 100.) 
		Ka = 17. / 90.;
	else if (d_e > 50)
		Ka = 19. / 90.;
	else if (d_e > 30)
		Ka = 21. / 90.;
	else if (d_e > 20)
		Ka = 23. / 90.;
	else 
		Ka = 25. / 90.;
	
	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)(+.17 * theta_e);
		vl = (int)(-.17 * theta_e);
	}
	Velocity(robot, vl, vr);
}