train.c

足球机器人仿真组CMU97的源码

/* train.c
 * CMUnited-97 (soccer client for Robocup-97)
 * Peter Stone <[e-mail]>
 * Computer Science Department
 * Carnegie Mellon University
 * Copyright (C) 1997 Peter Stone
 *
 * CMUnited-97 was created by Peter Stone and Manuela Veloso
 *
 * You may copy and distribute this program freely as long as you retain this notice.
 * If you make any changes or have any comments we would appreciate a message.
 */

#include "global.h"

/*******************************************/
/* Check NN_CDIST in usenns.h (now it's 14 */
/*******************************************/

int TrainCollect (int (*ChoiceFunction)(float*,float*), 
		  float CollectDist, float DashPower){

  float TurnAng = 0;        /* By default, just go straight at ball */
  
  if ( !Mem->BallValid() ) {        /* If Can't see ball */
    FINDBALL;                       /* Look for it       */ 
    WAITUNTILFREETOACT;
  }

#if 0

    float bdist = Mem->GetBallDistance();
    float ang  = Mem->GetBallAngle();
    float relvelmag  = Mem->GetBallRelVelMagnitude();
    float relvelang  = Mem->GetBallRelVelDirection();  /* My calculation  */
    float velmag     = Mem->GetBallVelMagnitude();     /* from the server   */
    float velang     = Mem->GetBallVelDirection();     /* In relative terms */
    float ballvalid  = Mem->BallValid();
    float relvelvalid = Mem->BallRelVelocityValid();
    int   velvalid   = Mem->BallVelValid();
    float ballx       = Mem->GetBallGlobalX();
    float bally       = Mem->GetBallGlobalY();
    float trajvalid   = Mem->BallTrajectoryValid();
    float trajdir,trajmag;
    if (trajvalid){
      trajdir     = rad_to_deg(Mem->GetBallTrajectoryAbsoluteDirection());
      trajmag     = Mem->GetBallTrajectoryMagnitude();
    }
    
    printf ("%d:%d me(%.1f) (%.1f,%.1f %.1f)   ball(%.1f): (%.1f %.1f)   global: (%.1f,%.1f)\n",
	    Mem->MyNumber,Mem->CurrentTime,
	    Mem->MarkerValid(THEIR_GOAL), Mem->GetGlobalX(),Mem->GetGlobalY(),
	    rad_to_deg(Mem->GetGlobalAngle()),
	    ballvalid,bdist,ang,  ballx,bally);
    if (trajvalid && trajmag < 5) /* else a jump */
      printf ("  **** traj conf = (%.1f)    dir = %.1f   mag = %.1f **** \n",
	      trajvalid,trajdir,trajmag);
    
    /*
      printf ("%d(%.1f,%.1f)  ball(%.1f): (%.1f %.1f)   relvel(%.1f): (%.1f %.1f)   vel(%d): (%.1f %.1f)\n",
      Mem->CurrentTime,Mem->GetGlobalX(),Mem->GetGlobalY(),
      ballvalid,dist,ang,  relvelvalid,relvelmag,relvelang,  velvalid,velmag,velang);
      */
#endif

  if ( !Mem->BallValid() ) {
    return FAILURE;
  }
  else if ( Mem->GetBallDistance() > CollectDist) { /* If Far from ball  */
    FACEBALL;                                       /* wait              */
    return TRYING;
  }
  else if ( Mem->GetBallDistance() > KICK_DISTANCE )  { /* If it's in range  */

#if CHOICE_FUNCTION_DEBUG
    if ( ChoiceFunction == &ChooseNN )
      printf("Player %d is trying to use the NN --- ",Mem->MyNumber);
#endif

#if 0
    float tmp1,tmp2;
    if ( /* (*ChoiceFunction)( &tmp1, &tmp2) == WAIT || */
	 Mem->BallValid() < 1 || !Mem->BallRelVelocityValid() || 
	 /* Really want to do this if I moved on last step (Dashed) */
	 Mem->GetBallDistance() < 6 || Mem->GetBallDistance() > NN_CDIST+10 || 
	 Mem->Dashed || Mem->GetBallVelDirection() == NODIR ||
	 !Mem->GetBallVelMagnitude() || 
	 fabs(Mem->GetBallVelDirection()) < 135 || 
	 Mem->GetBallRelVelMagnitude() < NN_MIN_VEL)/* Moving too slowly for NN */
      ChoiceFunction = &ChooseStraight;             /* Only use nets when ball
						       is approaching and near*/
#else
    if ( Mem->BallValid() < 1 || !Mem->BallTrajectoryValid() /*|| 
	 Mem->GetBallTrajectoryMagnitude() > 5 || 
	 Mem->GetBallTrajectoryMagnitude() < .3*/){
      /*printf("straight\n");*/
      ChoiceFunction = &ChooseStraight;  
    }
    else {
      /*printf("analytic\n");*/
      ChoiceFunction = &ChooseAnalytic;        
    }
#endif

#if CHOICE_FUNCTION_DEBUG
    printf("relvelvalid:  %.1f -- RelVelMag:  %.1f -- ",
	   Mem->BallRelVelocityValid(),Mem->GetBallRelVelMagnitude());
    if ( ChoiceFunction == &ChooseNN )
      printf("YES using it(%.1f)\n",Mem->GetBallVelDirection());
    else
      printf("NOT using it\n");
#endif
    if ( ((*ChoiceFunction)( &TurnAng, &DashPower) == TRY) &&
	 Mem->PlayMode != BEFORE_KICK_OFF ){ /* Not at a pause    */
#if SAVE_COLLECT_DATA
      if ( Mem->RecordCollectData ){
	RecordCollectData(TurnAng,DashPower);   /* Record now and not */
	Mem->RecordCollectData = FALSE;             /* again 'til result  */
      }
      else TurnAng =0;                              /* Stop after dash    */
#endif
      float BallAng = Mem->GetBallAngle();

      int moveRes;
      /* Go more quickly after the NN attempt */
      if (ChoiceFunction != &ChooseNN){
	int dashTimes = MAX_ACTS;
	float distFactor = 1;
	if (Mem->GetBallDistance() < 5)
	  dashTimes =2;
	/*printf("%d dashing\n",Mem->CurrentTime);*/
	moveRes = GOMOVE(distFactor*Mem->GetBallDistance(),TurnAng+BallAng, 
			 DashPower*Mem->GetBallDistance(),1,dashTimes);
      }
      else{
	moveRes = DASHMOVE(TurnAng+BallAng, DashPower*Mem->GetBallDistance());
                  /* move: returns TRIED if dash, TRYING if dodgeplayer */
      }
      /* if ( TurnAng ) TURN ( -TurnAng );
                     /* If you didn't move straight, look back           */
      return moveRes;
    }
    else { /* ChoiceFunction == WAIT or In between trials (before_kick_off) */
      /*printf("%d facing ball\n",Mem->CurrentTime);*/
      FACEBALL;
      return TRYING;
    }
  }
  else if ( Mem->BallValid() == 1 )  {
    Mem->ClearBallGlobalPosition();
    return SUCCESS;                             /* Sure you're at it */
  }
  else {
    Mem->ClearBallGlobalPosition();
    FACEBALL;
    return TRIED;                               /* Not sure          */
  }
}
/*#define TRAINCOLLECT(a,b,c) TrainCollect(a,b,c)*/


/******************************************************************************/
/******************************************************************************/
/******************************************************************************/
/*  Training functions for Justin's algorithm                                 */

void TestKickParameters(float TrapPower, float FirstKickPower, float SecondKickPower){

  NARROW_VIEW;
  FACEMARKER(THEIR_GOAL);
  WAITFORSIGHT;

  while ( !Mem->BallValid() || !Mem->MarkerValid(THEIR_GOAL) ){ 
    WAITFORSIGHT;  /* Wait until you know where the ball and the goal are */
  }

  while ( Mem->GetBallDistance() > KICK_DISTANCE ){
    WAITFORSIGHT;  /* Wait until the ball is nearby again                 */
  }
  
  /* kick the ball towards the player */
  KICK(TrapPower,Mem->GetBallAngle() + 180);

  /* Start kicking at the goal */
  float GoalAngle = Mem->GetMarkerAngle(THEIR_GOAL);
  KICK(FirstKickPower,GoalAngle);
  KICK(SecondKickPower,GoalAngle);

  int counter=0;
  WAITUNTILFREETOACT;
  while ( Mem->GetBallDistance() < KICK_DISTANCE ){
    KICK(100,GoalAngle);
    counter++;
    WAITUNTILFREETOACT;
  }

  printf("Trapped with power %.1f, kicked with powers: %.1f %.1f, 100 (%d times)\n",
	 TrapPower,FirstKickPower,SecondKickPower,counter);
}
/* #define TESTKICKPARAMETERS(a,b,c) TestKickParameters(a,b,c) */


/******************************************************************************/

/* Training for Justin's collect function */

void TestCollectParameters(float playerSpeed, float timeMultiplier1, 
			   float timeMultiplier2, float dashTwiceBuffer, 
			   float facePointBuffer, float DashPower,
			   float GoToPointBuffer,
			   float free1, float free2){

  /* DashPower = 50; GoToPointBuffer = 1; */

  float CollectDist = 30;

#if SAVE_COLLECT_DATA
      if ( Mem->RecordCollectData ){
	/*RecordCollectData(0,0);   /* Record now and not */
	Mem->RecordCollectData = FALSE;             /* again 'til result  */
      }
#endif

  if ( !Mem->BallValid() ) {        /* If Can't see ball */
    FINDBALL;                       /* Look for it       */ 
    WAITUNTILFREETOACT;
  }

  if ( !Mem->BallValid() ) {
    return;
  }
  else if ( Mem->GetBallDistance() > CollectDist) { /* If Far from ball  */
    FACEBALL;                                       /* wait              */
    return;
  }
  else if ( Mem->GetBallDistance() > KICK_DISTANCE &&
	    Mem->BallTrajectoryValid() )  { /* If it's in range  */

    float ballX,ballY, myX,myY, targetX,targetY;
    Mem->GetGlobalXY(&myX,&myY);
    Mem->GetBallGlobalXY(&ballX,&ballY);

    /* Convert my position to origin (0,0)*/
    ballX -= myX; ballY -= myY;
    
    float ballDistance = Mem->GetBallDistance();
    float ballDirection = Mem->GetBallTrajectoryAbsoluteDirection();
    float ballSpeed = Mem->GetBallTrajectoryMagnitude();

    float ballDx = ballSpeed * cos(ballDirection);
    float ballDy = ballSpeed * sin(ballDirection);

    float a = ballX*ballX + ballY*ballY;
    float b = 2*(ballX*ballDx + ballY*ballDy); 
    float c = ballDx*ballDx + ballDy*ballDy - playerSpeed*playerSpeed; 
    float disc = b*b - 4*a*c;

#if COLLECT_DEBUG    
    printf("%d:%d me (%.1f, %.1f), ball (%.1f, %.1f) -- s/d: %.1f %.1f\n",
	   Mem->MyNumber,Mem->CurrentTime,
	   myX,myY,ballX,ballY,ballSpeed,rad_to_deg(ballDirection));
    printf("           ballD (%.1f, %.1f)......",ballDx,ballDy);
#endif

    int BallTooFast = FALSE;
    if (disc < 0){
#if 1
      printf("%d:%d The ball is too fast!\n",Mem->MyNumber,Mem->CurrentTime);
#endif
      BallTooFast = TRUE;
    }

    float t;  /* Time to intersection point */
    if ( !BallTooFast ){
      float u1 = (-b + sqrt(disc)) / (2*a); 
      float u2 = (-b - sqrt(disc)) / (2*a); 
    
      float u = (u1 > u2) ? u1 : u2; 
      t = 1.0 / u; 
      t *= timeMultiplier1;

      printf("%d:%d Chose between %.1f and %.1f\n",
	     Mem->MyNumber,Mem->CurrentTime,
	     1.0/u1, 1.0/u2);

      if (u <= 0){
#if 1
	printf("%d:%d Why negative time? Chose between %.1f and %.1f\n",
	       Mem->MyNumber,Mem->CurrentTime,
	       1.0/u1, 1.0/u2);
#endif
	BallTooFast = TRUE;
      }
    }

    if ( BallTooFast )
      /* Aim for where ball is going anyway */
      t = (ballDistance/ballSpeed)*timeMultiplier2; 
    else
      printf("%d:%d Using analytic method\n",Mem->MyNumber,Mem->CurrentTime);

    targetX = ballX + ballDx * t; 
    targetY = ballY + ballDy * t; 
    
    /* First convert target back to relative to my position */
    targetX += myX; targetY += myY;

#if 1
    printf("target: (%.1f, %.1f)\n",targetX,targetY);
#endif

    /* Blowing up GOTOPOINT to put in params */
    float dist = GetDistance(&targetX,&targetY,&myX,&myY);
    if ( dist > GoToPointBuffer ) {         
      FacePoint(targetX,targetY,facePointBuffer);
      if (dist >= dashTwiceBuffer)
	GOMOVE(dist,0,DashPower*dist,2,3);            /* Two dashes at least*/
      else
	GOMOVE(dist,0,DashPower*dist,1,3);            /* One dash at least  */
      return;
    }
    else                                              /* Find ball          */
      return;     /* TRYING, TRIED (turned), or SUCCESS (seeing)*/
  }
  else if ( Mem->BallValid() == 1 )  {
    /* Mem->ClearBallGlobalPosition(); */
    return;                             /* Sure you're at it */
  }
  else {
    /* Mem->ClearBallGlobalPosition(); */
    FACEBALL;
    return;                               /* Not sure          */
  }
}
/*#define TESTCOLLECTPARAMETERS(a,b,c,d,e,f,g,h,i) TestCollectParameters(a,b,c,d,e,f,g,h,i)*/



















#if 0
int TrainCollect (int (*ChoiceFunction)(float*,float*), 
		  float CollectDist, float DashPower){

  float TurnAng = 0;        /* By default, just go straight at ball */
  
  if ( !Mem->BallValid() ) {        /* If Can't see ball */
    return FAILURE;
  }
  else if ( Mem->GetBallDistance() > CollectDist) { /* If Far from ball  */
    FACEBALL;                                       /* wait              */
    return TRYING;
  }
  else if ( Mem->GetBallDistance() > KICK_DISTANCE )  {         /* If it's in range  */
    if ( Mem->BallRelVelocityValid() &&             
	 Mem->GetBallRelVelMagnitude() &&           /* and moving        */
	 ((*ChoiceFunction)( &TurnAng, &DashPower) == TRY) &&
	 Mem->PlayMode != BEFORE_KICK_OFF ){        /* Not at a pause    */
#if SAVE_COLLECT_DATA
      if ( Mem->RecordCollectData ){
	RecordCollectData(TurnAng,DashPower,Mem);   /* Record now and not */
	Mem->RecordCollectData = FALSE;             /* again 'til result  */
      }
      else 
	TurnAng = DashPower = 0;                    /* Stop after dash    */
	/*TurnAng = 0;*/
#endif
      float BallAng = Mem->GetBallAngle();
      int moveRes = DASHMOVE(TurnAng+BallAng, DashPower*Mem->GetBallDistance());
                     /* go: returns TRIED if dash, TRYING if dodgeplayer */
      if ( TurnAng ) TURN ( -TurnAng );
                     /* If you didn't move straight, look back           */
      return moveRes;
    }
    else { /* ChoiceFunction == WAIT or In between trials (before_kick_off) */
      FACEBALL;
      return TRYING;
    }
  }
  else if ( Mem->BallValid() == 1 )  {
    return SUCCESS;                             /* Sure you're at it */
  }
  else {
    FACEBALL;
    return TRIED;                               /* Not sure          */
  }
}
/*#define TRAINCOLLECT(a,b,c) TrainCollect(a,b,c)*/
#endif