sobst.cc

遗传算法和神经网络结合

/*
  YAKS, a Khepera simulator including a separate GA and ANN 
  (Genetic Algoritm, Artificial Neural Net).
  Copyright (C) 2000  Johan Carlsson ([e-mail])
  
  This program is free software; you can redistribute it and/or
  modify it under the terms of the GNU General Public License
  as published by the Free Software Foundation; either version 2
  of the License, or any later version.
  
  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License for more details.
  
  You should have received a copy of the GNU General Public License
  along with this program; if not, write to the Free Software
  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.

 */

#include "sobst.h"

float Sobst::MaxObstacleDistans;
float Sobst::MinObstacleDistans;
float Sobst::Step;
float Sobst::r;
float Sobst::sobst[20][180][8];

/**
 * Constructor for Sobst, creates a new small obstacle
 * @param fp A file descriptor open for reading which contains the data for the new obstacle.
 */
Sobst::Sobst(FILE *fp){
  fscanf(fp,"sroundobst %f %f\n", 
	 &x,
	 &y
	 );
  taken=0.0;
  orgX = x;
  orgY = y;
}

/**
 * Constructor for Sobst, creates a new small obstacle
 * @param str A string which contains the data for the new obstacle.
 */
Sobst::Sobst(char *str){
  sscanf(str,"sroundobst %f %f\n", 
	 &x,
	 &y
	 );
  taken=0.0;
  orgX = x;
  orgY = y;
}


/**
 * Initialize all small obstacles. (static class members)
 * @param fileName The name of the camerafile to read.
 */
void Sobst::init(char *fileName){
  FILE *fp;
  int d,a,s,tmp,angles,sensors,steps;

  if(verboseLevel>0){
    printf("***[Init small objects]***\n");
  }
  fp=fopen(fileName, "r");
  if(fp==NULL){
    printf("Couldn't open small objects camera file %s for reading\n",fileName);
    exit(-1);
  }
  if(verboseLevel>0) printf("- %s\n",fileName);
  fscanf(fp,"ANGLES %d\n",&angles);
  if(verboseLevel>0) printf("- ANGLES %d\n",angles);
  fscanf(fp,"SENSORS %d\n",&sensors);
  if(verboseLevel>0) printf("- SENSORS %d\n",sensors);
  fscanf(fp,"STEPS %d\n",&steps);
  if(verboseLevel>0) printf("- STEPS %d\n",steps);
  fscanf(fp,"MAX DISTANS %f\n",&MaxObstacleDistans);
  if(verboseLevel>0) printf("- MAX DISTANS %f\n",MaxObstacleDistans);
  fscanf(fp,"MIN DISTANS %f\n",&MinObstacleDistans);
  if(verboseLevel>0) printf("- MIN DISTANS %f\n",MinObstacleDistans); 

  Step = (MaxObstacleDistans - MinObstacleDistans) / steps;
  for (d = 0; d < 20; d++){
    fscanf(fp,"TURN %d\n",&tmp);
    if(tmp!=d){
      printf("Something is wrong in camerafile %s\n",fileName);
      exit(-1);
    }
    
    for (a = 0; a < 180; a++){
      for (s = 0; s < 8; s++){
	fscanf(fp,"%d ",&tmp);
	sobst[d][a][s]=((double)tmp)/1024.0;
      }
      fscanf(fp,"\n");
    }
  }
  fflush(fp);    
  fclose(fp);
}

/**
 * Deconstructor for Sobst, does nothing.
 *
 */
Sobst::~Sobst(){
}
/**
 * Save current position of the obstacle
 * @see resetMove
 * @see tempMove
 */
void Sobst::saveMove(){
  orgX = x;
  orgY = y;
}

/**
 * Reset obstacle location to last save.
 * @see saveMove
 * @see tempMove
 */
void Sobst::resetMove(){
  x = orgX;
  y = orgY;
}

/**
 * Move the obstacle.
 * @param dist The distance to move the obstacle.
 * @param angR In which direction to move the obstacle.
 * @see resetMove
 * @see saveMove
 */
void Sobst::tempMove(double dist, double angR){
  x = g_displaceXR(x,dist,angR);
  y = g_displaceYR(y,dist,angR);
}

/**
 * Save the current static members to a camerafile.
 * @param fileName The name of the camerafile to save.
 */
void Sobst::save(char *fileName){
  FILE *fp;
  int    d,a,s;
  
  fp=fopen(fileName, "w");
  if(fp==NULL){
    printf("Couldn't open large objects camera file %s for writing\n",fileName);
    exit(-1);
  }

  fprintf(fp,"MAX DISTANS %f\n",MaxObstacleDistans);
  fprintf(fp,"MIN DISTANS %f\n",MinObstacleDistans);

  for (d = 0; d < 20; d++){
    fprintf(fp,"TURN %d\n",d);
    for (a = 0; a < 180; a++){
      for (s = 0; s < 8; s++){
	fprintf(fp,"%f ",sobst[d][a][s]);
      }
      fprintf(fp,"\n");
    }
  }
  fflush(fp);    
  fclose(fp);
}

/**
 * Set the radius of all small obstacles.
 * @param rad The new radius.
 */
void Sobst::setRadius(float rad){
  r = rad;
}

/**
 * Get the relative angle between the small obstacle and the khepera robot.
 * @param kx Robot \a x coordinate.
 * @param ky Robot \a y coordinate.
 * @param kangle Robot angle (direction/orientation).
 * @return The relative angle.
 */
int Sobst::relangle(float kx, float ky, float kangle){
   
   double ox,oy;
   double oad;
   int oa;
   int ka;
   int ra;

   /* obst is in direction */
   ox = (double)(x - kx);
   oy = (double)(y - ky);
   oad = atan2(ox,oy);
   oa =(int)g_RADtoDGR(oad);
   /* coordinate system is upside down */
   oa = oa * -1;

   /* rotate kangle to match oa */
   /* coordinate system is upside down */
   ka = g_adjustAngle360((int)kangle,270);
   ka = g_splitAngle180(ka);

   ra =(int)(oa - ka);
   ra = g_modulo(ra,360);   
   return ra;
}

/**
 * Get the sensor values that the small obstacle will produce on the robot.
 * @param kx Robot \a x coordinate.
 * @param ky Robot \a y coordinate.
 * @param kangle Robot angle (direction/orientation).
 * @param robRadius Robot radius.
 * @return An array of eight floats or NULL if the obstacle is out of range or a robot is carrying the obstacle.
 */
float *Sobst::sensorVal(float kx, float ky, float kangle,float robRadius){
  double dist;
  int relang; 

  if(taken == 0.0){
    dist = g_distPoint(kx,ky,x,y)-r-robRadius;
    if(dist<MaxObstacleDistans){
      relang = relangle(kx,ky,kangle);
      relang = relang >> 1;
      if(dist<MinObstacleDistans){ /* Very close no need to calculate distans */
	return sobst[0][relang]; /* returns pointer to the 8 sensor values */ 
      }
      else{ /*  Min < dist < Max */
	return sobst[(int)((dist-MinObstacleDistans)/Step)][relang];
      }
    }
    else{ /* the obstacle is to far away to be seen */ 
      return NULL;
    }
  }
  else{ /* The robot is carring the obstacle */
    return NULL;
  }
}