sweeper.cpp

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

// Sweeper.cpp: implementation of the CSweeper class.
//
//////////////////////////////////////////////////////////////////////

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

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

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

CSweeper::~CSweeper()
{


}

void CSweeper::sweeper(int whichRobot)
{
	if(env->currentBall.pos.x > 13.5 && env->currentBall.pos.x < 22 
		&& env->currentBall.pos.y > 26 && env->currentBall.pos.x < 57)
	{
		blockBall(whichRobot);
	}
	else
	{
		coverGoalArea(whichRobot);
	}
	
	return;
}

void CSweeper::blockBall(int whichRobot)
{
	

	double robotFront;
	double desiredAngle;
	double robotAngle = env->home[whichRobot].rotation;
	double LIMIT_X_HIGH =10.8;
	double LIMIT_X_LOW = 8.6;
	double LIMIT_Y_HIGH = 50;
	double LIMIT_Y_LOW = 31;
	double Vl, Vr;
	Vector3D predict = env->predictedBall.pos;
	Vector3D robotPosition = env->home[whichRobot].pos;
	
	
	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;
	}

	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 ((robotAngle > 89 && robotAngle < 91)
		|| (robotAngle > 269 && robotAngle < 271))
	{

		env->home[whichRobot].velocityLeft = Vl;
		env->home[whichRobot].velocityRight = Vr;
	}
	else
	{
		TurnAngle(whichRobot, desiredAngle);
	}
*/	
	env->home[whichRobot].velocityLeft = Vl;
	env->home[whichRobot].velocityRight = Vr;
	return;
}

void CSweeper::coverGoalArea(int whichRobot)
{

	
	double dy;
	Vector3D pos;
	if (env->home[whichRobot].pos.x > 15.5 && env->home[whichRobot].pos.x < 16.5)
		pos.x = env->home[whichRobot].pos.x;
	else
		pos.x = 16;		
	
	if (env->home[env->userData->goalie].pos.y > CENTERY)
	{
		dy = (env->home[env->userData->goalie].pos.y - GBOTY)/2 + ROBOT_RADIUS/2;
		pos.y = env->home[env->userData->goalie].pos.y - dy;
	}
	else
	{
		dy = (GTOPY - env->home[env->userData->goalie].pos.y)/2 + ROBOT_RADIUS/2;
		pos.y = env->home[env->userData->goalie].pos.y + dy;
	}

	positionMyro(pos, whichRobot);

	return;
}