update1-2-2004.txt

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

The new function called lexsineturn is a modified of sineturn function. We try to improve the accuracy of the robot to its destination. We have done the analysis for the over shooting of the robot in simurosot. After correcting the data, we draw a graph and create a formula by using excel. 

void CBasicAction::lexSineTurn(Vector3D destination, int whichRobot)
{
	double dx, dy, desiredAngle, angleError;
	double PI = 3.1415923;
	double MAX_ANGLE_ERROR = 90;
	double distanceError;
	double ANGULAR_GAP = 3;

	double averageVelocity;
	double differentialVelocity;
//	double velocity;
//	double currentSpeed;
	double MAX_POSITION_SPEED = 120.0;
	double AVERAGE_VELOCITY_FACTOR = 0.17f;
	double DIFFERENTIAL_VELOCITY_FACTOR = 1.25f;
	double temp;
	double direction;
	double robotFront;
	double robotvel;
	double breakVelocity;
	
	int actionControl=0;
	// 21012003
	//destination.x = 50;
	//destination.y= 40;
	destination.x -=2;
	dx = destination.x-env->home[whichRobot].pos.x;
	dy = destination.y-env->home[whichRobot].pos.y;

	distanceError = (double)(sqrt(dx*dx + dy*dy));

	desiredAngle = 180/3.142 * atan2((double)(dy), (double)(dx));

	angleError = (double)(desiredAngle - (double)env->home[whichRobot].rotation);

	angleError = angleNormalisation(angleError, 1);//normalised to 180 pos neg
	
	direction = findTurningDirection(angleError);

	robotFront = decideRobotFront(angleError);
	
	angleError = fabs(angleError);

	angleError = (float)(fabs(angleError));//0-180

	if (angleError > 90)
	{
		angleError -= 180;
		angleError = (float)(fabs(angleError));
	}


	if (distanceError>3)
		temp=PI/4;
	else 
		temp = (distanceError/3)*(PI/4);

	temp = 2*(temp-(PI/4));

	//cosine rule for steering
//	robotvel=(env->home[whichRobot].velocityLeft+env->home[whichRobot].velocityRight)/2;
	averageVelocity = 126*(1+sin(temp))*cos(angleError*PI/180);
//	breakVelocity = -127 * findDistance(env->home[whichRobot].pos,env->userData->HomeRobotData[whichRobot].lastPosition);

//	if (distanceError<4)//&&(robotvel>1.0))
//		averageVelocity = -127;//averageVelocity=-100;
//	if (distanceError<4)
//		averageVelocity=0;

	
	differentialVelocity = 1.2 * 0.6*angleError*sin(angleError*PI/180);

	env->home[whichRobot].velocityLeft = averageVelocity*robotFront+direction*differentialVelocity;
	env->home[whichRobot].velocityRight = averageVelocity*robotFront-direction*differentialVelocity;
	if (distanceError < findRobotMomentum(whichRobot))
		brake(whichRobot);

	return; 
}



double CBasicAction::findRobotMomentum(int whichRobot )
{
	static double momentumCounter=0;
	
	if(fabs(env->home[whichRobot].velocityLeft - env->home[whichRobot].velocityRight) < 17 && (env->home[whichRobot].velocityLeft > 20/*8*/ && env->home[whichRobot].velocityRight >20/*8*/))//calculated using gramatic is 17
		momentumCounter++;
	else
		momentumCounter=0;

//	momentumCounter = momentumCounter / 15 + 1/*1.34*/;//adjustable
	momentumCounter = -0.001* momentumCounter*momentumCounter + 0.1147 * momentumCounter + 0.9005;//use excel to find the formula.....hohoho

//	momentumCounter = -0.0007* momentumCounter*momentumCounter + 0.0881 * momentumCounter + 1.3404;//use excel to find the formula.....hohoho
	momentumCounter = momentumCounter * 42 / 60;
//if can, make momentumCounter between 1.34 to 4.15;


	return momentumCounter;
}
void CBasicAction::spin(Robot *robot, int speed, int direction)
{
/*	direction 
	0 - anticlockwise
	1 - clockwise

    count = delay before stopping
	speed = velocity of spin
*/
	double Vl, Vr;
	
	if(direction == ANTI_CLOCKWISE)
	{
		Vl = 0-speed;
		Vr = speed;
	}
	else
	{
		if(direction == CLOCKWISE)
		{
			Vl = speed;
			Vr = 0-speed;
		}
	}

	robot->velocityLeft = Vl;
	robot->velocityRight = Vr;

	
	return;	
}

void CBasicAction::avoidObstacle(Environment *env, int whichRobot,Vector3D targetPoint)
{	
//	double PI = 3.1415923;
	int i;
	bool pathClear;
	int direction = 1;


	for(i=1;i<5;i++)
	{
		if(isInBetweenPos(env, env->home[whichRobot].pos, targetPoint, env->opponent[i].pos, 4) == 1)
		{
			
			orbit(env->opponent[i].pos,&env->home[whichRobot],whichRobot,9,direction);
			break;

		}

		else if(isInBetweenPos(env, env->home[whichRobot].pos, targetPoint, env->home[i].pos, 4) == 1)
		{
			orbit(env->home[i].pos,&env->home[whichRobot],whichRobot,9,direction);
			break;

		}

	}

	return;
}
command : is use to avoid obstacle between a robot to a target point, still not perfect yet. currently is for supporter use only.

result: after flip the code, will cause hang.no matter VS the lingo or our team.
	attacker perform quite well but sometimes the predicted ball not so accurate, and sometime when the ball near the 	attacker it also cannot chase or shoot the ball.
	defender go round not take any advantage, it will let the ball go to our side easily. so let it block horizontally.
	supporter(midfield) just go to the position which take advantage. but sometime will block the attacker. we have wrote 	a code call avoidObstacle. but can be improve further. 
	 
penalty: the gamestate cannot be change in the code, which mean " env->gameState = 0;" will not work, when it is in the 	freeball mode, we cannot toggle gamestate to 0 because the platform will keep sending the gamestate is freeball.
whichmean we have to write code inside case FREEBALL.


switch (env->gameState)
	{
		case FREE_BALL:
			BA.spin(&env->home[1],125,1);
			//team.normalGame(env);
			break;

		case PLACE_KICK:
			team.normalGame(env);
			break;

		case PENALTY_KICK:
			switch (env->whosBall)
			{
				case ANYONES_BALL:
					team.normalGame(env);
					break;

				case BLUE_BALL:
					
					//t.Goalie(0);
					//BA.Spin(&env->home[0], 125 , 1);
					break;
				case YELLOW_BALL:
					//t.Penalty_Kick(UserData->Attacker);
					//BA.Spin(&env->home[0], 125 , 1);

					break;
			}
            break;
		case FREE_KICK:
			switch (env->whosBall)
			{
				case ANYONES_BALL:
					team.normalGame(env);
					break;

				case BLUE_BALL:
					team.normalGame(env);
					break;

				case YELLOW_BALL:
					team.normalGame(env);             
					break;
			}
			break;

		case GOAL_KICK:
			switch (env->whosBall)
			{
				case ANYONES_BALL:
					team.normalGame(env);
					break;

				case BLUE_BALL:
					team.normalGame(env);
					break;

				case YELLOW_BALL:
					team.normalGame(env);
					break;
			}
			break;

		default:

		BA.spin(&env->home[2],125,1);
			//gameStageCounter = 0;
	//		env->home[0].velocityLeft=125;
	//		env->home[0].velocityRight=-50;
	//		env->home[1].velocityLeft=50;
	//		env->home[1].velocityRight=125; 


		//	team.normalGame(env);
			break;

:We created a spin function for robot 1 under the FREEBALL function. It is used to test whether the robot will return to the default game
function after its spin. The result was that it cannot return to the default game state after the FREEBALL function is run. 
By right it should have returned to the default state.That implies the problem lies with the platform.

26-1-2004
computation::predictBall function
	old line -----howFar = log(distance)/log(2.718) *4.2/5;

change 	new line ------howFar = log(distance)/log(2.718) *3.2/5;------------
reason: 3.2 is more accurate prediction.

void CAttacker::lexMidField(Environment *env, int whichRobot) 
	old line ----------pointToGo.x = env->home[env->userData->attacker].pos.x;
	change new line pointToGo.x = env->home[env->userData->attacker].pos.x - 6;


1-2-2004
void CDefender::blockWhenBallNearGoal(int whichRobot, int spinDirection)

addlines

else if(isInBetweenPos(env,guardPosition,env->home[whichRobot].pos,env->currentBall.pos,5)==1)
	{
		if(env->home[whichRobot].pos.x > env->currentBall.pos.x && env->home[whichRobot].pos.y > env->currentBall.pos.y)
			{
				orbit(env->currentBall.pos,&(env->home[whichRobot]),whichRobot,ORBIT_DISTANCE,ANTI_CLOCKWISE);
			}
		else //if(env->home[whichRobot].pos.x > env->currentBall.pos.x && env->home[whichRobot].pos.y < env->currentBall.pos.y)
			{
				orbit(env->currentBall.pos,&(env->home[whichRobot]),whichRobot,ORBIT_DISTANCE,CLOCKWISE);
			}
	}

reason: to avoid the defender knock the ball back to our goal