object.cpp

本程序是2005年参加中国机器人大赛的比赛程序

	else//球在左方则直接追球
	{
		return ball.Get_vPos();
	}
	return Vector(0,0);
}

Vector Player::ChooseDP1(Ball ball) //1号防守队员的选点
{
	double dest[8];
	double value[8];
	double y=ball.Get_vPos().y-( (ball.Get_vPos().x-DEFFENSEX)*(ball.Get_vPos().y-m_MyGoal.y) )/(ball.Get_vPos().x-m_MyGoal.x);
	for(int i=0; i<5; i++)
	{
		if(this->m_Num!=i)
		{
			dest[i]=y+i*LENGTH;
			Vector d=Vector(DEFFENSEX,dest[i]);
			if(d.NearRobot()==TRUE)
			{
				value[i]=0;
			}
			else
			{
				value[i]=1;
			}
		}
		else
		{
			value[i]=1;
			dest[i]=0;
		}
	}
	for(i=0;i<5; i++)
	{
		if( (value[i]!=0) && (dest[i]!=0) )
		{
			return Vector(DEFFENSEX,dest[i]);
		}
	}
	return Vector(DEFFENSEX,MIDDLEPOINTY);
}

Vector Player::ChooseDP2(Ball ball)
//2号防守队员的选点，直接选取球与球门中心的连线，当球与本方球门的距离比较近时，让该队员出击！
{	
	Vector d=ball.Get_vPos()-m_MyGoal;
	int R=130;//防守半径
	double x=m_MyGoal.x+R*cos( d.GetAngle()*(pi/180));
	double y=m_MyGoal.y+R*sin( d.GetAngle()*(pi/180));
	return Vector(x,y);
}

Vector Player::ChooseGoalieP(Ball ball)
{
	if( g_Ball.Get_vPos().x>GOALIEDEFFENSEX)
	{
		if( g_Ball.Get_vPos().x<FORBIDDENRY )
		{
			return Vector(GOALIEDEFFENSEX,FORBIDDENRY);
		}
		if( g_Ball.Get_vPos().x>FORBIDDENLY )
		{
			return Vector(GOALIEDEFFENSEX,FORBIDDENLY);
		}
		return Vector(GOALIEDEFFENSEX,g_Ball.Get_vPos().y);
	}
	else
	{
		if( g_Ball.Get_vPos().x<FORBIDDENRY )
		{
			return Vector(GOALIEDEFFENSEX,FORBIDDENRY);
		}
		else
		{
			if( g_Ball.Get_vPos().x>FORBIDDENLY )
			{
				return Vector(GOALIEDEFFENSEX,FORBIDDENLY);
			}
			else
			{
				return Vector(GOALIEDEFFENSEX,g_Ball.Get_vPos().y);
			}
		}
	}
	
}

Vector Player::ChooseFPSecondHalf(Ball ball)
{//下半场时的，右前锋（右后卫）的点的选择
	double dest[8];
	double value[8];
	if(ball.Get_vPos().y<MIDDLEPOINTY)
	{
		for(int i=0; i<8; i++)
		{
			if(this->m_Num!=i)
			{
				dest[i]=MIDDLEPOINTY+(i+1)*LENGTH;
				Vector d=Vector(ball.Get_vPos().x,dest[i]);
				if(d.NearRobot()==TRUE)
				{
					value[i]=0;
				}
				else
				{
					value[i]=1;
				}
			}
			else
			{
				value[i]=1;
			}
		}
		for(i=0;i<8; i++)
		{
			if(value[i]!=0)
			{
				if(ball.Get_vPos().x>OPPONENTDIFFENSEX)
				{
					return Vector(OPPONENTDIFFENSEX,dest[i]);
				}
				if(ball.Get_vPos().x<DEFFENSEX)
				{	
					return Vector(DEFFENSEX,dest[i]);
				}
				return Vector(ball.Get_vPos().x,dest[i]);
			}
		}
	}
	else//球在右方则直接追球
	{
		return ball.Get_vPos();
	}
	return Vector(0,0);
}

Vector Player::ChooseSPSecondHalf(Ball ball)
{//左前锋（左后卫的选点方法）
	double dest[8];	//在球和自己一边的时候去尽力抢球，否则和球保持对应关系
	double value[8];
	if(ball.Get_vPos().y>MIDDLEPOINTY)
	{
		for(int i=0; i<8; i++)
		{
			if(this->m_Num!=i)
			{
				dest[i]=MIDDLEPOINTY-(i+1)*LENGTH;
				Vector d=Vector(ball.Get_vPos().x,dest[i]);
				if(d.NearRobot()==TRUE)
				{
					value[i]=0;
				}
				else
				{
					value[i]=1;
				}
			}
			else
			{
				value[i]=1;
			}
		}
		for(i=0;i<8; i++)
		{
			if(value[i]!=0)
			{
				if(ball.Get_vPos().x>OPPONENTDIFFENSEX)
				{
					return Vector(OPPONENTDIFFENSEX,dest[i]);
				}
				if(ball.Get_vPos().x<DEFFENSEX)
				{	
					return Vector(DEFFENSEX,dest[i]);
				}
				return Vector(ball.Get_vPos().x,dest[i]);
			}
		}
	}
	else//球在左方则直接追球
	{
		return ball.Get_vPos();
	}
	return Vector(0,0);
}

Vector Player::ChooseDP1SecondHalf(Ball ball)
{
	double dest[8];
	double value[8];
	double y=ball.Get_vPos().y-( (OPPONENTDIFFENSEX-ball.Get_vPos().x)*(m_OpponentGoal.y-ball.Get_vPos().y) )/(m_OpponentGoal.x-ball.Get_vPos().x);
	for(int i=0; i<8; i++)
	{
		if(this->m_Num!=i)
		{
			dest[i]=y+i*LENGTH;
			Vector d=Vector(OPPONENTDIFFENSEX,dest[i]);
			if(d.NearRobot()==TRUE)
			{
				value[i]=0;
			}
			else
			{
				value[i]=1;
			}
		}
		else
		{
			value[i]=1;
			dest[i]=0;
		}
	}
	for(i=0;i<8; i++)
	{
		if( (value[i]!=0) && (dest[i]!=0) )
		{
			return Vector(OPPONENTDIFFENSEX,dest[i]);
		}
	}
	return Vector(OPPONENTDIFFENSEX,MIDDLEPOINTY);
}

Vector Player::ChooseDP2SecondHalf(Ball ball)
{
	Vector d=ball.Get_vPos()-m_OpponentGoal;
	int R=120;
	double x=m_OpponentGoal.x+R*cos( d.GetAngle()*(pi/180));
	double y=m_OpponentGoal.y+R*sin( d.GetAngle()*(pi/180));
	return Vector(x,y);
}

BOOL Player::TurnTo(Vector v,UINT para_depth)
{
/*	DWORD time;
	Player::depth[this->Get_Num()]=para_depth;
	if(para_depth==0)
	{
		BasicAct(0,0,0,0);
		return TRUE;//认为已经转到
	}
	Vector a=Vector((dest.x-this->m_vPos.x),(dest.y-this->m_vPos.y));
	double angle=a.GetAngle()-(this->Get_m_Angle());

	if(angle>0)
		angle=(((int)angle%360)>180)?(int)angle%360-360:(int)angle%360;

	if(angle<0) 
		angle=(((int)(-angle)%360)>180)?(int)angle%360+360:(int)angle%360; 

	if(fabs(angle)<15 && fabs(angle)>165 )//认为已经转到,停止转动并返回
	{	
		BasicAct(0,0,0,0);
		return TRUE;
	}
	double V=256*MAX_VELOCITY/256;
	UINT elapse=fabs(angle)>90  ?  (180-fabs(angle))  :  fabs(angle);

	BOOL b;
	elapse=((UINT) fabs(angle) % 90 )> 45 ? 1.8*elapse:1.5*elapse;
	(angle>0)? b=(BOOL)(fabs(angle)<90):b=(BOOL)(fabs(angle)>90);
	
	BasicAct(0,b,FALSE,V,2.0*elapse+25);

//	time=1.8*elapse+10;
//	SetMMTimer(time,(DWORD)this,MMProc_TurnTo);
	return TRUE;

//	Set_vL(-255*MAX_VELOCITY/256);
//	Set_vR(255*MAX_VELOCITY/256);
//	SendCommand(30 );
*/

//	return TRUE;
	double angle;

	angle = v.GetAngle() - m_Angle;
	TRACE("\n---v.GetAngle:%f, m_Angle:%f, angle:%f------\n", v.GetAngle(), m_Angle, angle);
	if(fabs(angle) > 180)
	{
		if(angle < 0)
			angle = 360 + angle;
		else
			angle = angle - 360;
	}
	TurnAngle(angle);

	if(m_vL == 0 && m_vR == 0)
	{
		return TRUE;
	}
	else
		return FALSE;
}

BOOL Player::bShoot()
//判断是否进入射门范围：球的后面且与球的距离小于shootdist
//或者用更加严格的，在球与对方球门的延长线上的某点的圆内则射门
//那么就不需要用integratedAction了
{	
	/*11111111111111*/
/*	if( (this->Get_vPos().x<g_Ball.Get_vPos().x) && (this->HaveObstacle(g_Ball.Get_vPos())==FALSE) && (randb.mod()<SHOOTDIST ) )
	{
		return TRUE;
	}
	return FALSE;
*/
	/***2222222222222222222222222*************/
	double rdesty=g_Ball.Get_vPos().y+80*(g_Ball.Get_vPos().y-m_OpponentGoal.y)/(m_OpponentGoal.x-g_Ball.Get_vPos().x);
	Vector rafterball=Vector(g_Ball.Get_vPos().x-80,rdesty);//距离长设置为60
	Vector randb=rafterball-this->Get_vPos();
	if((this->Get_vPos().x<g_Ball.Get_vPos().x) && (this->HaveObstacle(g_Ball.Get_vPos())==FALSE) && (randb.mod()<SHOOTRANGE) )
	{
		return TRUE;
	}
	return FALSE;	
}


void Player::SimpleActionTest()
{
	Set_vL(CDebugDlg::m_pDebugDlg->m_vl);
	Set_vR(CDebugDlg::m_pDebugDlg->m_vr);
	this->SendCommand(300);
}

void Player::AllStop()
{
	SendSingleCommand(0x20,0x00,0x00);
	SendSingleCommand(0x40,0x00,0x00);
	SendSingleCommand(0x60,0x00,0x00);
	SendSingleCommand(0x80,0x00,0x00);
	SendSingleCommand(0x00,0x00,0x00);
}

BOOL Player::bShootSecondHalf()
{
	double rdesty=g_Ball.Get_vPos().y+80*(g_Ball.Get_vPos().y-m_MyGoal.y)/(g_Ball.Get_vPos().x-m_MyGoal.x);
	Vector rafterball=Vector(g_Ball.Get_vPos().x+80,rdesty);//距离长设置为60
	Vector randb=rafterball-this->Get_vPos();
	if((this->Get_vPos().x>g_Ball.Get_vPos().x) && (this->HaveObstacle(g_Ball.Get_vPos())==FALSE) && (randb.mod()<SHOOTRANGE) )
	{
		return TRUE;
	}
	return FALSE;	
}

Vector Player::ChooseGoaliePSecondHalf(Ball &ball)
{
	if( g_Ball.Get_vPos().x<OPPONENTGOALIEX)
	{
		if( g_Ball.Get_vPos().x<FORBIDDENRY )
		{
			return Vector(OPPONENTGOALIEX,FORBIDDENRY);
		}
		if( g_Ball.Get_vPos().x>FORBIDDENLY )
		{
			return Vector(OPPONENTGOALIEX,FORBIDDENLY);
		}
		return Vector(OPPONENTGOALIEX,g_Ball.Get_vPos().y);
	}
	else
	{
		if( g_Ball.Get_vPos().x<FORBIDDENRY )
		{
			return Vector(OPPONENTGOALIEX,FORBIDDENRY);
		}
		else
		{
			if( g_Ball.Get_vPos().x>FORBIDDENLY )
			{
				return Vector(OPPONENTGOALIEX,FORBIDDENLY);
			}
			else
			{
				return Vector(OPPONENTGOALIEX,g_Ball.Get_vPos().y);
			}
		}
	}
}

void Player::SendSingleCommand2(unsigned char command1,unsigned char command2,unsigned char command3)
{
	TCHAR FrameHead1;
	TCHAR FrameHead2;
	TCHAR FrameHead3;
	TCHAR FrameHead4;
	FrameHead1=(TCHAR)0x55;
	FrameHead2=(TCHAR)0xff;
	FrameHead3 = (TCHAR)0xaa;
	FrameHead4 = (TCHAR)0xbb;
	
/*	WriteABuffer(&FrameHead1, 1);
	WriteABuffer(&FrameHead1, 1);
	WriteABuffer(&FrameHead1, 1);
	WriteABuffer(&FrameHead1, 1);
*/
	WriteABuffer(&FrameHead2, 1);
	WriteABuffer(&FrameHead2, 1);
	WriteABuffer(&FrameHead2, 1);
	WriteABuffer(&FrameHead2, 1);


	WriteABuffer(&FrameHead3, 1);	
	WriteABuffer(&FrameHead3, 1);	
//	WriteABuffer(&FrameHead3, 1);	
//	WriteABuffer(&FrameHead4, 1);

//	WriteABuffer((TCHAR*)&command1, 1);
	WriteABuffer((TCHAR*)&command2, 1);//命令字
//	WriteABuffer((TCHAR*)&command3, 1);

//	WriteABuffer(&FrameHead2, 1);
//	WriteABuffer(&FrameHead2, 1);
	WriteABuffer(&FrameHead2, 1);
	WriteABuffer(&FrameHead2, 1);
	WriteABuffer(&FrameHead2, 1);
	WriteABuffer(&FrameHead2, 1);
}

unsigned char Player::TurnToExact(Vector vec)//有底层测速的转角度函数
//效果是将左右方向负值给变量	并且计算出需要走过的孔数负值给m_holes
//按照最近的的偏角转,结果为正对或者背对
{
	UINT m_holes =0 ;
	int  direct = 0 ;
	Vector r = vec - g_MyTeam[1].Get_vPos() ;//机器人指向目标点的向量
	Vector d = g_MyTeam[1].Get_vPos() - vec ;//目标点指向机器人的向量
	double delta =  g_MyTeam[1].m_Angle - r.GetAngle();
	//以下计算转过的孔数和轮数
	double t =  0.0;
	t = fmod( fabs( delta ), 180.0 );

	if( fabs(r.GetAngle() - g_MyTeam[1].m_Angle) < 20  )
		//机器人的正方向对准了球，误差为+-20；
	{
		return 0xcf;//正向直线走动
	}
	else if ( fabs( d.GetAngle() - g_MyTeam[1].m_Angle ) < 20 )
		//机器人的负方向对准了球，误差为+-20；
	{
		return 0x0f;//反向直线走动
	}

	if( t > 90 )
	{
		t = 180 - t ;
		m_holes = (UINT)( t / 14.5 ) ; //计算出转角的孔数
	}
	else
	{
		m_holes = (UINT)( t / 14.5 ) ; //计算出转角的孔数
	}
//以下计算方向；
	double t2 = fmod( delta, 180.0 ) ;
	if ( ( t2 <=0 ) && (t2 >=-90) )//向左
	{
		direct = 64 ;
	}
	else if ( (t2>=90)&&(t2<=180) ) //向左
	{
		direct = 64 ;
	}
	else//向右
	{
		direct = 128 ;
	}
	TCHAR command2 ;
	command2 = (TCHAR)( m_holes | direct ) ;
	return command2 ;
	/*	double delta2 = g_MyTeam[1].m_Angle - d.GetAngle() ;

	if( fabs(r.GetAngle() - g_MyTeam[1].m_Angle) < 20  )
		//机器人的正方向对准了球，误差为+-20；
	{
		return 0xff;//正向直线走动
	}
	else if ( fabs( d.GetAngle() - g_MyTeam[1].m_Angle ) <20 )
		//机器人的负方向对准了球，误差为+-20；
	{
		return 0x3f;//反向直线走动
	}
	else if ( fabs( delta ) < 90 )//正方向与目标点较近
//	else if( 1 )//试验
	{
		if( delta < 0 )//向左转
		{
			m_holes = (UINT)( -delta / 10) ; //计算出转角的孔数
			TCHAR command2 ;
			command2 = (TCHAR)( m_holes | 64 );
	//		Player::SendSingleCommand2( 1, command2, 1 );
			return command2;
		}
		else	//向右转
		{
			m_holes = (UINT)( delta /10 );
			TCHAR command2 ;
			command2 = (TCHAR)( m_holes | 128 ) ;
			return command2 ;
		}
	}
	else //反方向与目标点较近
	{
		if( delta2 > 0 )//向左转
		{
			m_holes = (UINT)( delta2 / 10) ; //计算出转角的孔数
			TCHAR command2 ;
			command2 = (TCHAR)( m_holes | 64 );
	//		Player::SendSingleCommand2( 1, command2, 1 );
			return command2;
		}
		else//向右转
		{
			delta2 = -delta2 )% 90 ; 
			m_holes = (UINT)( delta2 /10 );
			TCHAR command2 ;
			command2 = (TCHAR)( m_holes | 128 ) ;
			return command2 ;
		}
	}
*/
}

unsigned char Player::MoveToExact(Vector dest)
{
	TCHAR command2 ;
	UINT m_holes = 0 ;
	Vector r = g_Ball.Get_vPos() - g_MyTeam[1].Get_vPos() ;
	double m_distant = r.mod();
	if( m_distant > 80)  //超过80时，按照单位为80走
	{//80 / 1.15=70
		m_holes = 70 ;	
		command2 = (TCHAR)( m_holes | 192 );
//		Player::SendSingleCommand2( 1, command2, 1 );
		return command2;
	}
	else
	{
		m_holes = (int)(m_distant / 1.15) ;
		command2 = (TCHAR)( m_holes | 192 );
//		Player::SendSingleCommand2( 1, command2, 1 );
		return command2;
	}
}

void Player::SendSingleCommand3(unsigned char command1,unsigned char command2,unsigned char command3)
{
	TCHAR FrameHead1;
	TCHAR FrameHead2;
	TCHAR FrameHead3;
	TCHAR FrameHead4;
	FrameHead1=(TCHAR)0x55;
	FrameHead2=(TCHAR)0xff;
	FrameHead3 = (TCHAR)0xaa;
	FrameHead4 = (TCHAR)0xbb;
	
/*	WriteABuffer(&FrameHead1, 1);
	WriteABuffer(&FrameHead1, 1);
	WriteABuffer(&FrameHead1, 1);
	WriteABuffer(&FrameHead1, 1);
*/
	WriteABuffer(&FrameHead2, 1);
	WriteABuffer(&FrameHead2, 1);
	WriteABuffer(&FrameHead2, 1);
	WriteABuffer(&FrameHead2, 1);


	WriteABuffer(&FrameHead3, 1);	
	WriteABuffer(&FrameHead3, 1);	
//	WriteABuffer(&FrameHead3, 1);	
//	WriteABuffer(&FrameHead4, 1);

//	WriteABuffer((TCHAR*)&command1, 1);
	WriteABuffer((TCHAR*)&command2, 1);//转角命令字
	WriteABuffer((TCHAR*)&command3, 1);//直线命令字

//	WriteABuffer(&FrameHead2, 1);
//	WriteABuffer(&FrameHead2, 1);
	WriteABuffer(&FrameHead2, 1);
	WriteABuffer(&FrameHead2, 1);
	WriteABuffer(&FrameHead2, 1);
	WriteABuffer(&FrameHead2, 1);
}

void Player::slxSimpleAction(int vl, int vr, int time)
{
	Set_vL( vl);
	Set_vR( vr);
	SendCommand( time );	
}