robot.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 "robot.h"


float Robot::Step;
float Robot::MinObstacleDistans;
float Robot::MaxObstacleDistans;
float Robot::robot[20][180][8];

/**
 * Constructor for robot, creates a new robot and initiates it's motor.
 * @attention You must call loadParams() before creating a robot.
 */
Robot::Robot(){
  char tmpStr[200];
  robotRadius = 27.5;
  rmotor = new Motor();
  sprintf(tmpStr,"%s/%s",cameraPath,motorCamera);
  rmotor->init(tmpStr);

  /* N */
  nRod = 0;
  fRodX = NULL;
  fRodY = NULL;
  /* N */
  carry = -1;
}

/**
 * Deconstructor for robot. Deallocates all memory and destroys motor.
 */
Robot::~Robot(){
  /* N */
  if(nRod>0){
    free(fRodX);
    free(fRodY);
  }
  /* N */
  delete(rmotor);
}

/* N */
/** 
 * Get the x coordinate of rod with internal index \a index.
 * @param index Internal index of the rod.
 * @return The x coordinate or 0.0 if index is out of range.
 */
float Robot::getRodX(int index) const{
  if((index>=0) && (index<nRod))
    return fRodX[index];
  return 0.0;
}
/** 
 * Get the y coordinate of rod with internal index \a index.
 * @param index Internal index of the rod.
 * @return The y coordinate or 0.0 if index is out of range.
 */
float Robot::getRodY(int index) const{
  if((index>=0) && (index<nRod))
    return fRodY[index];
  return 0.0;
}

/**
 * Returns 1 if a gripper robot.
 * @return Allways 0 here.
 */
int Robot::isaGripperRobot(){
  return(0);
}

/**
 * Adds a rod a to the robot.
 * @param x Relative x coordinate (relative to robot center).
 * @param y Relative y coordinate (relative to robot center).
 */
void Robot::addRod(float x, float y){
  fRodX = (float*)realloc(fRodX,sizeof(float) * (nRod + 1));
  fRodY = (float*)realloc(fRodY,sizeof(float) * (nRod + 1));
  fRodX[nRod] = x;
  fRodY[nRod] = y;
  nRod++;
}

/* N */
/**
 * Initiate the robot with the motor specified.
 * @param fileName The file name of the motor file.
 */
void Robot::init(char *fileName){
  FILE *fp;
  int    d,a,s;
  int tmp,angles,sensors,steps;

  if(verboseLevel>0){
    printf("***[Init robot body]***\n");
  }
  fp=fopen(fileName, "r");
  if(fp==NULL){
    printf("Couldn't open robots 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);
       robot[d][a][s]=(float)(tmp/1024.0);
      }
      fscanf(fp,"\n");
    }
  }
  fflush(fp);    
  fclose(fp);
}

/**
 * Get the radius of the robot.
 * @return The radius.
 */
float Robot::getRadius(){
  return robotRadius;
}

/**
 * Get the x coordinate of the robot.
 * @return The coordinate.
 */
float Robot::getX(){
  return x;
}

/**
 * Get the y coordinate of the robot.
 * @return The coordinate.
 */
float Robot::getY(){
  return y;
}

/**
 * Get the x coordinate of the last time step.
 * @return The coordinate.
 */
float Robot::getOX(){
  return oldx;
}

/**
 * Get the y coordinate of the last time step.
 * @return The coordinate.
 */
float Robot::getOY(){
  return oldy;
}

/**
 * Set the x coordinate of the robot.
 * @param val The coordinate.
 */
void Robot::setX(float val){
  x = val;
}

/**
 * Set the y coordinate of the robot.
 * @param val The coordinate.
 */
void Robot::setY(float val){
  y = val;
}

/**
 * Sets motor state to zero, saves last timestep.
 */
void Robot::resetRobot(){
  oldx = x;
  oldy = y;
  mot1 = mot2 =oldmot1 = oldmot2 = 0;
  carry = -1;
  olddirection = direction;
}

/**
 * Recall last saved state.
 */
void Robot::recallState(){
  x = oldx;
  y = oldy;
  mot1 = oldmot1;
  mot2 = oldmot2;
  direction = olddirection;
}

/**
 *  Get the angle for the last call to run().
 *  
 *  The angle which specifies in what direction the robot moved.
 */
double Robot::getLastAngR(){
  return(lastAngR);
}

/**
 *  Get the distance for the last call to run().
 *  
 *  The distance which specifies how long the robot moved.
 */
double Robot::getLastDist(){
  return(lastDist);
}

/**
 * Runs the khepera robot for 100 ms.
 * @param o1 Left motor activation, [0.0 - full back, 0.5 - stand still, 1.0 - full forward].
 * @param o2 Right motor activation, [0.0 - full back, 0.5 - stand still, 1.0 - full forward].
 */
void Robot::run(float o1, float o2){

  int v1,v2;
  float   ang,dist;
  float tmpD;
  /* o1 and o2 is in the range 0 < x < 1*/

  v1 =(int) (o1 * 20.0);
  if ( ((o1 * 200) - (v1 * 20)) > 5)
    v1++;
  v2 = (int)(o2 * 20.0);
  if ( ((o2 * 200) - (v2 * 20)) > 5)
    v2++;
  v1 -= 10;
  v2 -= 10;

  mot1 = v1 / 10.0;
  mot2 = v2 / 10.0;
  ang = rmotor->motor[v1+12][(v2+12)*2]; /* (v2+12)*2 */
  dist = rmotor->motor[v1+12][(v2+12)*2+1] * 10.0;

  tmpD = (direction/180)*M_PI;

  lastAngR = tmpD;
  lastDist = dist;
  
  x = g_displaceXR(x,dist,tmpD);
  y = g_displaceYR(y,dist,tmpD);

  direction += ang;
  if (direction >= 360.0)
    direction -= 360;
  if (direction < 0.0)
    direction += 360;
}

/**
 * Save current robot state.
 */
void Robot::saveState(){
  oldx = x;
  oldy = y;
  oldmot1 = mot1;
  oldmot2 = mot2;
  olddirection = direction;
}

/**
 * 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 Robot::relangle(float kx, float ky, float kangle){
   
   double ox,oy;
   double oad;
   int oa;
   int ka;
   int ra;

   /* robot 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 robot 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 robot is out of range.
 */
float *Robot::sensorVal(float kx, float ky, float kangle,float robRadius){
  double dist;
  int relang; 

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