usenns.c

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

/* usenns.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"

#define COLLECT_DEBUG         1

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

int ChooseWait( float *tang, float *dpow){

  *dpow = 0;     /* Just stay put                 */
  return TRY;    
}

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

int ChooseStraight (float *tang, float *dpow){
  if ( Mem->BallValid() == 1 && fabs(Mem->GetBallAngle()) < 30 )
    return TRY;
  else 
    return WAIT; /* take a cycle to face the ball */
}

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

int ChooseRandom( float *tang, float *dpow){
#if 0
  /* Now do this in int_random() (called by range_random() */
  static int FirstTime = TRUE;
  
  if ( FirstTime ){
    /* initialize the random number seed. */
    timeval tp;
    gettimeofday( &tp, NULL );
    srandom( (unsigned int) tp.tv_usec );
    FirstTime = FALSE;
  }
#endif
  *tang = range_random(-45,45);
  return TRY;
}

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

int ChooseNN( float *tang, float *dpow){

  static int FirstTime = TRUE;
  
  if ( FirstTime ){
    /* initialize the NN. */
    printf ("%s \n",INIT_ToUse_FILE);
    NN_initialize_to_use(INIT_ToUse_FILE);
    FirstTime = FALSE;
  }

  if (Mem->GetBallDistance() > NN_CDIST){
    return WAIT;
  }
  
  float Net_array[((NN_n_inp > NN_n_out) ? NN_n_inp : NN_n_out)];

#if 0
  /* vdir was 4th input before 6/12/96                                        */
  float vdir = 180 - Mem->GetBallRelVelDirection();  /* Reverse the direction */
  if (vdir == 180) vdir = 0;                         /* Unreverse if it was 0 */
  CleanAngle(&vdir);                                                       
#endif
  Net_array[0] = Mem->GetBallDistance();
  Net_array[1] = Mem->GetBallAngle();
  Net_array[2] = Mem->GetBallRelVelMagnitude();
  /*Net_array[2] = Mem->GetBallRelVelMagnitude()*(Mem->CurrentTime - Mem->LastTime);*/
  /*Net_array[3] = vdir;*/

#if 0
  /* test new velocity calcs */
  /* No good--the global position isn't exact enough */
  float realvdir = Mem->GetBallVelocityAngle();
  float realvmag = Mem->GetBallVelocityMagnitude();
  printf("   -- mypos  (%.1f, %.1f)\n",
	 Mem->GetGlobalX(),Mem->GetGlobalY());
  printf("time: %d -- relvel(r,t): (%.1f, %.1f)     realvel: (%.1f, %.1f)\n",
	 Mem->CurrentTime,Mem->GetBallRelVelMagnitude(),vdir,
	 realvmag,rad_to_deg(realvdir));
#endif

  NN_use(Net_array);

  /*    float confidence = Net_array[2];*/
  /*      *GoPower   = Net_array[1];*/
  *tang = Net_array[0];

  return TRY;
}

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

int ChooseTable( float *tang, float *dpow){

  float dist = Mem->GetBallDistance();
  float ang = Mem->GetBallAngle();
  float vel = Mem->GetBallRelVelMagnitude()*(Mem->CurrentTime - Mem->LastTime);

  if ( ang == 0 )
    *tang = 0;
  else if ( fabs(ang) == 1 )
    *tang = 9;
  else if ( fabs(ang) == 2 )
    *tang = 16;
  else if ( fabs(ang) == 3 )
    *tang = 21;
  else if ( fabs(ang) == 4 )
    *tang = 27;
  else if ( fabs(ang) == 5 )
    *tang = 29;
  else if ( fabs(ang) == 6 )
    *tang = 35;
  else if ( fabs(ang) == 7 )
    *tang = 40;
  else
    *tang = 45;

  if ( ang<0 )
    *tang *= -1;

  return TRY;
}

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

int ChooseMikeAnalytic( float *tang, float *dpow){

  float dist = Mem->GetBallDistance();
  float ang  = Mem->GetBallAngle();
  float vel  = Mem->GetBallRelVelMagnitude()*(Mem->CurrentTime - Mem->LastTime);

  float x,y,old_x,old_y,dx,dy,aim_x,aim_y,aim_r,aim_t;
  float factor = 2;   /* amount by which to multiply velocity */
  
  polar_to_relxy(dist,0, &x,&y);
  polar_to_relxy(dist+vel,-deg_to_rad(ang), &old_x,&old_y);
  dx = x - old_x;
  dy = y - old_y;
  aim_x = x + factor*dx;
  aim_y = y + factor*dy;
  relxy_to_polar(aim_x,aim_y, &aim_r,&aim_t);
  aim_t = rad_to_deg(aim_t);

  *tang = aim_t;

  return TRY;
}

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

int ChooseAnalytic( float *tang, float *dpow){

  if ( !Mem->BallTrajectoryValid() ){
    my_error("Can't use analytic method without knowing trajectory");
  }
  if ( !Mem->BallValid() ){
    my_error("Can't use analytic method without knowing ball position");
  }

  float speed  = Mem->GetBallTrajectoryMagnitude();
  float x,y;     Mem->GetBallGlobalXY(&x,&y);
  float veldir = Mem->GetBallTrajectoryAbsoluteDirection();
  float dist   = Mem->GetBallDistance();

  /*int factor = MIN(15,(int)(dist/speed));   /* amount by which to multiply velocity   */
                                            /* Make sure it's in front of the player? */
  int factor = (int)(dist/speed);   
  
  float dx = speed*cos(veldir);
  float dy = speed*sin(veldir);
  float aim_x = x + factor*dx;
  float aim_y = y + factor*dy;

  float aim_r,aim_t;
  Mem->globalxy_to_polar(aim_x,aim_y, &aim_r,&aim_t);
  aim_t = rad_to_deg(aim_t);

  CleanAngle(&aim_t);

  /* printf("  factor %d  ball dir %.1f  aim (%.1f, %.1f) %.1f (%d:%d)\n",
	 factor,rad_to_deg(veldir),aim_x,aim_y,aim_t,
	 Mem->MyNumber,Mem->CurrentTime); */

  *tang = aim_t - Mem->GetBallAngle(); /* will be added back on */

  return TRY;
}