support.cpp

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

// Support.cpp: implementation of the CSupport class.
//
//////////////////////////////////////////////////////////////////////

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

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

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

}

CSupport::~CSupport()
{

}

void CSupport::blockInFrontOfGoal(int WhichRobot1, int WhichRobot2)
{

	double gradient, centrePositionY;
	Vector3D HOME_GOAL_CENTER_POSITION, GuardPosition;
	double angle;
	HOME_GOAL_CENTER_POSITION.x = FLEFTX;
	HOME_GOAL_CENTER_POSITION.y = CENTERY;
	GuardPosition.x = 32.1111;
	angle = findPointToPointAngle(env->currentBall.pos, HOME_GOAL_CENTER_POSITION);
	angle = -angle;
//	angle = 90 - Angle;
//	angle = Angle_Normalisation(Angle, 2 );
	positionRobotToField(HOME_GOAL_CENTER_POSITION, WhichRobot1, 26, angle + 1.1);
	positionRobotToField(HOME_GOAL_CENTER_POSITION, WhichRobot2, 26, angle - 1.1);
/*	gradient = (env->currentBall.pos.y - HOME_GOAL_CENTER_POSITION.y) / (env->currentBall.pos.x - HOME_GOAL_CENTER_POSITION.x);
	centrePositionY = (gradient * (GuardPosition.x - HOME_GOAL_CENTER_POSITION.x)) + HOME_GOAL_CENTER_POSITION.y;

	GuardPosition.y = centrePositionY + 1.1;
	SineTurn(GuardPosition,20,1,-90,1,WhichRobot1);
	GuardPosition.y = centrePositionY - 1.1;
	SineTurn(GuardPosition,20,1,90,1,WhichRobot2);*/
	return;
}