kicknrun.cpp

robocup3d源化码 Nexus3D.tar.gz

//		action_info.kick_power=100;
//		action_info.do_kick=true;
/////////////
	PostAction();
}

void KickNRun::Behave()
{
    AdjustHomePos();
    switch (mWM->GetPlayMode())
        {
        case PM_BeforeKickOff :
            BehaveBeforeKickOff();
	    		 DoLog(LOG_GOALIE,"BehaveBeforeKickOff done");
            break;

        case PM_KickOff_Left :
        case PM_KickOff_Right :
		DoLog(LOG_GOALIE,"PM_KickOff done");
            	BehaveKickOff();
            break;

        case PM_PlayOn :
		do_beam_on_kickof=true;
		DoLog(LOG_GOALIE,"PM_PlayOn done");
            	BehavePlayOn();
            break;

        case PM_KickIn_Left :
		if(mWM->GetMyTeam() == TI_RIGHT) BehaveKickIn();
		else BehaveKickIn();
	    break;
        case PM_KickIn_Right :
           	if(mWM->GetMyTeam() == TI_LEFT) BehaveKickIn();
		else BehaveKickIn();
	    break;
	case PM_GOAL_KICK_LEFT:
		PredictBallPos();
	    	DoLog(LOG_GOALIE,"goal kick left");
	    	if(mWM->GetMyTeam() == TI_LEFT && self.IsGoalie){
			//BehavePlayOn();
			if(ball.pos[0] < -FieldLength/2 + 10)
			{
				if(!AdjustKickPos.AdjustNeeded(0,10)){
					action_info.kick_angle=0;
					action_info.kick_direction=0;
					action_info.kick_power=40;
					action_info.do_kick=true;
					DoLog(LOG_GOALIE,"L Initial Kick");
				}
				else {
					Vector3f Pos=AdjustKickPos.MoveAroundBall(0);
					DoLog(LOG_GOALIE,"adjust nedded    pos(%f,%f)",Pos[0],Pos[1]);
					action_info.x = Pos[0] ;
					action_info.y = Pos[1] ;
					action_info.do_drive = true ;
					DoLog(LOG_GOALIE,"L initial turn");
				}
			}
			else
				if(!AdjustKickPos.AdjustNeeded(0,10)){
					action_info.kick_angle=25;
					action_info.kick_direction=0;
					action_info.kick_power=100;
					action_info.do_kick=true;
					DoLog(LOG_GOALIE,"L kick");
				}
				else {
					Vector3f Pos=AdjustKickPos.MoveAroundBall(0);
					DoLog(LOG_GOALIE,"adjust nedded    pos(%f,%f)",Pos[0],Pos[1]);
					action_info.x = Pos[0] ;
					action_info.y = Pos[1] ;
					action_info.do_drive = true ;
					DoLog(LOG_GOALIE,"L turn");
				}
			DoLog(LOG_GOALIE,"l done");
	    	}
	    	else
			positioning.Position();
			//GoTo(self.home_pos[0],self.home_pos[1],0);
		PostAction();
	    break;
	case PM_GOAL_KICK_RIGHT:
		PredictBallPos();
	    	DoLog(LOG_GOALIE,"goal kick right");
	    	if(mWM->GetMyTeam() == TI_RIGHT && self.IsGoalie){
			//BehavePlayOn();
			if(ball.pos[0] < -FieldLength/2 + 10)
			{
				if(!AdjustKickPos.AdjustNeeded(0,10)){
					action_info.kick_angle=0;
					action_info.kick_direction=0;
					action_info.kick_power=40;
					action_info.do_kick=true;
					DoLog(LOG_GOALIE,"L Initial Kick");
				}
				else {
					Vector3f Pos=AdjustKickPos.MoveAroundBall(0);
					DoLog(LOG_GOALIE,"adjust nedded    pos(%f,%f)",Pos[0],Pos[1]);
					action_info.x = Pos[0] ;
					action_info.y = Pos[1] ;
					action_info.do_drive = true ;
					DoLog(LOG_GOALIE,"L initial turn");
				}
			}
			else
				if(!AdjustKickPos.AdjustNeeded(0,10)){
					action_info.kick_angle=25;
					action_info.kick_direction=0;
					action_info.kick_power=100;
					action_info.do_kick=true;
					DoLog(LOG_GOALIE,"L kick");
				}
				else {
					Vector3f Pos=AdjustKickPos.MoveAroundBall(0);
					DoLog(LOG_GOALIE,"adjust nedded    pos(%f,%f)",Pos[0],Pos[1]);
					action_info.x = Pos[0] ;
					action_info.y = Pos[1] ;
					action_info.do_drive = true ;
					DoLog(LOG_GOALIE,"L turn");
				}
			DoLog(LOG_GOALIE,"l done");
	    	}
	    	else
			positioning.Position();
			//GoTo(self.home_pos[0],self.home_pos[1],0);
		PostAction();
	    break;
	case PM_CORNER_KICK_LEFT:
		if(mWM->GetMyTeam() == TI_RIGHT){
			int nearest2ball=11;
			float nearestdist=MyPlayer[11].dist2ball;
			for(int i=1;i<=11;i++) if(MyPlayer[i].dist2ball<nearestdist){
				 nearest2ball=i;
				 nearestdist=MyPlayer[i].dist2ball;
			}
			if(MyNumber!=nearest2ball){
				BehavePlayOn();
			}
			else{
				BehaveKickIn();
				
			}
		}
		else{
			if(MyNumber==1&&ball.pos[1]<0) {
				action_info.x = -FieldLength/2 ;
				action_info.y = -GoalWidth/2 ;
				action_info.do_drive = true ;
			}
			else if(MyNumber==1){
				action_info.x = -FieldLength/2 ;
				action_info.y = GoalWidth/2 ;
				action_info.do_drive = true ;
			}
			else{
				BehavePlayOn();
			}
			
		}
	    break;     
	case PM_CORNER_KICK_RIGHT:
		if(mWM->GetMyTeam() == TI_LEFT){
			int nearest2ball=11;
			float nearestdist=MyPlayer[11].dist2ball;
			for(int i=1;i<=11;i++) if(MyPlayer[i].dist2ball<nearestdist){
				 nearest2ball=i;
				 nearestdist=MyPlayer[i].dist2ball;
			}
			if(MyNumber!=nearest2ball){
				BehavePlayOn();
			}
			else{
				BehaveKickIn();
				
			}
		}
		else{
			if(MyNumber==1&&ball.pos[1]<0) {
				action_info.x = -FieldLength/2 ;
				action_info.y = -GoalWidth/2 ;
				action_info.do_drive = true ;
			}
			else if(MyNumber==1){
				action_info.x = -FieldLength/2 ;
				action_info.y = GoalWidth/2 ;
				action_info.do_drive = true ;
			}
			else{
				BehavePlayOn();
			}
			
		}
	    break;    
        default:
		BehavePlayOn();
		DoLog(LOG_GOALIE,"default done");
            break;
        }
}
void KickNRun::AdjustHomePos(){
		if(ball.pos[0]<-45){
			MyPlayer[1].home_pos = Vector3f(-47,0,0);
			MyPlayer[2].home_pos = Vector3f(-45,-20,0);
			MyPlayer[3].home_pos = Vector3f(-45,-5,0);
			MyPlayer[4].home_pos = Vector3f(-45,5,0);
			MyPlayer[5].home_pos = Vector3f(-45,20,0);
			MyPlayer[6].home_pos = Vector3f(-20,-9,0);
			MyPlayer[7].home_pos = Vector3f(-25,0,0);
			MyPlayer[8].home_pos = Vector3f(-20,9,0);
			MyPlayer[9].home_pos = Vector3f(-10,12,0);
			MyPlayer[10].home_pos = Vector3f(-5,0,0);
			MyPlayer[11].home_pos = Vector3f(-10,-12,0);
		}
		else if(ball.pos[0]<-30){
			MyPlayer[1].home_pos = Vector3f(-47,0,0);
			MyPlayer[2].home_pos = Vector3f(-40,-20,0);
			MyPlayer[3].home_pos = Vector3f(-45,-5,0);
			MyPlayer[4].home_pos = Vector3f(-45,5,0);
			MyPlayer[5].home_pos = Vector3f(-40,20,0);
			MyPlayer[6].home_pos = Vector3f(-15,-9,0);
			MyPlayer[7].home_pos = Vector3f(-20,0,0);
			MyPlayer[8].home_pos = Vector3f(-15,9,0);
			MyPlayer[9].home_pos = Vector3f(-5,12,0);
			MyPlayer[10].home_pos = Vector3f(0,0,0);
			MyPlayer[11].home_pos = Vector3f(-5,-12,0);
		}
		else if(ball.pos[0]<-20){
			MyPlayer[1].home_pos = Vector3f(-47,0,0);
			MyPlayer[2].home_pos = Vector3f(-33,-12,0);
			MyPlayer[3].home_pos = Vector3f(-38,-5,0);
			MyPlayer[4].home_pos = Vector3f(-38,5,0);
			MyPlayer[5].home_pos = Vector3f(-33,12,0);
			MyPlayer[6].home_pos = Vector3f(-12,-9,0);
			MyPlayer[7].home_pos = Vector3f(-17,0,0);
			MyPlayer[8].home_pos = Vector3f(-12,9,0);
			MyPlayer[9].home_pos = Vector3f(0,12,0);
			MyPlayer[10].home_pos = Vector3f(5,0,0);
			MyPlayer[11].home_pos = Vector3f(0,-12,0);
		}
		else if(ball.pos[0]<-10){
			MyPlayer[1].home_pos = Vector3f(-47,0,0);
			MyPlayer[2].home_pos = Vector3f(-25,-12,0);
			MyPlayer[3].home_pos = Vector3f(-30,-5,0);
			MyPlayer[4].home_pos = Vector3f(-30,5,0);
			MyPlayer[5].home_pos = Vector3f(-25,12,0);
			MyPlayer[6].home_pos = Vector3f(-5,-10,0);
			MyPlayer[7].home_pos = Vector3f(-15,0,0);
			MyPlayer[8].home_pos = Vector3f(-5,10,0);
			MyPlayer[9].home_pos = Vector3f(5,12,0);
			MyPlayer[10].home_pos = Vector3f(10,0,0);
			MyPlayer[11].home_pos = Vector3f(5,-12,0);
		}
		else if(ball.pos[0]<0){
			MyPlayer[1].home_pos = Vector3f(-47,0,0);
			MyPlayer[2].home_pos = Vector3f(-20,-12,0);
			MyPlayer[3].home_pos = Vector3f(-23,-5,0);
			MyPlayer[4].home_pos = Vector3f(-25,5,0);
			MyPlayer[5].home_pos = Vector3f(-20,12,0);
			MyPlayer[6].home_pos = Vector3f(0,-10,0);
			MyPlayer[7].home_pos = Vector3f(-10,0,0);
			MyPlayer[8].home_pos = Vector3f(0,10,0);
			MyPlayer[9].home_pos = Vector3f(10,12,0);
			MyPlayer[10].home_pos = Vector3f(15,0,0);
			MyPlayer[11].home_pos = Vector3f(10,-12,0);
		}
		else if(ball.pos[0]<10){
			MyPlayer[1].home_pos = Vector3f(-47,0,0);
			MyPlayer[2].home_pos = Vector3f(-18,-12,0);
			MyPlayer[3].home_pos = Vector3f(-15,0,0);
			MyPlayer[4].home_pos = Vector3f(-23,0,0);
			MyPlayer[5].home_pos = Vector3f(-18,12,0);
			MyPlayer[6].home_pos = Vector3f(7,-10,0);
			MyPlayer[7].home_pos = Vector3f(0,0,0);
			MyPlayer[8].home_pos = Vector3f(7,10,0);
			MyPlayer[9].home_pos = Vector3f(18,12,0);
			MyPlayer[10].home_pos = Vector3f(25,0,0);
			MyPlayer[11].home_pos = Vector3f(18,-12,0);
		}
		else if(ball.pos[0]<20){
			MyPlayer[1].home_pos = Vector3f(-47,0,0);
			MyPlayer[2].home_pos = Vector3f(-15,-12,0);
			MyPlayer[3].home_pos = Vector3f(-6,0,0);
			MyPlayer[4].home_pos = Vector3f(-20,0,0);
			MyPlayer[5].home_pos = Vector3f(-15,12,0);
			MyPlayer[6].home_pos = Vector3f(12,-11,0);
			MyPlayer[7].home_pos = Vector3f(8,0,0);
			MyPlayer[8].home_pos = Vector3f(12,11,0);
			MyPlayer[9].home_pos = Vector3f(30,12,0);
			MyPlayer[10].home_pos = Vector3f(35,0,0);
			MyPlayer[11].home_pos = Vector3f(30,-12,0);
		}
		else if(ball.pos[0]<30){
			MyPlayer[1].home_pos = Vector3f(-47,0,0);
			MyPlayer[2].home_pos = Vector3f(-10,-12,0);
			MyPlayer[3].home_pos = Vector3f(5,0,0);
			MyPlayer[4].home_pos = Vector3f(-15,0,0);
			MyPlayer[5].home_pos = Vector3f(-10,12,0);
			MyPlayer[6].home_pos = Vector3f(17,-12,0);
			MyPlayer[7].home_pos = Vector3f(13,0,0);
			MyPlayer[8].home_pos = Vector3f(17,12,0);
			MyPlayer[9].home_pos = Vector3f(38,11,0);
			MyPlayer[10].home_pos = Vector3f(43,0,0);
			MyPlayer[11].home_pos = Vector3f(38,-11,0);
		}
		else if(ball.pos[0]<40){
			MyPlayer[1].home_pos = Vector3f(-47,0,0);
			MyPlayer[2].home_pos = Vector3f(-3,-12,0);
			MyPlayer[3].home_pos = Vector3f(10,0,0);
			MyPlayer[4].home_pos = Vector3f(-7,0,0);
			MyPlayer[5].home_pos = Vector3f(-3,12,0);
			MyPlayer[6].home_pos = Vector3f(20,-12,0);
			MyPlayer[7].home_pos = Vector3f(15,0,0);
			MyPlayer[8].home_pos = Vector3f(20,12,0);
			MyPlayer[9].home_pos = Vector3f(41,11,0);
			MyPlayer[10].home_pos = Vector3f(43,0,0);
			MyPlayer[11].home_pos = Vector3f(41,-11,0);
		}
		else{
			MyPlayer[1].home_pos = Vector3f(-47,0,0);
			MyPlayer[2].home_pos = Vector3f(-3,-12,0);
			MyPlayer[3].home_pos = Vector3f(15,0,0);
			MyPlayer[4].home_pos = Vector3f(-7,0,0);
			MyPlayer[5].home_pos = Vector3f(-3,12,0);
			MyPlayer[6].home_pos = Vector3f(25,-12,0);
			MyPlayer[7].home_pos = Vector3f(20,0,0);
			MyPlayer[8].home_pos = Vector3f(25,12,0);
			MyPlayer[9].home_pos = Vector3f(45,11,0);
			MyPlayer[10].home_pos = Vector3f(45,0,0);
			MyPlayer[11].home_pos = Vector3f(45,-11,0);
		}
		//DoLog(LOG_GOALIE,"Self.home_pos=(%f,%f)",MyPlayer[MyNumber].home_pos[0],MyPlayer[MyNumber].home_pos[1]);
		if(ball.pos[1]>20){
			MyPlayer[1].home_pos[1]+=10;
			MyPlayer[2].home_pos[1]+=5;
			MyPlayer[3].home_pos[1]+=6;
			MyPlayer[4].home_pos[1]+=7;
			MyPlayer[5].home_pos[1]+=10;
			MyPlayer[6].home_pos[1]+=5;
			MyPlayer[7].home_pos[1]+=8;
			MyPlayer[8].home_pos[1]+=10;
			MyPlayer[9].home_pos[1]+=5;
			MyPlayer[10].home_pos[1]+=8;
			MyPlayer[11].home_pos[1]+=10;

		}
		else if(ball.pos[1]>10){
			MyPlayer[1].home_pos[1]+=5;
			MyPlayer[2].home_pos[1]+=2;
			MyPlayer[3].home_pos[1]+=3;
			MyPlayer[4].home_pos[1]+=3;
			MyPlayer[5].home_pos[1]+=5;
			MyPlayer[6].home_pos[1]+=2;
			MyPlayer[7].home_pos[1]+=4;
			MyPlayer[8].home_pos[1]+=5;
			MyPlayer[9].home_pos[1]+=2;
			MyPlayer[10].home_pos[1]+=4;
			MyPlayer[11].home_pos[1]+=5;

		}
		else if(ball.pos[1]<-20){
			MyPlayer[1].home_pos[1]-=10;
			MyPlayer[2].home_pos[1]-=5;
			MyPlayer[3].home_pos[1]-=6;
			MyPlayer[4].home_pos[1]-=7;
			MyPlayer[5].home_pos[1]-=10;
			MyPlayer[6].home_pos[1]-=5;
			MyPlayer[7].home_pos[1]-=8;
			MyPlayer[8].home_pos[1]-=10;
			MyPlayer[9].home_pos[1]-=5;DoLog(LOG_GOALIE,"cear1");
			MyPlayer[10].home_pos[1]-=8;
			MyPlayer[11].home_pos[1]-=10;

		}
		else if(ball.pos[1]<-10){
			MyPlayer[1].home_pos[1]-=5;
			MyPlayer[2].home_pos[1]-=2;
			MyPlayer[3].home_pos[1]-=3;
			MyPlayer[4].home_pos[1]-=4;
			MyPlayer[5].home_pos[1]-=5;
			MyPlayer[6].home_pos[1]-=2;
			MyPlayer[7].home_pos[1]-=4;
			MyPlayer[8].home_pos[1]-=5;
			MyPlayer[9].home_pos[1]-=2;
			MyPlayer[10].home_pos[1]-=4;
			MyPlayer[11].home_pos[1]-=5;

		}
		self.home_pos = MyPlayer[MyNumber].home_pos;
		//DoLog(LOG_GOALIE,"Self.home_pos=(%f,%f)",self.home_pos[0],self.home_pos[1]);

}
void KickNRun::PostAction(){
	if(action_info.do_drive) GoTo(action_info.x,action_info.y,action_info.drive_speed);
	if(action_info.do_kick){
		 Kick(action_info.kick_angle,action_info.kick_power);
		 DoLog(LOG_GOALIE,"kick done2");
	}
	if(!action_info.do_drive && !action_info.do_kick){
		 DoLog(LOG_GOALIE,"No action perfromed");
    		 Drive(Vector3f(0,0,0));
		 //GoTo(self.pos[0],self.pos[1],0.0);
	}
	action_info.do_drive=false;
	action_info.do_kick=false;
	action_info.x = 0;
	action_info.y = 0;
	action_info.drive_speed = 0;
	action_info.kick_angle = 0;
	action_info.kick_direction = 0;
	action_info.kick_power = 0;
}
void KickNRun::PredictBallPos(){
	//estimation of ball pos
	Vector3f ball_vel,ball_pos,ball_decay;
	float 	ln_time=(CurrentTime - self.last_info[self.index_pos].time);
	ball_vel=ball.vel*ln_time;
	ball_pos=ball.pos;
	DoLog(LOG_GOALIE,"ln_time=%f",ln_time);
	bool indexed=false;
	for(int i=1;i<=40;i++){
		if(fabs(ball_vel[0])>1.5) ball_decay[0]=0.78;
		else if(fabs(ball_vel[0])>1) ball_decay[0]=0.80;
  		else if(fabs(ball_vel[0])>0.85) ball_decay[0]=0.85;
		else ball_decay[0]=0.88;
		if(fabs(ball_vel[1])>1.5) ball_decay[1]=0.78;
		else if(fabs(ball_vel[1])>1) ball_decay[1]=0.80;
  		else if(fabs(ball_vel[1])>0.85) ball_decay[1]=0.85;
		else ball_decay[1]=0.88;
		ball_pos=ball_pos+ball_vel;
		ball_vel[0]=ball_vel[0]*ball_decay[0];
		ball_vel[1]=ball_vel[1]*ball_decay[1];
		//if(!indexed&&fabs(ball_vel[0])<0.02&&fabs(ball_vel[1])<0.02){
		if(!indexed && ball_vel.Length() < 0.3){
			stop_index=i;
			indexed=true;
		}
		predicted_ball_pos[i]=ball_pos;
		DoLog(LOG_GOALIE,"ball_pos[%d](%f,%f)   ball_decay=%f,%f",i,ball_pos[0],ball_pos[1],ball_decay[0],ball_decay[1]);
		DoLog(LOG_GOALIE,"ball_vel[%d](%f,%f)",i,ball_vel[0],ball_vel[1]);
	}
	DoLog(LOG_GOALIE,"ball_pos[%d](%f,%f)",stop_index,predicted_ball_pos[stop_index][0],predicted_ball_pos[stop_index][1]);


}