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

int Real::serialLine[50];

Real::Real(){
}

Real::~Real(){
}

// We now only support one serial line,
// buy a radio turret if you cant deal with that
boolean Real::init(){
  struct termios term;
  static long int speedkey;
  int fd;
  serialLine[0] = open(serialDevice[0], O_RDWR | O_EXCL);
  if(serialLine[0] < 0){
    perror("Real::init() serial open");
    return FALSE;
  }
  fd = serialLine[0];
  if (tcgetattr(fd,&term)!=0)
  {
    perror("Real::init() getting attributes");
    return FALSE;
  }
  switch(baudRate[0])
  {
    case 9600:	speedkey=B9600; break;
    case 19200:	speedkey=B19200; break;
    case 38400:	speedkey=B38400; break;
    default:	return FALSE;
  }
  cfsetispeed(&term,speedkey);  /* Speed */
  cfsetospeed(&term,speedkey);
  clear(term.c_iflag,            /* Input modes */
        IGNBRK|BRKINT|IGNPAR|INPCK|ISTRIP|ICRNL|INLCR|IXON|IXOFF);
  set(term.c_iflag,0);
  clear(term.c_oflag, OPOST);    /* Output modes */
  set(term.c_oflag,0);
  clear(term.c_cflag,CSIZE|PARENB);     /* Control modes */
  set(term.c_cflag,CS8|CSTOPB);
                                               /* Line modes */
  clear(term.c_lflag,ISIG|ICANON|XCASE|ECHO|IEXTEN);
  set(term.c_lflag,0);   /* MIN and TIME */
  term.c_cc[VMIN]  = 0;
  term.c_cc[VTIME] = KHEP_DEFAULT_TIMEOUT;
  if (tcsetattr(fd,TCSANOW,&term)!=0)
  {
    perror("REAL::init() setting attributes");
    return FALSE;
  }
  return TRUE;
}

int Real::readline(int fd, char *buffer, int bsize){
   int nRd,rSize;
   char c;
   rSize = 0;
   do{
     nRd=read(fd,&c,1);
     if(nRd < 0)
       break;
     buffer[rSize] = c;
     rSize+=nRd;
   }while(nRd==1 && rSize < bsize && c != '\n');
   buffer[rSize]='\0';
#ifdef REAL_DEBUG
   printf("%s\n",buffer);
#endif
   if(nRd>=0) 
     return rSize;
   else{
     perror("Real::readline()");
     return nRd;
   }
   
}

/*int Real::order(int wR, char *command, char *answear,int aSize){
  
  write(serialLine[wR], command, strlen(command));
  return readline(serialLine[wR],answear,aSize);
  
  }*/

void Real::serialDrain(int fd ,boolean verbose)
{
  char answer[100];
  int  rsize;

  do
  {
    rsize = readline(fd,answer,strlen(answer));
    if (verbose) fprintf (stderr,"draining %d bytes",rsize);
  }
  while (rsize > 0); /* Read till nothing more to read */
}

/* Khepera package routines                                                 */
/* These routine make a specific use of the serial management functions to  */
/* provide a more complex tool to communicate with a Khepera                */

/* Send line send           */
/* and receive line receive */
/* without any check control*/
boolean Real::khepSendRecv(int fd,char *send,char *receive,int recv_len)
{
  int rd;

  write(fd,send,strlen(send));

  if ((rd = readline(fd,receive,recv_len)) > 0 ) return(TRUE);
  else
  {
    if (rd < 0) perror("Real::khepSendRecv()  read error");
    else perror("Real::khepSendRecv()  khepera not responding");
    return(FALSE);
  }
}


/* Send line send and receive line       */
/* receive with check control :          */
/* The first character of the received   */
/* line is the lower first character of  */
/* the send line.                        */
/* Try to have the good answer           */
/* KHEP_NB_RETRY times                   */
boolean Real::khepTalk(int fd,char *send,char *receive,int recv_len){
  int  rd,rsize,tries = 0;
  char respond = tolower(send[0]);
  
  do{
    write(fd,send,strlen(send));
    rd = readline(fd,receive,recv_len);
    if (rd < 0){ /* File error */
      perror("Real::khepTalk() read/write error");
      break;
    }
    else if (rd == 0) /* No response */
      tries++;
    else if (receive[0] != respond){ /* Incorrect answer */
      perror("Real::khepTalk() protocol error");
      serialDrain(fd,FALSE);
      tries++;
    }
    else{ /* Correct answer */
      if ((rsize = strlen(receive)) >= 2){ /* Strip terminating '\r\n' if any */
	if (receive[rsize-1] == '\n') receive[rsize-1] = '\0';
	if (receive[rsize-2] == '\r') receive[rsize-2] = '\0';
      }
      return(TRUE);
    }
  }while (tries < KHEP_NB_RETRY);
  if (rd == 0) 
    perror("Real::khepTalk() Khepera not responding");
  return(FALSE);
}
		   
void Real::setInput(int robotNr){
  static char buff[200];
  int kalle[8];
  buff[0] = '\0';
  if(khepTalk(serialLine[0],"N\n",buff,199)){
    sscanf(buff,"n, %d, %d, %d, %d, %d, %d, %d, %d",
	   &kalle[0],
	   &kalle[1],
	   &kalle[2],
	   &kalle[3],
	   &kalle[4],
	   &kalle[5],
	   &kalle[6],
	   &kalle[7]);
    for(int i=0; i < 8; i++){
      input[robotNr][i]=((double)kalle[i]/1024.0);
    }
  }
  else{
    fprintf(stderr,"Real::setInput() failed for robot %d\n",robotNr);
  }
}

void Real::runStep(double mot1, double mot2, int robotNr){
  char buff[200];
  char command[30];
  sprintf(command,"D,%d,%d\n",int((double(mot1*20)-10)),
	  int((double(mot2*20)-10)));
  if(!khepTalk(serialLine[0],command,buff,199)){
    fprintf(stderr,"Real::runStep serial failure\n");
  }
}

void Real::nextIndivid(){
  char buff[200];

  //  for(int i=0; i < paramNrOfRobots; i++)
  if(!khepTalk(serialLine[0],"D,0,0\n",buff,199))
    fprintf(stderr,"Real::nextIndivid() failed to stop robot\n");
}