chaseaction.cpp

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

// ChaseAction.cpp: implementation of the CChaseAction class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "ChaseAction.h"
#include "Strategy.h"
#include "math.h"

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////
CChaseAction::CChaseAction(Environment *envPointer, UserDefStruct *UserDataPointer)  : CBasicAction(envPointer, UserDataPointer)
{
	UserData = UserDataPointer;
	env = envPointer;
}

CChaseAction::~CChaseAction()
{

}

void CChaseAction::ChaseBallBlindly(Vector3D Destination,Robot *robot,int WhichRobot)//obsolete
{
	/*if(env->currentBall.pos.x > (robot->pos.x+4))//robot behind the ball
	{
		
		if(env->currentBall.pos.y > (robot->pos.y+4))//if robot below ball
		{
			//Go behind ball (Move up) and hit towards opponent goal
			Position(env->currentBall.pos,WhichRobot);//edit
		}
		else
		{
			if(env->currentBall.pos.y < (robot->pos.y-4))//if robot above ball
			{
				//Go behind ball (Move down) and hit towards opponent goal
				Position(env->currentBall.pos,WhichRobot);//edit
			}
			else
			{
				if((env->currentBall.pos.y >= (robot->pos.y-4)) && (env->currentBall.pos.y <= (robot->pos.y+4)))//if in robot radius range
				{
					//Hit ball towards opponent goal
					Position(env->currentBall.pos,WhichRobot); //ignore this for now
				}
			}
		}
	}
	else//robot in front of ball ( ball towards homegoal )
	{	
		//Go around ball (Move to offset position behind ball)
		Go_Behind_Pos(env->currentBall.pos,robot,WhichRobot,ROBOTRADIUS,ROBOTRADIUS);
		
	}*/
	return;
}

void CChaseAction::PositionRobottoBall(Vector3D Destination,Robot *robot,int WhichRobot,int distanceError,int PositionAngle)
{
	//0 degrees for PositionAngle is in front of where the ball is going.
	//180 degrees is behind where the ball is going
	//anticlockwise orientation
	double dx,dy,DesiredAngle;
	
	dx = env->predictedBall.pos.x - env->currentBall.pos.x;
	dy = env->predictedBall.pos.y - env->currentBall.pos.y;

	DesiredAngle = 180/3.142 * atan2((double)(dy), (double)(dx));
	if(DesiredAngle < 0)
	{
		DesiredAngle +=360;
	}

	Vector3D offset;
	int Angle = PositionAngle+DesiredAngle;

	if(Angle >= 360)
	{
		Angle -= 360;
	}

	offset.x = env->currentBall.pos.x + cos(Angle*PI/180)*distanceError;
	offset.y = env->currentBall.pos.y + sin(Angle*PI/180)*distanceError;

	if(robot->pos.x >= (offset.x + 7) && robot->pos.x <= (offset.x - 7) && robot->pos.y >= (offset.y + 7) && robot->pos.y <= (offset.y - 7) )
	{
		robot->velocityLeft = 0;
		robot->velocityRight = 0;
	}
	else
	{
		SineTurn(offset, 127, 1, 0, 1, WhichRobot);
	}
	return;
}

void CChaseAction::PositionRobottoField(Vector3D Destination,int WhichRobot,int distanceError,double PositionAngle)
{

	Vector3D offset;
	int Angle = PositionAngle;

	offset.x = Destination.x + cos(Angle*PI/180)*distanceError;
	offset.y = Destination.y + sin(Angle*PI/180)*distanceError;

	if(env->home[WhichRobot].pos.x >= (offset.x + 7) && env->home[WhichRobot].pos.x <= (offset.x - 7) && env->home[WhichRobot].pos.y >= (offset.y + 7) && env->home[WhichRobot].pos.y <= (offset.y - 7) )
	{
		env->home[WhichRobot].velocityLeft = 0;
		env->home[WhichRobot].velocityRight = 0;
	}
	else
	{
		SineTurn(offset, 127, 1, 0, 1, WhichRobot);
	}
	return;
}


void CChaseAction::Herd_Ball(Vector3D Destination,Robot *robot,int WhichRobot,int dist)
{
/*	double dx,dy,dx2,dy2,DesiredAngle,DistanceError;
	Vector3D RPos = robot->pos;
	Vector3D BPos = env->predictedBall.pos;
	Vector3D OBPos = env->currentBall.pos;
	dx = BPos.x - RPos.x;
	dy = BPos.y - BPos.y;

	
	dx2 = env->predictedBall.pos.x - env->currentBall.pos.x;
	dy2 = env->predictedBall.pos.y - env->currentBall.pos.y;
	//not used yet
	DesiredAngle = 180/3.142 * atan2((double)(dy), (double)(dx));
	if(DesiredAngle < 0)
	{
		DesiredAngle +=360;
	}
	//YELLOW


	switch(fabs(OBPos.y - BPos.y)<ROBOTRADIUS/4)
	{
		case 1:	switch(fabs(OBPos.x - BPos.x)<ROBOTRADIUS/4)
				{
					//not much change for ball position on both axis
					case 1:	if(BPos.x > 18)// Ball away from home goal 
							{
								if(BPos.x > RPos.x)//Robot closer to home goal than ball
								{
									switch((fabs(RPos.y - BPos.y)<ROBOTRADIUS))// if ball is within the robots length on the y axis
									{
										//
										case 1:		//Chase Ball
													SineTurn(BPos,127,1,0,1,WhichRobot);
													
													break;

										case 0:
													//Go Behind Pos
													PositionRobottoField(BPos,robot,WhichRobot,ROBOTRADIUS,LEFT);

													break;

									}
								}
								if(BPos.x < RPos.x)//Robot closer to oppo goal than ball
								{
									switch((fabs(RPos.y - BPos.y)<ROBOTRADIUS))// if ball is within the robots length on the y axis
									{
										//
										case 1:		if(BPos.y <= 45)// if in lower half of field
													{
														//Go Above Pos
														PositionRobottoField(BPos,robot,WhichRobot,ROBOTRADIUS,ABOVE);
													}
													else // if in higher half of field
													{
														//Go Below Pos
														PositionRobottoField(BPos,robot,WhichRobot,ROBOTRADIUS,BELOW);
													};
													
													break;

										case 0:
													if(RPos.y > (BPos.y+ROBOTRADIUS/2))//robot above ball
													{
														//Go AboveNBehind Pos
														PositionRobottoField(BPos,robot,WhichRobot,ROBOTRADIUS,ABOVELEFT);
													}
													if(RPos.y <  (BPos.y-ROBOTRADIUS/2))//robot below ball
													{
														//Go BelowNBehind Pos
														PositionRobottoField(BPos,robot,WhichRobot,ROBOTRADIUS,BELOWLEFT);
													}

													break;
									}			



								}
							}
							else
							{
								switch((BPos.y < GTOPY && BPos.y > GBOTY))//ball within home goal boundary
								{
									case 1:		Brake(WhichRobot);//need new function
												break;
									case 0:
												if(BPos.y > GTOPY)//if ball position is above goal boundary
												{
													if(BPos.x > RPos.x)//Robot closer to home goal than ball
													{
														PositionRobottoField(BPos,robot,WhichRobot,ROBOTRADIUS/4,BELOW);
														
														if((fabs(RPos.y - BPos.y)<ROBOTRADIUS/4))//Add ability for Robot to clear ball here
														{
															SineTurn(BPos,127,1,0,1,WhichRobot);
														}

													}
													else
													{
													if(BPos.x < RPos.x)//Robot closer to opponent goal than ball
													{
														PositionRobottoField(BPos,robot,WhichRobot,ROBOTRADIUS/2,BELOWLEFT);
													}
													}
												}
												else
												{
												if(BPos.y < GBOTY)
												{
													if(BPos.x > RPos.x)//Robot closer to home goal than ball
													{
														PositionRobottoField(BPos,robot,WhichRobot,ROBOTRADIUS/4,ABOVE);
														if((fabs(RPos.y - BPos.y)<ROBOTRADIUS/4))//Add ability for Robot to clear ball here
														{
															SineTurn(BPos,127,1,0,1,WhichRobot);
														}
													}
													else
													{
													if(BPos.x < RPos.x)//Robot closer to opponent goal than ball
													{
														PositionRobottoField(BPos,robot,WhichRobot,ROBOTRADIUS/2,ABOVELEFT);
													}
													}
												}
												}
												break;
								}
							}
break;
						
					//notable change on x axis only
					case 0:	if((BPos.x - OBPos.x)<0)
							{


							}
							break;
				}
				break;

		case 0:	switch(fabs(OBPos.x - BPos.x)<ROBOTRADIUS/4)
				{
					//notable change on y axis only 
					case 1:	break;
					
					//notable change on both axis
					case 0:	break;
				}
				break;
	}

	

/*

*/
	return;
}

void CChaseAction::Orbit(Vector3D OrbitPoint,Robot *robot,int WhichRobot,int distanceError,int direction)
{
	double dx,dy,CurrentAngle,WantedAngle;
	Vector3D offset;
	
	dx = env->home[WhichRobot].pos.x - env->predictedBall.pos.x;
	dy = env->home[WhichRobot].pos.y - env->predictedBall.pos.y;

	CurrentAngle = 180/3.142 * atan2((double)(dy), (double)(dx));
	if(CurrentAngle < 0)
	{
		CurrentAngle +=360;
	}

	if(direction)//CLOCKWISE
	{
		WantedAngle = CurrentAngle - 75;
		if(WantedAngle < 0)
		{
			WantedAngle +=360;
		}
	}
	else
	{
		WantedAngle = CurrentAngle + 75;
		if(WantedAngle > 360)
		{
			WantedAngle -= 360;
		}
	}

	offset.x = env->predictedBall.pos.x + cos(WantedAngle*PI/180)*distanceError;
	offset.y = env->predictedBall.pos.y + sin(WantedAngle*PI/180)*distanceError;

	SineTurn(offset, 127, 1, 0, 1, WhichRobot);

	return;
}

void CChaseAction::ObstacleTest()
{
	double d[4],dx,dy;
	double a[4];
	double ballDistance,ballAngle,closestObstacleDist = 100;

	int i,j=5;

	dx = env->home[4].pos.x - env->predictedBall.pos.x;
	dy = env->home[4].pos.y - env->predictedBall.pos.y;
	ballDistance = sqrt(dx*dx+dy*dy);
	ballAngle = 180/3.142 * atan2((double)(dy), (double)(dx));

	for(i = 0;i<=3;i++)
	{
		dx = env->home[4].pos.x - env->home[i].pos.x;
		dy = env->home[4].pos.y - env->home[i].pos.y;
		d[i] = sqrt(dx*dx+dy*dy);
		a[i] = 180/3.142 * atan2((double)(dy), (double)(dx));
		if(a[i] < 0)
		{
			a[i] +=360;
		}
		if((d[i]*sin(env->home[i].rotation-a[i]))<10 && closestObstacleDist > d[i] && ballDistance > d[i])
		{
			j = i;
			closestObstacleDist = d[i];
		}
	}

	if(j!=5)
	{
		if(ballAngle-a[j]>=0)
		{
			Orbit(env->home[j].pos,&(env->home[4]),4,10,ANTICLOCKWISE);
		}
		else
		{
			Orbit(env->home[j].pos,&(env->home[4]),4,10,CLOCKWISE);
		}
	}
	else
	{
		Spin(&(env->home[4]),125,ANTICLOCKWISE);
//		SineTurn(env->predictedBall.pos, 127, 1, 0, 1, 4);

	}


	return;
}