penaltykick.cpp

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

// PenaltyKick.cpp: implementation of the CPenaltyKick class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "PenaltyKick.h"
#include "BasicAction.h"
#include "ChaseAction.h"
#include "math.h"

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

CPenaltyKick::CPenaltyKick(Environment *envPointer, UserDefStruct *UserDataPointer)
{
	UserData = UserDataPointer;
	env = envPointer;
}

CPenaltyKick::~CPenaltyKick()
{

}

void CPenaltyKick::Penalty_Kick_BlueBall()
{
	CBasicAction BA(env, UserData);
	Goalie(0);
	
	double RobotFront;

	RobotFront = BA.Decide_Robot_Front(env->home[1].rotation);

	Goalie(0);
	env->home[1].velocityLeft = -RobotFront*125;
	env->home[1].velocityRight = -RobotFront*125;

	env->home[2].velocityLeft = -RobotFront*125;
	env->home[2].velocityRight = -RobotFront*125;

	env->home[3].velocityLeft = -RobotFront*125;
	env->home[3].velocityRight = -RobotFront*125;

	env->home[4].velocityLeft = -RobotFront*125;
	env->home[4].velocityRight = -RobotFront*125;

	if(env->home[1].pos.x < 35)
	{
		BA.Brake(0);
		BA.Brake(1);
		BA.Brake(2);
		BA.Brake(3);
		BA.Brake(4);


	}

}

void CPenaltyKick::Attacker(int WhichRobot)
{
	CBasicAction BA(env, UserData);
	CChaseAction CA(env, UserData);

	double dx2 = env->home[WhichRobot].pos.x - env->predictedBall.pos.x;
	double dy2 = env->home[WhichRobot].pos.y - env->predictedBall.pos.y;
	
	double CurrentAngle = 180/3.142 * atan2((double)(dy2), (double)(dx2));
	if(CurrentAngle < 0)
	{
		CurrentAngle +=360;
	}

	double Angle = CurrentAngle;


	if((env->currentBall.pos.x - env->home[WhichRobot].pos.x) > 0 //behind ball
		&& (env->currentBall.pos.x - env->home[WhichRobot].pos.x) < 4.0 //distance more then 4
		&& (env->home[WhichRobot].pos.y - env->currentBall.pos.y) < 1.5 // y more then 1.5
		&& (-(env->home[WhichRobot].pos.y - env->currentBall.pos.y)) < 1.5)
	{
		BA.Spin(&(env->home[WhichRobot]),127,CLOCKWISE);
	}
	else
	{
		if(Angle<230.0&& Angle>130.0)
		{
			
			BA.SineTurn(env->predictedBall.pos, 127, 1, 0, 1, WhichRobot);
		}
		else // robot not in position
		{
			CA.Orbit(env->predictedBall.pos,&(env->home[WhichRobot]),WhichRobot,5,CLOCKWISE);
		}
		
	}
	
}


void CPenaltyKick::Goalie (int WhichRobot)
{
	CBasicAction BA(env, UserData);
	CChaseAction CA(env, UserData);

	double RobotFront;

	RobotFront = BA.Decide_Robot_Front(env->home[WhichRobot].rotation);
	
	if (env->home[WhichRobot].pos.x >18)
	{
		env->home[WhichRobot].velocityLeft = -RobotFront*100;
		env->home[WhichRobot].velocityRight = -RobotFront*100;
		
	}
	else
	{
		env->home[WhichRobot].velocityLeft = RobotFront*125;
		env->home[WhichRobot].velocityRight = RobotFront*125;
	}
	/*

	double Vl = 0, Vr = 0;
	
	Vector3D Current_Ball_Position;
	Vector3D Robot_Position;
	Vector3D Point_To_Go;
	
	Current_Ball_Position.x = env->currentBall.pos.x;
	Current_Ball_Position.y = env->currentBall.pos.y;
	
	
	Robot_Position.x = robot->pos.x;
	Robot_Position.y = robot->pos.y;

	double Robot_Angle = robot->rotation;

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


		
	if(Robot_Position.x >= 9.3 && Robot_Position.x < 10.8 && env->currentBall.pos.x < 70 )
	{
		
		if ( Robot_Position.y > env->currentBall.pos.y + 1.5 && robot->pos.y > 36 )
		{
			Vl = -100 ;
			Vr = -100 ;
		}

		if ( Robot_Position.y < env->currentBall.pos.y - 1.5 && robot->pos.y < 48 )
		{
			Vl = 100 ;
			Vr = 100 ;
		}
	

		if ( Robot_Position.y <= 36)
		{
			Point_To_Go.x = 10;
			Point_To_Go.y = 36;
			BA.Position(Point_To_Go, Which_Robot);
		}

		if ( Robot_Position.y >= 48 )
		{
			Point_To_Go.x = 10;
			Point_To_Go.y = 48;
			BA.Position(Point_To_Go, Which_Robot);
		}

		

		double T_Robot_Angle = robot->rotation - 180;
		if ( T_Robot_Angle < 0.001 )
			T_Robot_Angle = T_Robot_Angle + 360;
		if ( T_Robot_Angle > 360.001 )
			T_Robot_Angle = T_Robot_Angle - 360;
		if ( T_Robot_Angle > 270.5 )
			Vl = Vl + fabs ( T_Robot_Angle - 270 );
		else if ( T_Robot_Angle < 269.5 )
			Vr = Vr + fabs ( T_Robot_Angle - 270 );
			
		robot->velocityLeft = Vl;//velocityLeft;
		robot->velocityRight = Vr;//velocityRight;
	
	}
	

	else
	{
		Point_To_Go.x = 10;
		Point_To_Go.y = 42;
		BA.Position(Point_To_Go, Which_Robot);

	}
	return;
	*/

}