www.pudn.com > StrategyClass1.rar > Strategy.cpp, change:2006-02-23,size:3667b


// Strategy.cpp : Defines the entry point for the DLL application. 
// 
 
#include "stdafx.h" 
#include "Strategy.h" 
#include "StrategySystem.h" 
 
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; 
} 
 
CStrategySystem* pStrategy = NULL; 
 
extern "C" STRATEGY_API void Create ( Environment *env ) 
{ 
	// allocate user data and assign to env->userData 
	// eg. env->userData = ( void * ) new MyVariables (); 
	pStrategy = new CStrategySystem; 
} 
 
extern "C" STRATEGY_API void Destroy ( Environment *env ) 
{ 
	// free any user data created in Create ( Environment * ) 
 
	// eg. if ( env->userData != NULL ) delete ( MyVariables * ) env->userData; 
 
	if (NULL != pStrategy) 
	{ 
		delete pStrategy; 
		pStrategy = NULL; 
	} 
 
} 
 
 
extern "C" STRATEGY_API void Strategy ( Environment *env ) 
{ 
	// the below codes are just for demonstration purpose....don't take this seriously please. 
	 
	pStrategy->Run(*env); 
 
} 
 
/******************************************************************************* 
*******************				常用函数					******************** 
*******************************************************************************/ 
 
 
/*************************** 
函数名:StandardAngle 
功  能:对角度进行归一化处理 
输  入:需要归一化处理的角度 
返  回:修改输入参数的值 
备  注:无 
***************************/ 
void StandardAngle(int *ang) 
{ 
       while(*ang > 180) 
              *ang -= 360; 
       while(*ang < -180) 
              *ang += 360;  
} 
 
 
/*************************** 
函数名:StandardAngle_pi 
功  能:对弧度进行归一化处理 
输  入:需要归一化处理的弧度 
返  回:修改输入参数的值 
备  注:无 
***************************/ 
void StandardAngle_pi(double *ang) 
{ 
       double pi = PI; 
       while(*ang > pi) 
              *ang -= 2*pi; 
       while(*ang < -1*pi) 
              *ang += 2*pi;  
} 
 
void CRobot::Position( const double& x, const double& y) 
{ 
	int desired_angle = 0, theta_e = 0, d_angle = 0, vl, vr, vc = 125; 
 
	double dx, dy, d_e, Ka = 10.0/90.0; 
	dx = x - pos.x; 
	dy = y - 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)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( vl, vr ); 
} 
 
void CRobot::Velocity(const int& vl, const int& vr) 
{ 
	velocityLeft = vl; 
	velocityRight = vr; 
}