goalie.cpp

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

// Goalie.cpp: implementation of the CGoalie class.
//
//////////////////////////////////////////////////////////////////////

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

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

CGoalie::CGoalie(Environment *envPointer) : CBasicAction(envPointer)
{
}

CGoalie::~CGoalie()
{

}

void CGoalie::goalie (Robot *robot, int whichRobot )
{
	double Vl = 0, Vr = 0;
	double desiredAngle;
	double robotFront;
	double direction;
	double angleError;
	double distance;	
	Vector3D ballVelocity;
	Vector3D currentBallPosition;
	Vector3D robotPosition;
	Vector3D pointToGo;
	Vector3D ballToRobotDistance;
	currentBallPosition.x = env->currentBall.pos.x;
	currentBallPosition.y = env->currentBall.pos.y;
	
	
	robotPosition.x = robot->pos.x;
	robotPosition.y = robot->pos.y;

	double robotAngle = robot->rotation;
	double RETURN_POINT = 9.2;
	double predictMultiplier;
	double predictPointToRobotDistance;
	double differenceInPredict;

	robot->velocityLeft = 0;
	robot->velocityRight = 0;
	

	ballVelocity.x = fabs(env->currentBall.pos.x - env->lastBall.pos.x);
	ballVelocity.y = fabs(env->currentBall.pos.y - env->lastBall.pos.y);

	ballToRobotDistance.x = fabs(robotPosition.x - env->currentBall.pos.x);
	ballToRobotDistance.y = fabs(robotPosition.y - env->currentBall.pos.y);

	double LIMIT_X_HIGH =10.8;
	double LIMIT_X_LOW = 8.6;
	double LIMIT_Y_HIGH = 50;
	double LIMIT_Y_LOW = 31;
	Vector3D predict;
	
	predict = env->predictedBall.pos;

	if(ballVelocity.x < 0.3 && ballVelocity.y < 0.3 && env->currentBall.pos.x < 22 && env->currentBall.pos.y < 57 && env->currentBall.pos.y > 26)
	{
		pointToGo.x = env->currentBall.pos.x;
		pointToGo.y = env->currentBall.pos.y;
		sineTurn(pointToGo, /*125, 1, 90, 1,*/ whichRobot);
			
		return;
	}

	if(	ballToRobotDistance.x < 5 && ballToRobotDistance.y < 2 && ballVelocity.x < 0.1 && ballVelocity.y < 0.1)
	{
		Vl = 125;
		Vr = -125;

		robot->velocityLeft = Vl;//velocityLeft;
		robot->velocityRight = Vr;//velocityRight;
		return;
	}

	if(robotAngle > 180.001)
	{
		robotAngle -= 360;
	}
	else
	{
		if (robotAngle < -180.001)
		{
			robotAngle += 360;
		}
	}

	if(robotAngle > 0 && robotAngle < 180)
	{
		desiredAngle = 90;
		robotFront = 1;

	}

	else
	{
		desiredAngle = -90;
		robotFront = -1;
	}
	

	angleError = desiredAngle - robotAngle;

	if(angleError > 0)
	{
		direction = -1; //turn anti-clockwise
	}
	else
	{
		direction = 1;
	}

	angleError = fabs(angleError);

	if(angleError < 3)
	{
		angleError = 0;
	}

	
	if(env->currentBall.pos.x < 13 && robotPosition.x > 9 && robotPosition.x < 13 && env->currentBall.pos.y < 60 && env->currentBall.pos.y > 20)
	{
		pointToGo.x = 9.8;
		pointToGo.y = env->currentBall.pos.y;
		sineTurn(pointToGo, /*125, 1, 90, 1,*/ whichRobot);
			
		return;
		
	}

	distance = fabs(robotPosition.x - env->currentBall.pos.x) ;
	if(distance > 70)
	{
		predict.y = predict.y;
	}
	else
	{
		if(distance > 25)
		{
		
			predictMultiplier = sin(distance/70) ;
			predictPointToRobotDistance =predict.y - robotPosition.y;
			differenceInPredict = predictMultiplier * predictPointToRobotDistance;
			predict.y = robotPosition.y + differenceInPredict;
		}
		else
		{
			predict.y = env->currentBall.pos.y;
		}

	}


	if(robotPosition.x >= LIMIT_X_LOW && robotPosition.x < LIMIT_X_HIGH  )
	{
		if(predict.y > LIMIT_Y_HIGH)
		{
			predict.y = LIMIT_Y_HIGH;
		}

		if(predict.y < LIMIT_Y_LOW)
		{
			predict.y = LIMIT_Y_LOW;
		}

		if ( robotPosition.y > predict.y + 0.9  && robotPosition.y >= LIMIT_Y_LOW )
		{
			Vl = -100 * robotFront;
			Vr = -100 * robotFront;
		}

		if ( robotPosition.y < predict.y - 0.9 && robotPosition.y <= LIMIT_Y_HIGH )
		{
			Vl = 100 * robotFront;
			Vr = 100 * robotFront;
		}

		if ( robotPosition.y <= LIMIT_Y_LOW)
		{
			Vl = 100 * robotFront;
			Vr = 100 * robotFront;	
		}

		if ( robotPosition.y >= LIMIT_Y_HIGH )
		{
			Vl = -100 * robotFront;
			Vr = -100 * robotFront;
			
		}
		if(angleError > 0)
		{
			if(direction > 0)
			{
				Vl = angleError;
				Vr = -angleError;
			}
			if(direction < 0)
			{
				Vl = -angleError;
				Vr = angleError;
			}
		}

	
	
		robot->velocityLeft = Vl;//velocityLeft;
		robot->velocityRight = Vr;//velocityRight;
	

	}
	
	else
	{
		pointToGo.x = RETURN_POINT;
		pointToGo.y = predict.y;
		sineTurn(pointToGo, /*70, 1, 0, 1,*/ whichRobot);

	}
	

	
	return;

}