basicaction.cpp

FIRA 5V5比赛中一个机器人源代码 本科毕业设计做的

// BasicAction.cpp: implementation of the CBasicAction class.
//
//////////////////////////////////////////////////////////////////////


#include "stdafx.h"
#include "BasicAction.h"
#include <math.h>
#include "Computation.h"
#include "stdlib.h"


//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

CBasicAction::CBasicAction(Environment *envPointer)
{
	env = envPointer;
}

CBasicAction::~CBasicAction()
{

}


void CBasicAction::avoidObstacle(Environment *env, int whichRobot,Vector3D targetPoint) //避障
{	
//	double PI = 3.1415923;
	int i;
	int direction = 1;


	if(isInBetweenPos(env, env->home[whichRobot].pos, targetPoint, env->home[env->userData->attacker].pos, 4) == 1)
		{
			orbit(env->home[i].pos,&env->home[whichRobot],whichRobot,9,direction);
	}
	else
	{
		for(i=1;i<5;i++)
		{
			if(isInBetweenPos(env, env->home[whichRobot].pos, targetPoint, env->opponent[i].pos, 4) == 1)
			{
				orbit(env->opponent[i].pos,&env->home[whichRobot],whichRobot,9,direction);
				break;
			}
		}
	}

	return;
}
void CBasicAction::brake(int whichRobot) //刹车
{
	env->home[whichRobot].velocityLeft = 0;
	env->home[whichRobot].velocityRight = 0;
}

double CBasicAction::decideRobotFront(double angle)
{
	double MAX_ANGLE_ERROR = 85;
	double robotFront;

	/*
	Front
	  |      --> 1 (Actual Head)
	-----
	| R	|
	| R |
	-----
	  |      --> -1 (Switch Actual Tail To Be Head)
	 Back
	*/

	if(angle > -MAX_ANGLE_ERROR && angle < MAX_ANGLE_ERROR)
	{
		robotFront = 1;
	}
	else
	{
		if ((angle <= 180 && angle > 180-MAX_ANGLE_ERROR) || (angle > -180 && angle <= -180+MAX_ANGLE_ERROR) )
		{
			if(angle < -90)	//-- switch robot's front direction
			{
				robotFront = -1;
			}
			else
			{
				if(angle > 90)
				{
					robotFront = -1;
				}
			}
		}
		else
		   robotFront = 0;
	}
	return robotFront;
}


bool CBasicAction::isInBetweenPos(Environment *env, Vector3D firstPosition, Vector3D secondPosition, Vector3D robotPosition, double desireWidth)
{
	
	double PI = 3.1415923;
	bool inLine;
	double gradient, angle, limit;
	Vector3D betweenPosition;
	

	


// lex: it is the point between firstPosition and secondPosition and parallel with  opponent[1]..
	angle = findPointToPointAngle(firstPosition, secondPosition);

	gradient = findGradient(firstPosition, secondPosition);

	
	if(gradient >=-1 && gradient <= 1)
	{
		if(angle<0)
			angle = -angle;
		limit = desireWidth/ (cos (angle * PI/ 180));
					
			if(	robotPosition.x < secondPosition.x && robotPosition.x > firstPosition.x)
			{
				/*gradient = betweenPosition.y - firstPosition.y
					         ______________________________________
							 betweenPosition.x - firstPosition.x
							 */

				betweenPosition.x = robotPosition.x;
				betweenPosition.y = (gradient * (betweenPosition.x - firstPosition.x)) + firstPosition.y;
				if (robotPosition.y < (betweenPosition.y + limit) && robotPosition.y > (betweenPosition.y - limit))
				{
					inLine = 1;
				}
				else
				{
					inLine = 0;
				}
			}
			else
			{
				inLine = 0;
			}
	}
	else if(gradient > 1)
	{
		if(angle < 0)
			angle = -angle;
		limit = desireWidth/ (sin (angle * PI/ 180));
		if(robotPosition.y < secondPosition.y && robotPosition.y > firstPosition.y)
				{
					betweenPosition.y = robotPosition.y;
					betweenPosition.x = ((betweenPosition.y - firstPosition.y) / gradient) + firstPosition.x;
					if ( robotPosition.x < (betweenPosition.x + limit) && robotPosition.x > (betweenPosition.x - limit))
					{
						inLine = 1;
					}
					else
					{
						inLine = 0;
					}
				}
		else
		{
			inLine = 0;
		}
	}
	else if(gradient < -1)
	{
		if(angle < 0)
			angle = -angle;
		limit = desireWidth/ (sin (angle * PI/ 180));
		if(robotPosition.y < firstPosition.y && robotPosition.y > secondPosition.y)
				{
					betweenPosition.y = robotPosition.y;
					betweenPosition.x = ((betweenPosition.y - firstPosition.y) / gradient) + firstPosition.x;
					if ( robotPosition.x < (betweenPosition.x + limit) && robotPosition.x > (betweenPosition.x - limit))
					{
						inLine = 1;
					}
					else
					{
						inLine = 0;
					}
				}
				else
				{
					inLine = 0;
				}
	}

	return inLine;
}

bool CBasicAction::isOppoDribble(OpponentRobot *Opponent_robot, Ball *ball)//LEX: to detect the opponent drebble the balll or not
{
	bool choice = 0;
	if( Opponent_robot->pos.x - ball->pos.x > 0
		&& Opponent_robot->pos.x - ball->pos.x < 4
		&& Opponent_robot->pos.y - ball->pos.y < 2
		&& ball->pos.y - Opponent_robot->pos.y  < 2)
		choice = 1;
	
	else 
	{
		choice = 0;
	}
	return choice;
		

}

bool CBasicAction::isJam(Environment *env, int whichRobot)//lex: done in 19-6-2003
{

	bool choice = 0, jam = 0;
	int i;
	double limit = 5;
	for(i=0;i<5;i++)
	{
		if(env->opponent[i].pos.x > env->home[whichRobot].pos.x)
		{
			if(env->opponent[i].pos.y > env->home[whichRobot].pos.y)
			{
				if(env->opponent[i].pos.x - env->home[whichRobot].pos.x < limit && env->opponent[i].pos.y - env->home[whichRobot].pos.y < limit)
				{
					choice = 1;break;
				}
				else
				{
					choice = 0;
				}
			}
			else
			{
				if(env->opponent[i].pos.x - env->home[whichRobot].pos.x < limit && env->home[whichRobot].pos.y - env->opponent[i].pos.y < limit)
					{
						choice = 1;break;
					}
					else
					{
						choice = 0;
					}
			}
		}
		else
		{
			if(env->opponent[i].pos.y > env->home[whichRobot].pos.y)
			{
				if(env->home[whichRobot].pos.x - env->opponent[i].pos.x < limit && env->opponent[i].pos.y - env->home[whichRobot].pos.y < limit)
				{
					choice = 1;break;
				}
				else
				{
					choice = 0;
				}
			}
			else
			{
				if(env->home[whichRobot].pos.x - env->opponent[i].pos.x < limit && env->home[whichRobot].pos.y - env->opponent[i].pos.y < limit)
				{
					choice = 1;break;
				}
				else
				{
					choice = 0;
				}
			}
		}
	}
	static int sp_count;// lex: fake timer
	if(choice == 1)
	{
		sp_count++;
	}
	else if(choice == 0)
	{
		sp_count = 0;
	}

	if(sp_count>100)// lex: this set the time..... adjust it to desire value
	{
		jam = 1;
	}
	else
	{
		jam = 0;
	}

	return jam;

}

bool CBasicAction::isPathClear(Environment *env, Vector3D firstPosition, Vector3D secondPosition)
{

	double PI = 3.1415923;
	int i;
	bool pathClear;



	for(i=1;i<5;i++)
	{
		if(isInBetweenPos(env, firstPosition, secondPosition, env->opponent[i].pos, 5) == 0)
		{
			pathClear = 1;
		

		}

		else
		{
			pathClear = 0;
			break;
		}

		if(isInBetweenPos(env, firstPosition, secondPosition, env->home[i].pos, 5) == 0)
		{
			pathClear = 1;
		
		}


		else
		{
			pathClear = 0;
			break;
		}
	}

	return pathClear;
}
	

void CBasicAction::orbit(Vector3D orbitPoint,Robot *robot,int whichRobot,int distanceError,int direction)
{
	double dx,dy,currentAngle,wantedAngle;
	Vector3D offset;
	
	dx = env->home[whichRobot].pos.x - env->predictedBall.pos.x;
	dy = env->home[whichRobot].pos.y - env->predictedBall.pos.y;

	currentAngle = 180/3.142 * atan2((double)(dy), (double)(dx));
	if(currentAngle < 0)
	{
		currentAngle +=360;
	}

	if(direction)//CLOCKWISE
	{
		wantedAngle = currentAngle - 75;
		if(wantedAngle < 0)
		{
			wantedAngle +=360;
		}
	}
	else
	{
		wantedAngle = currentAngle + 75;
		if(wantedAngle > 360)
		{
			wantedAngle -= 360;
		}
	}

	offset.x = env->predictedBall.pos.x + cos(wantedAngle*PI/180)*distanceError;
	offset.y = env->predictedBall.pos.y + sin(wantedAngle*PI/180)*distanceError;

	sineTurn(offset,/* 127, 1, 0, 1,*/ whichRobot);

	return;
}

void CBasicAction::position(Vector3D destination, int whichRobot)
{
	double dx, dy, desiredAngle, angleError;
	double PI = 3.1415923;
	double MAX_ANGLE_ERROR = 90;
	double distanceError;
	double ANGULAR_GAP = 3;
	double robotFront;
	double direction;

	//Added Today
	double averageVelocity;
	double differentialVelocity;
	double angularConstant;
	double inde;
	double temp;

	dx = destination.x - env->home[whichRobot].pos.x;
	dy = destination.y - env->home[whichRobot].pos.y;

	distanceError = (double)(sqrt(dx*dx + dy*dy));

	desiredAngle = 180/3.142 * atan2((double)(dy), (double)(dx));

	angleError = (double)(desiredAngle - (double)env->home[whichRobot].rotation);

	angleError = angleNormalisation(angleError,1);

	direction = findTurningDirection(angleError);

	robotFront = decideRobotFront(angleError);

	angleError = (float) fabs(angleError);

	if (angleError > 90)
	{
		angleError -= 180;
		angleError = (float)fabs(angleError);
	}

	angleError = angleNormalisation(angleError,1);

	//Under Testing
	inde = log(distanceError)/log(2.718); //-- seattle profile

	angularConstant = 0.4 - 0.0435*inde;

	if (angularConstant < 0.2)
	{
		angularConstant = 0.2;
	}

	if(angularConstant > 0.3)
	{
		angularConstant = 0.3;
	}

	differentialVelocity = angularConstant * angleError;

	temp = 30*inde - 15;  // -31.074	//18,-40

	if(distanceError<2)
	{
		temp=0;
	}

	if(temp>120.0)
	{
		temp=120.0;
	}

	if(temp < -0.0)
	{
		temp=0.0;
	}

	temp = temp*fabs(cos(angleError*PI/90));//cos(half theta)
	averageVelocity = temp * 2;

	 env->home[whichRobot].velocityLeft = robotFront*averageVelocity + direction*differentialVelocity;
	 env->home[whichRobot].velocityRight = robotFront*averageVelocity - direction*differentialVelocity;
	
	if(distanceError < 1.5)
	{
		env->home[whichRobot].velocityLeft = 0;
		env->home[whichRobot].velocityRight = 0;
	}