penaltyaction.cpp

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

// PenaltyAction.cpp: implementation of the PenaltyAction class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "PenaltyAction.h"
#include <math.h>

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

CPenaltyAction::CPenaltyAction(Environment *envPointer)  : CBasicAction(envPointer)
{

}

CPenaltyAction::~CPenaltyAction()//处罚
{

}

void CPenaltyAction::penaltyKick(int whichRobot)
{

	double robotFront = decideRobotFront(env->home[whichRobot].rotation);

	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)

	//if(env->home[WhichRobot].pos.x>100 )
	{
		spin(&env->home[whichRobot],127,CLOCKWISE);
	}
	else
	{
		if(angle<230.0&& angle>130.0)
		{
			//env->home[WhichRobot].velocityLeft = RobotFront*125;
			//env->home[WhichRobot].velocityRight = RobotFront*125;
			
			sineTurn(env->predictedBall.pos, whichRobot);
		}
		else // robot not in position
		{
			orbit(env->predictedBall.pos,&(env->home[whichRobot]),whichRobot,5,CLOCKWISE);
		}
		
	}
	


}