strategy.cpp

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

	{
		Ball.y = Limit_Y_High;
	}

	if(Ball.y < Limit_Y_Low)
	{
		Ball.y = Limit_Y_Low;
	}
	if(Robot_Position.x >= Limit_X_Low && Robot_Position.x < Limit_X_High  )
	{
		

		if ( Robot_Position.y > Ball.y + 0.9  && Robot_Position.y >= Limit_Y_Low )
		{
			Vl = -100 * RobotFront;
			Vr = -100 * RobotFront;
		}

		if ( Robot_Position.y < Ball.y - 0.9 && Robot_Position.y <= Limit_Y_High )
		{
			Vl = 100 * RobotFront;
			Vr = 100 * RobotFront;
		}

		if ( Robot_Position.y <= Limit_Y_Low)
		{
			Vl = 100 * RobotFront;
			Vr = 100 * RobotFront;	
		}

		if ( Robot_Position.y >= Limit_Y_High )
		{
			Vl = -100 * RobotFront;
			Vr = -100 * RobotFront;
			
		}

	//	Vl = 100 * RobotFront;
	//	Vr = 100 * RobotFront;

		if(AngleError > 0)
		{
			if(Direction > 0)
			{
				Vl = AngleError;
				Vr = -AngleError;
			}
			if(Direction < 0)
			{
				Vl = -AngleError;
				Vr = AngleError;
			}
		}

	
	
		env->home[0].velocityLeft = Vl;//velocityLeft;
		env->home[0].velocityRight = Vr;//velocityRight;
	

	}
	else
	{
		Vector3D Point_To_Go;
		Point_To_Go.x = 9.7316;
		Point_To_Go.y = Ball.y;
		CBasicAction BA(env, UserData);
		BA.SineTurn(Point_To_Go, 125, 1, 90, 1, 0);
	}

	

	return;
}


void CTest::TestGoalKick(int Which_Robot)  //NJS
{
	int count = 0;
	int state = 0;
	double DistanceError;
	int num = rand()%2;
	CBasicAction BA(env, UserData);
	DistanceError = BA.Robot_To_Ball_Distance(Which_Robot);

	if (DistanceError >4.1 && state==0)
	{
		CBasicAction BA(env, UserData);
		//BA.Position(env->currentBall.pos, Which_Robot);
		BA.SineTurn(env->currentBall.pos,0,0,0,0, Which_Robot);
	}
	else
	{
		count++;
		state=1;

		if(num)
		{
			
			env->home[Which_Robot].velocityLeft = 125;
			env->home[Which_Robot].velocityRight = -125;
		}
		else
		{
			env->home[Which_Robot].velocityLeft = -125;
			env->home[Which_Robot].velocityRight = 125;
		}
		if(count<500)
		{
			state=1;
		}
		else
		{
			count=0;
			state=0;
		}
		
	}
	return;
	

	
}
		
	

Vector3D CTest::TestPoint() //ACS
{
	CBasicAction BA(env,UserData);
	Vector3D Point;
	double y,m,c;
	double DistanceX;
	//float x1,x2,y1,y2;

	//scanf("%f %f %f %f", &x1,&y1,&x2,&y2);

	DistanceX=BA.CalculateDistance(env->currentBall.pos, env->lastBall.pos);
	
	if(DistanceX > 0.8)
	{

		m= (env->currentBall.pos.y - env->lastBall.pos.y) / (env->currentBall.pos.x - env->lastBall.pos.x);
		c= env->currentBall.pos.y - (m * env->currentBall.pos.x );
		//printf("%.2f\n", c);
		//scanf("%f", &x);
		y=(m*9.8) + c; // x=86 FRIGHTX- FLEFTX
		Point.y= y; // value of y to be passed out of the function, Point.y= any value
		Point.x= 9.8; //value of x to be passed out of the function
		//printf("%.2f\n", y);
	}

	else
	{
		Point.y=env->currentBall.pos.y ; // value of y to be passed out of the function, Point.y= any value
		Point.x= 9.8;
	}

	return Point;
	
}

void CTest::TestFreeKick(int Which_Robot) //ACS
{
	double DistanceError;
	int num = rand()%2;
	CBasicAction BA(env, UserData);
	DistanceError = BA.Robot_To_Ball_Distance(Which_Robot);

	if (DistanceError >4.1)
	{
		CBasicAction BA(env, UserData);
		//BA.Position(env->currentBall.pos, Which_Robot);
		BA.SineTurn(env->currentBall.pos,0,0,0,0, Which_Robot);
	}
	else
	{	
		
		if(num)
		{
			env->home[Which_Robot].velocityLeft = 125;
			env->home[Which_Robot].velocityRight = -125;
		}
		else
		{
			env->home[Which_Robot].velocityLeft = -125;
			env->home[Which_Robot].velocityRight = 125;
		}
	
	}
	return;
	

	
}

void CTest::TestFreeBall(int Which_Robot)//ACS
{
	double DistanceError;
	int num = rand()%2;
	CBasicAction BA(env, UserData);
	DistanceError = BA.Robot_To_Ball_Distance(Which_Robot);

	if (DistanceError >4.1)
	{
		CBasicAction BA(env, UserData);
		//BA.Position(env->currentBall.pos, Which_Robot);
		BA.SineTurn(env->currentBall.pos,0,0,0,0, Which_Robot);
	}
	else
	{	
		
		if(num)
		{
			env->home[Which_Robot].velocityLeft = 125;
			env->home[Which_Robot].velocityRight = -125;
		}
		else
		{
			env->home[Which_Robot].velocityLeft = -125;
			env->home[Which_Robot].velocityRight = 125;
		}
	
	}
	return;
	

	
}


void CTest::TestGoalie(int Which_Robot) //ACS
{	
	// declare variables

	//double UpperLimit=GTOPY;
	//double LowerLimit=GBOTY;
	Vector3D Point; // Name of Vector3D must be same as XXX=Testpoint
	Vector3D GoaliePos;
	//Vector3D Predict;
	
	GoaliePos = env->home[Which_Robot].pos; //current position of robot

	Point=TestPoint(); // call the test point function in test.h

	if( Point.y > GTOPY)
	{
		Point.y= GTOPY;
	
	}
	/*else
	{
		env->home[Which_Robot].velocityLeft=0;
		env->home[Which_Robot].velocityRight=0;
	}

*/
	
/*	if ( Point.y < GBOTY)
	{
		Point.y= GBOTY;
	}

/*	else
	{
		env->home[Which_Robot].velocityLeft=0;
		env->home[Which_Robot].velocityRight=0;
	}*/
			
/*	PositionFunction(Point,0);
//	CBasicAction BA(env,UserData);
//	BA.Position(Point,0);
/*	return;
}


void CTest::TestPenalty(int Which_Robot)// acs
{
	double DistanceError;
	int num = rand()%2;
	CBasicAction BA(env, UserData);
	DistanceError = BA.Robot_To_Ball_Distance(Which_Robot);

	if (DistanceError >4.1)
	{
		CBasicAction BA(env, UserData);
		BA.Position(env->currentBall.pos, Which_Robot);
		BA.SineTurn(env->currentBall.pos,0,0,0,0, Which_Robot);
	}
	else
	{	
		
		if(num)
		{
			env->home[Which_Robot].velocityLeft = 125;
			env->home[Which_Robot].velocityRight = -125;
		}
		else
		{
			env->home[Which_Robot].velocityLeft = -125;
			env->home[Which_Robot].velocityRight = 125;
		}
	
	}
	return;
	

	
}



void CTest::Penalty_Kick(int WhichRobot)
{
	CBasicAction BA(env, UserData);
	CChaseAction CA(env, UserData);

	double RobotFront = BA.Decide_Robot_Front(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 )
	{
		BA.Spin(WhichRobot,127,CLOCKWISE);
	}
	else
	{
		if(Angle<230.0&& Angle>130.0)
		{
			//env->home[WhichRobot].velocityLeft = RobotFront*125;
			//env->home[WhichRobot].velocityRight = RobotFront*125;
			
			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 CTest::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 = env->home[WhichRobot].pos.x;
	Robot_Position.y = env->home[WhichRobot].pos.y;

	double Robot_Angle = env->home[WhichRobot].rotation;

	env->home[WhichRobot].velocityLeft = 0;
	env->home[WhichRobot].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 && env->home[WhichRobot].pos.y > 36 )
		{
			Vl = -100 ;
			Vr = -100 ;
		}

		if ( Robot_Position.y < env->currentBall.pos.y - 1.5 && env->home[WhichRobot].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, WhichRobot);
		}

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

		

		double T_Robot_Angle = env->home[WhichRobot].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 );
			
		env->home[WhichRobot].velocityLeft = Vl;//velocityLeft;
		env->home[WhichRobot].velocityRight = Vr;//velocityRight;
	
	}
	

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

	}
	
*//*
	return;
	

}
*/