csim.c

遗传算法和神经网络结合

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

 */

/* Standard libraries */
#include <stdio.h>
#include <unistd.h>
#include <values.h>
#include <stdio.h>
#include <fcntl.h>
#include <termio.h>
#include <time.h>

/* Simulator related includes*/ 
#include "geom.h"
#include "environ.h"
#include "param.h"
#include "real.h"
#include "terminal.h"

/* GUI related includes*/
#ifdef GUI

#include "monitor.h"
#include <pthread.h>
#include <semaphore.h>
#include <interface.h>
#include <support.h>

pthread_t tid;

sem_t gui_run;
sem_t gui_step;
sem_t gui_ready;
                 
#endif /* GUI */


/* The world and such alike */
Environ *world;
Real *real;

/* double   P90,P180,P360; */
#ifdef GUI
/* Callback functions for GUI */

void sim_redrawWorld(){
  world->redrawWorld();
}

float sim_getRobotX(){
  return world->robot[0]->getX();
}

float sim_getRobotY(){
  return world->robot[0]->getY();
}

void sim_setRobotX(float x){
  world->robot[0]->setX(x);
}
void sim_setRobotY(float y){
  world->robot[0]->setY(y);
}

void sim_setWorldSize(){
  gui_setWorldSize((int)(world->dx*xScale),
		   (int)(world->dy*xScale));
}

void sim_toggleGFX(){
  if(gfxUpdate){
    gfxUpdate=0;
  }
  else{
    gfxUpdate=1;
    world->redrawWorld();
  }
}

void sim_toggleUSER(){
  if(userUpdate){
    userUpdate=0;
  }
  else{
    userUpdate=1;
  }
}

#endif /* GUI */

void sim_shutdown(){
  exit(0);
}
void sigCtrlC(int dontCare){
  Terminal *term;
  term = new Terminal();
  term->addItem("q","Simulator shutdown",sim_shutdown);
#ifdef GUI
  term->addItem("t","Toggle GFX update",sim_toggleGFX);
#endif
  if(useSerialLine){
    for(int i=0; i < paramNrOfRobots; i++){
      /* Stop the robots */
      fprintf(stderr,"Telling bot %d to stop\n",i);
      real->runStep(0.5,0.5,i);
    }
  }
  term->runTerminal(); 
  delete(term);
}


int main(int argc,char **argv){
  int run;            /* running Step (interaction steps between the robot and the world */
  float i1,i2;
  char worldFileStr[200];
  clock_t time_now;
  double tps;
  double tused;


  seed=time(NULL);
  srand(seed);
  setlinebuf(stdout);
  setlinebuf(stderr);

  if(argc > 1)
    loadParams(argv[1]);
  else{
    fprintf(stderr,"No option file specified\n");
    fprintf(stderr,"use: sim optionFile.opt \n\n");
    return -1;
  }

  /* Prepare the world */
  /* First we create the world */
  world = new Environ();

  /* We tell the world to load the world specified in the option file 
   * and all nescary sensor sample files will be loaded to 
   */
  sprintf(worldFileStr,"%s/%s",worldPath,worldFile);
  world->loadWorld(worldFileStr);
  signal(SIGINT,sigCtrlC);
   
#ifdef GUI
  gtk_set_locale ();
  gtk_init (&argc, &argv);

  add_pixmap_directory ((const gchar*)"./pixmaps");
  add_pixmap_directory ((const gchar*)"./pixmaps");

  sim_setWorldSize();

  sem_init(&gui_ready,1,0); 
  sem_init(&gui_run,1,0);
  sem_init(&gui_step,1,1);

  pthread_create(&tid, NULL,(void *(*)(void *))gui_init,NULL);

  printf("Wait for gui to launch\n");
  sem_wait(&gui_ready);
  printf("GUI ready -> GO !!! \n");
  sem_wait(&gui_run);
#endif /* GUI */     

  /*############################################################REAL###############*/
  if(useSerialLine){ 
    real = new Real();
    if(!real->init()){
       fprintf(stderr,"main() Couldnt init serial communication\n");
       exit(-1);
    }

    /**************************************************/
    /*   Let the robot interact with the environment  */
    /*   for nrOfSteps time steps                     */
    for(run = 0; 1; run++){
      
      /**************************************************/
      /* Let the world generate sensor values for       */
      /* robot with index 0 */
      /* 0.5 0.5 */
#define T 0.5
#define T2 0.5
#define C 0.18
#define C1 0.1
      real->setInput(0);
      /* Try one 	 */
      /*      if(input[0][2] > C && input[0][1]> C1 ){ //|| input[0][3] > C && input[0][4]> C1){
	//	printf("turn right !!!\n");
	i1 = 0.6;
	i2 = 0.4;
      }
      else*/ if(input[0][0] > T){
	//	printf("adjust right\n");
	i1 = 0.7;
	i2 = 0.6;
      }
      else if(input[0][0] < T){
	//	printf("adjust left\n");
	i1 = 0.6;
	i2 = 0.7;
      }
      else{
	//	printf("Stroll in the park\n");
	i1 = i2 = 0.65;
      }
     
      /*
      i1 = 0.6  - (input[0][5] - input[0][0])*0.05;
      i2 = 0.6  + (input[0][5] - input[0][0])*0.05;
      */

#ifdef REAL_DEBUG
      //      printf("Motor left %f Motor right %f\n",i1,i2);
#endif
          real->runStep(i1,i2,0);  
    } 
    real->runStep(0.5,0.5,0);
  }
  /*############################################################SIMULATED##########*/
  else{

    world->resetEnvironment();
    world->robot[0]->saveState();
    time_now = clock();
    for(run = 0; run < 1000000; run++){
      
      /**************************************************/
      /* Let the world generate sensor values for       */
      /* robot with index 0 */

      world->setInput(0);
      /* Try one 	 */
      if(input[0][0] > T){
	i1 = 0.7;
	i2 = 0.6;
      }
      else if(input[0][5] > T){
	i1 = 0.6;
	i2 = 0.7;
      }
      else{
	i1 = i2 = 0.6;
      }
      world->runStep(i1,i2,0);  // robot 0

#ifdef GUI
      world->drawBots();	           
      if(gui_stepping)
	sem_wait(&gui_step);
#endif /* GUI */
      world->robot[0]->saveState();
    } /* End of individ loop */
  }
  tused = (double)(clock()-time_now)/(double)CLOCKS_PER_SEC;
  tps = 1000000 / tused;
  printf("Time used for 1.000.000 steps %.2f s %.2f steps/second %.2f faster than reality\n",tused,tps,tps/10); 
  return 0;
}