basicaction.cpp

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

	return;
}

void CBasicAction::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
	//ANTI_CLOCKWISE 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 CBasicAction::positionMyro(Vector3D destination,/* double finalSpeed, BOOL speedFlag, double finalAngle, BOOL angleFlag,*/ 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.0f;
	double AVERAGE_VELOCITY_FACTOR = 0.17f;
	double DIFFERENTIAL_VELOCITY_FACTOR = 1.25f;
	double temp;
	double direction;
	double robotFront;
	double robotvel;
	double BRAKE_DISTANCE=9.0;
	static double sp_previousX; 
	static double sp_previousY;
	static int sp_actionControl=0;
	// 21012003
	//destination.x = 50;
	//destination.y= 40;

	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>8)//max velocity if distance >8
		temp=PI/4;
	else temp = (distanceError/8)*(PI/4);

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

	//cosine rule for steering
	averageVelocity = 125*(1+sin(temp))*cos(angleError*PI/180);
	if (averageVelocity > 125) {averageVelocity=125;}

	differentialVelocity = 0.5*angleError*sin(angleError*PI/180);
	
//compute robot vel
	dx = env->home[whichRobot].pos.x-sp_previousX;
	dy = env->home[whichRobot].pos.y-sp_previousY;

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

	switch(sp_actionControl)
	{
	case 0: robotvel=0;
			sp_actionControl=1;
	case 1: 
		if ((distanceError<BRAKE_DISTANCE)&&(robotvel>1.0))//braking
			{sp_actionControl=2;}
		else
			{sp_actionControl=1;}
		break;

	case 2: if (robotvel<0.4)//braked send zero velocity
			{sp_actionControl=3;}
			else
			{sp_actionControl=2;}
		break;
	case 3: {sp_actionControl=0;}
		break;
	default:{sp_actionControl=0;}
	
	}

	if(distanceError>8)
			{sp_actionControl=1;}

	switch(sp_actionControl)
	{
	case 0:
	case 1: averageVelocity = 125*(1+sin(temp))*cos(angleError*PI/180);
			if (averageVelocity > 125) {averageVelocity=125;}
			break;
	case 2: temp = (distanceError/BRAKE_DISTANCE)*(PI/2);
			averageVelocity = -125*sin(temp);
			if 	(averageVelocity > -60)
					averageVelocity = -60;
			break;
	case 3: averageVelocity = 0;
			break;
	default:averageVelocity = 0;
			break;
	
	}
	
	env->home[whichRobot].velocityLeft = averageVelocity*robotFront+direction*differentialVelocity;
	env->home[whichRobot].velocityRight = averageVelocity*robotFront-direction*differentialVelocity;

	//	if(angleFlag && (distanceError <= 5.0))
//	{
//		turnAngle(whichRobot, finalAngle);
//	}

//	avoidBoundary(whichRobot);
	sp_previousX=env->home[whichRobot].pos.x;
	sp_previousY=env->home[whichRobot].pos.y;


	return; 
}


void CBasicAction::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;
}

double CBasicAction::robotToBallAngle(Robot *robot)
{
	double Rx, Ry, Bx, By, Tx, Ty, angle;
	double PI = 3.1415923;

	Bx = env->currentBall.pos.x;
	By = env->currentBall.pos.y;

	Rx = robot->pos.x;
	Ry = robot->pos.y;

	Tx = Bx - Rx;
	Ty = By - Ry;
	
	angle = 180.0 / PI * atan2((double)(Ty), (double)(Tx));

	return angle;
}


void CBasicAction::shootStraight(Robot *robot)
{
	double Vl, Vr;

	Vl = 127;
	Vr = 127;

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


void CBasicAction::sineTurn(Vector3D destination,/* double finalSpeed, BOOL speedFlag, double finalAngle, BOOL angleFlag,*/ 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;
	
	int sp_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=0.5*(env->home[3].velocityLeft+env->home[3].velocityRight)/2;
	averageVelocity = 126*(1+sin(temp))*cos(angleError*PI/180);

	if ((distanceError<5)&&(robotvel>1.0))
		averageVelocity=-100;
	if (distanceError<2)
		averageVelocity=0;

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

	differentialVelocity = 0.6*angleError*sin(angleError*PI/180);
	env->home[whichRobot].velocityLeft = averageVelocity*robotFront+direction*differentialVelocity;
	env->home[whichRobot].velocityRight = averageVelocity*robotFront-direction*differentialVelocity;
	
	return; 
}

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 sp_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 sp_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
		sp_momentumCounter++;
	else
		sp_momentumCounter=0;

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

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


	return sp_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::turnAngle(int whichRobot, double desiredAngle)
{
	double robotAngle;
	double angleError;
	double averageVelocity;
	double differentialVelocity;
	double AVERAGE_VELOCITY_FACTOR = 0.12;
	double DIFFERENTIAL_VELOCITY_FACTOR = 0.5;
	double DELTA_ERROR = 1.0;

	robotAngle = env->home[whichRobot].rotation;
	angleError = desiredAngle - robotAngle;
	angleError = angleNormalisation(angleError,1);                                  

	// swith head
	if(angleError < -90)   	
	{
		angleError += 180; 
	}
	else if(angleError > 90)  	
	{
		angleError -= 180;	
	}

	averageVelocity = AVERAGE_VELOCITY_FACTOR;
	differentialVelocity = DIFFERENTIAL_VELOCITY_FACTOR;

	env->home[whichRobot].velocityLeft = ((-averageVelocity * angleError) - (differentialVelocity * DELTA_ERROR)) * 2.0;
	env->home[whichRobot].velocityRight = ((+averageVelocity * angleError) + (differentialVelocity * DELTA_ERROR)) * 2.0;
	return;
}