computation.cpp

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

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



double angleNormalisation(double angle, double choice)
{
	/*
	choice = 0 --> Normalise -90 To 90 Degree
	choice = 1 --> Normalise -180 To 180 Degree
	*/
	
	if(choice == 1) // --> Normalise -180 To 180 Degree
	{
		while(angle > 180)		
		{
			angle -= 360;
		}
		while(angle < -180)
		{
			angle += 360;
		}
	}
	else // --> Normalise -90 To 90 Degree
	{
		while(angle > 90)
		{
			angle -= 180;
		}
		while(angle < -90)
		{
			angle += 180;
		}
	}

	return angle;
}

double findPointToPointAngle(Vector3D referencePoint, Vector3D targetPoint)
{
	double angle;
	
	double dy, dx;

	dy = targetPoint.y - referencePoint.y;
	dx = referencePoint.x - targetPoint.x;
	angle = ( ( atan2(dy, dx) ) * ( 180/PI ) );
	return angle;

}


double findDistance(Vector3D firstPos, Vector3D secondPos)
{
	//Vector3D tempPos;
	double distX, distY, distance;
	//if(firstPos.x > secondPos.x)
	//{
//		tempPos = firstPos;
//		firstPos = secondPos;
//		secondPos = tempPos;
//	}

	distX = secondPos.x - firstPos.x;
	distY = secondPos.y - firstPos.y;
	distance = sqrt(distX * distX + distY * distY);
	return distance;
}

double findGradient( Vector3D firstPosition, Vector3D secondPosition)
{
	double gradient;
	Vector3D tempPosition;
	if(firstPosition.x > secondPosition.x)
	{
		//lex: just swap firstPosition and secondPosition
		tempPosition = firstPosition;
		firstPosition = secondPosition;
		secondPosition = tempPosition;
	}
	gradient = (secondPosition.y - firstPosition.y)/(secondPosition.x - firstPosition.x);
	return gradient;
}

double findTurningDirection(double angle)
{
	double direction;

	if ((angle < 90)&&(angle >0))	//Turning 1
	{
			direction = -1;
	}
	else if((angle > 90)&&(angle < 180))	//Turning 2
	{
			direction = 1;
	}
	else if((angle >-90)&&(angle <0))	//Turning 3
	{
		direction = 1;
	}
	else if((angle < -90)&&(angle >-180))	//Turning 4
	{
		direction = -1;
	}
	
	return direction;
}

Vector3D findFurtherPos(Environment *env,int whichRobot,Vector3D targetPoint, double furtherHowMuch)
{
	double angle;
	Vector3D pointToGo;
	angle = findPointToPointAngle(env->home[whichRobot].pos,targetPoint);
	pointToGo.x = targetPoint.x + cos(angle*PI/180)*furtherHowMuch;//good formula
	pointToGo.y = targetPoint.y + sin(angle*PI/180)*furtherHowMuch;//lex:change the robot target point to further more by adding 2 to currentball position
	return pointToGo;
}

void fliCoordinatep(Environment *env)
{
//	double tempVelocity;
	env->currentBall.pos.x = OFFSETX-env->currentBall.pos.x;
	env->currentBall.pos.y = OFFSETY-env->currentBall.pos.y;
	env->lastBall.pos.x = OFFSETX-env->lastBall.pos.x;
	env->lastBall.pos.y = OFFSETY-env->lastBall.pos.y;
	env->predictedBall.pos.x = OFFSETX-env->predictedBall.pos.x;
	env->predictedBall.pos.y = OFFSETY-env->predictedBall.pos.y;
	//env->fieldBounds
	//env->goalBounds
	for (int i=0; i<5; i++)
	{
		env->home[i].pos.x = OFFSETX-env->home[i].pos.x;
		env->home[i].pos.y = OFFSETY-env->home[i].pos.y;

		env->opponent[i].pos.x = OFFSETX-env->opponent[i].pos.x;
		env->opponent[i].pos.y = OFFSETY-env->opponent[i].pos.y;
	}

	return;
}

void flipVelocity(Environment *env)
{
	double tempVelocity;
	for (int i=0; i<5; i++)
	{
		tempVelocity = -env->home[i].velocityLeft;
		env->home[i].velocityLeft = -env->home[i].velocityRight;
		env->home[i].velocityRight = tempVelocity;
	}
	return;
}
Vector3D predictedBall(Environment *env, float howFar)
{//Should add static
	static Vector3D sp_lastBall[5];

	//initilise
//	for(int i=0; i<=19; i++)
//	{
//		sp_lastBall[i].x = 0;
//		sp_lastBall[i].y = 0;
//	}

	sp_lastBall[4] = env->currentBall.pos;

	for(int i=0; i<=3; i++)
	{
		sp_lastBall[i] = sp_lastBall[i+1];
		//if (sp_lastBall[i].x == 0) //nonsence
		//	sp_lastBall[0] = env->currentBall.pos;
	}




	Vector3D predictPos;
//	Vector3D tempPos;
	double predictedDistance, predictedMargin,ballVelocityX,ballVelocityY,ballVelocity, angle;
	double predictedX,predictedY;
	ballVelocityX = env->currentBall.pos.x - sp_lastBall[0].x;
	ballVelocityY = env->currentBall.pos.y - sp_lastBall[0].y;

	ballVelocity = (float)sqrt(ballVelocityX * ballVelocityX + ballVelocityY * ballVelocityY);
	predictedDistance = 2.8*ballVelocity * howFar;
	predictedMargin = ballVelocity * 5;

	if (predictedDistance > predictedMargin) {predictedDistance = predictedMargin;}
	
	angle = (float) (atan2(ballVelocityY,ballVelocityX) * 180/PI);

	predictedX = (float)(predictedDistance * cos(angle * PI/180));
	predictedY = (float)(predictedDistance * sin(angle * PI/180));

	predictPos.x =  env->currentBall.pos.x + predictedX;
	predictPos.y =  env->currentBall.pos.y + predictedY;

	if(predictPos.y > FTOP)
	{
		predictPos.y = (2 * FTOP) - predictPos.y;
	}

	else if(predictPos.y < FBOT)
	{
		predictPos.y = (2 * FBOT) - predictPos.y;
	}

	if(predictPos.x < FLEFTX)
	{
		predictPos.x = (2 * FLEFTX) - predictPos.x;
	}

	else if(predictPos.x > FRIGHTX)
	{
		predictPos.x = (2 * FRIGHTX) - predictPos.x;
	}

	return predictPos;
}

void PredictBallOld (Environment *env,Robot *robot)
{
	//define new velocity
	double Distance_Error,Time_Required;
	double Velocity_Error_X,Velocity_Error_Y;
	Vector3D Ball_Position;
	Ball_Position.x = env->currentBall.pos.x;
	Ball_Position.y = env->currentBall.pos.y;
	Vector3D Difference_In_Axis;
	Vector3D Robot_Position;
	Robot_Position.x = robot->pos.x;
	Robot_Position.y = robot->pos.y;
	static double sp_old_pred_x=20.0,sp_old_pred_y=20.0;
	double Updated_Velocity_X,Updated_Velocity_Y;
	double Ball_Velocity,Ball_Velocity_X,Ball_Velocity_Y;
	double Minimum_Ball_Velocity = 0.0;
	double Predicted_X,Predicted_Y;
	double Updated_Velocity,Updated_Angle;
	int Predicted_Flag=0;
	double Predicted_Margin=30.0,Predicted_Distance;


	Ball_Velocity_X = Ball_Position.x - env->lastBall.pos.x;
	Ball_Velocity_Y = Ball_Position.y - env->lastBall.pos.y;
	
	Velocity_Error_X = (Ball_Position.x - env->predictedBall.pos.x)/2;
	Velocity_Error_Y = (Ball_Position.y - env->predictedBall.pos.y)/2;

	Updated_Velocity_X = Ball_Velocity_X + Velocity_Error_X;
	Updated_Velocity_Y = Ball_Velocity_Y + Velocity_Error_Y;

	Updated_Velocity = (float) sqrt(Updated_Velocity_X * Updated_Velocity_X + Updated_Velocity_Y * Updated_Velocity_Y);
	Updated_Angle = (float) (atan2(Ball_Velocity_Y,Ball_Velocity_X) * 180/PI);
	
	Difference_In_Axis.x = Ball_Position.x - Robot_Position.x;
	Difference_In_Axis.y = Ball_Position.y - Robot_Position.y;

	Distance_Error = (float) sqrt(Difference_In_Axis.x * Difference_In_Axis.x + Difference_In_Axis.y * Difference_In_Axis.y);
	Time_Required = (int) (Distance_Error/1.0);
	Ball_Velocity = (float) sqrt(Ball_Velocity_X*Ball_Velocity_X + Ball_Velocity_Y * Ball_Velocity_Y);	
	
	//Prediction of ball 
	if (Ball_Velocity < Minimum_Ball_Velocity)//condition for no prediction
		Predicted_Flag = 0;//reset prediction flag 		
	else //condition to predict
		Predicted_Flag = 1;//set prediction flag
	
//compute predicted distance
	if(Predicted_Flag == 1)
	{
		Predicted_Margin=(Ball_Velocity - Minimum_Ball_Velocity) * 15;
		Predicted_Distance=(float) (fabs (Updated_Velocity * Time_Required));
		if (Predicted_Distance > Predicted_Margin) {Predicted_Distance = Predicted_Margin;}
		
		Predicted_X = (float)(Predicted_Distance * cos(Updated_Angle * PI/180));
		Predicted_Y = (float)(Predicted_Distance * sin(Updated_Angle * PI/180));
		Predicted_X = Ball_Position.x + Predicted_X;
		Predicted_Y = Ball_Position.y + Predicted_Y;

	}
	else
	{
		Predicted_X = Ball_Position.x;
		Predicted_Y = Ball_Position.y;
	}
	
	env->predictedBall.pos.x = Predicted_X;
	env->predictedBall.pos.y = Predicted_Y;
	//assign predicted ball position by robot
	return;
}
float predictHowFar(Environment *env, int whichRobot)
{
	CBasicAction BA(env);
	double distance, angle;
	float howFar;

	distance = findDistance(env->home[whichRobot].pos,env->currentBall.pos);
	angle = BA.robotToBallAngle(&env->home[whichRobot]);
//	if(env->home[whichRobot].rotation >90 )
//	{
//		robotAngleToBall = 180 - env->home[whichRobot].rotation;
//	}
///	if(env->home[whichRobot].rotation < -90)
//	{
//		robotAngleToBall = 180 + env->home[whichRobot].rotation;
///	}
//	else
//	{
///		robotAngleToBall = angle;
//	}

	//howFar = abs(sin(angle))* 0.05 + distance * 0.2;
	howFar = /*abs(sin(angle)) * 0.05 + */log(distance)/log(2.718) *3.2/5;

//	howFar = distance * 0.1;

	return howFar;
}

void transferEnvironmentDataToUserData(Environment *env)
{
	
	int Counter;

	for(Counter=0;Counter<5;Counter++)
	{
		env->userData->HomeRobotData[Counter].Position.x = env->home[Counter].pos.x;
		env->userData->HomeRobotData[Counter].Position.y = env->home[Counter].pos.y;
		env->userData->HomeRobotData[Counter].Position.z = env->home[Counter].pos.z;
		env->userData->HomeRobotData[Counter].Angle.Degree = env->home[Counter].rotation;
		env->userData->HomeRobotData[Counter].LastVelocity.Left = env->home[Counter].velocityLeft;
		env->userData->HomeRobotData[Counter].LastVelocity.Right = env->home[Counter].velocityRight;

//		UserData->OpponentRobotData[Counter].Position.x = env->opponent[Counter].pos.x;
//		UserData->OpponentRobotData[Counter].Position.y = env->opponent[Counter].pos.y;
//		UserData->OpponentRobotData[Counter].Position.z = env->opponent[Counter].pos.z;
//		UserData->OpponentRobotData[Counter].Angle.Degree = env->opponent[Counter].rotation;
	}

	env->userData->BallData.CurrentBall.Position.x = env->currentBall.pos.x;
	env->userData->BallData.CurrentBall.Position.y = env->currentBall.pos.y;
	env->userData->BallData.CurrentBall.Position.z = env->currentBall.pos.z;

	env->userData->BallData.LastBall.Position.x = env->lastBall.pos.x;
	env->userData->BallData.LastBall.Position.y = env->lastBall.pos.y;
	env->userData->BallData.LastBall.Position.z = env->lastBall.pos.z;

	env->userData->BallData.PredictedBall.Position.x = env->predictedBall.pos.x;
	env->userData->BallData.PredictedBall.Position.y = env->predictedBall.pos.y;
	env->userData->BallData.PredictedBall.Position.z = env->predictedBall.pos.z;
}

void zoning(Environment *env)
{
/*  To detect corners and other stuff
	FTOP = 77.2392;
	FBOT = 6.3730;
	GTOPY = 49.6801;
	GBOTY = 33.9320;
	GRIGHT = 97.3632;
	GLEFT = 2.8748;
	FRIGHTX = 93.4259;
	FLEFTX = 6.8118;
*/
	if(env->currentBall.pos.x <= 10)
	{
		env->userData->homeGoal = 1;
		env->userData->oppoGoal = 0;
	}
	else
	{
		if(env->currentBall.pos.x >= 84)
		{
			env->userData->oppoGoal = 1;
			env->userData->homeGoal = 0;
		}
		else
		{
			env->userData->oppoGoal = 0;
			env->userData->homeGoal = 0;
		}
	}

	if(env->currentBall.pos.y <= 13)
	{
		env->userData->lower = 1;
		env->userData->upper = 0;
	}
	else
	{
		if(env->currentBall.pos.y >= 70)
		{
			env->userData->upper = 1;
			env->userData->lower = 0;
		}
		else
		{
			env->userData->upper = 0;
			env->userData->lower = 0;
		}
	}

	if(env->currentBall.pos.y > 30 && env->currentBall.pos.y < 50 && env->currentBall.pos.x > 70)
	{
		env->userData->shootArea = 1;
	}
	else
	{
		env->userData->shootArea = 0;
	}
	return;
}