Servidor de Control (S.C.)
// Proyecto Robot Deimos 2000 // Creado por Carlos Boto Coto // Archivo RobotDeimos.cpp #include <iostream> #include <string.h> #include <pthread.h> #include <sys/time.h> #include "sistemas/SisLocoMotor.h" #include "sistemas/SisNeuronal.h" #include "sistemas/SisSensorial.h" using namespace std; pthread_mutex_t mutexComando; pthread_cond_t despierta; int iTermina; int aComandos[4]; void *cerebro(void *threadid) { SisLocoMotor *sisLocoMotor; SisNeuronal *sisNeuronal; long tid; tid = (long)threadid; int aAux[4]; cout << "Arrancando sistema locomotor....\n"; sisLocoMotor=new SisLocoMotor(); cout << "Sistema Locomotor:OK\n"; cout << "Arrancando sistema Neuronal.....\n"; sisNeuronal=new SisNeuronal(sisLocoMotor); cout << "Sistema Neuronal:OK\n"; struct timespec ts; struct timeval tp; while (iTermina) { pthread_mutex_lock(&mutexComando); gettimeofday(&tp, NULL); ts.tv_sec = tp.tv_sec; ts.tv_nsec = tp.tv_usec * 1000; ts.tv_sec += 1; pthread_cond_timedwait(&despierta, &mutexComando,&ts); for (int i=0;i<4;i++) { aAux[i]=aComandos[i]; aComandos[i]=0; } // iDireccion=iDireccion+1; pthread_mutex_unlock(&mutexComando); sisNeuronal->procesaComando(aAux); /*iSegundos++; if (iSegundos==60) { //Comprueba que llegamos con un pin while ((iSegundos==0)) { sisNeuronal->procesaComandoInverso(&aOrdenes[iSegundos]); iSegundos--; } }*/ } delete sisLocoMotor; pthread_exit(NULL); } int main(void) { SisSensorial *sisSensorial; pthread_t thread; long t; //1. El sistema sensorial enviará datos al sistema neuronal. //2. El sistema neuronal procesara los datos recibidos del sistema senorial y enviará órdenes al sistema LocoMotor sisSensorial=new SisSensorial(); cout << "Sistema sensorial: OK\n"; pthread_mutex_init(&mutexComando, NULL); pthread_cond_init (&despierta, NULL); char sCmd[255]=""; iTermina=1; pthread_create(&thread, NULL, cerebro, (void *)t); while (sisSensorial->getEstimulo(sCmd)) { pthread_mutex_lock(&mutexComando); if (sCmd[0]=='1') aComandos[0]=1; else if (sCmd[0]=='2') aComandos[0]=2; else if (sCmd[0]=='0') aComandos[0]=0; if (sCmd[1]=='1') aComandos[1]=1; else if (sCmd[1]=='2') aComandos[1]=2; else if (sCmd[1]=='0') aComandos[1]=0; if (sCmd[2]=='1') aComandos[2]=1; else if (sCmd[2]=='2') aComandos[2]=2; else if (sCmd[2]=='0') aComandos[2]=0; if (sCmd[3]=='1') aComandos[3]=1; else if (sCmd[3]=='2') aComandos[3]=2; else if (sCmd[3]=='0') aComandos[3]=0; pthread_cond_signal(&despierta); pthread_mutex_unlock(&mutexComando); } iTermina=0; delete sisSensorial; pthread_mutex_destroy(&mutexComando); pthread_cond_destroy(&despierta); sleep(1); pthread_exit(NULL); return 0; } ------------------------------------------ // Proyecto Robot Deimos 2000 // Creado por Carlos Boto Coto // Archivo sistemas/SisLocoMotor.h #ifndef SISLOCOMOTOR_H_ #define SISLOCOMOTOR_H_ #include "../drivers/SerialDriver.h" class SisLocoMotor { public: SisLocoMotor(); virtual ~SisLocoMotor(); int avanza(); int retrocede(); int derecha(); int izquierda(); int para(); int recto(); int arriba(); int abajo(); int izquierdacam(); int derechacam(); int stop(); private: SerialDriver *puerto; void cambiaPuerto(); }; #endif /*SISLOCOMOTOR_H_*/ ------------------------------------------------------------------------ // Proyecto Robot Deimos 2000 // Creado por Carlos Boto Coto // Archivo sistemas/SisLocoMotor.cpp #include "SisLocoMotor.h" #include <unistd.h> /* UNIX standard function definitions */ #include <stdio.h> SisLocoMotor::SisLocoMotor() { puerto=new SerialDriver("/dev/ttyUSB1"); char c; c= 0x6E; //OFF Todos if (!puerto->writeport(c)) cambiaPuerto(); puerto->writeport(c); } void SisLocoMotor::cambiaPuerto() { char c; c=0x6E; printf("CambiaPuerto\n"); if (puerto) delete puerto; puerto=new SerialDriver("/dev/ttyUSB0"); if (!puerto->writeport(c)) { if (puerto) delete puerto; puerto=new SerialDriver("/dev/ttyUSB1"); puerto->writeport(c); } } SisLocoMotor::~SisLocoMotor() { char c; c= 0x6E; if (!puerto->writeport(c)) { cambiaPuerto(); puerto->writeport(c); } delete puerto; } int SisLocoMotor::stop() { char c; c= 0x6E; //OFF Todos if (!puerto->writeport(c)) { cambiaPuerto(); puerto->writeport(c); } return 0; } int SisLocoMotor::avanza() { char c; c= 0x65; //ON Relà if (!puerto->writeport(c)) { cambiaPuerto(); puerto->writeport(c); } c= 0x70; //OFF Relé 2 if (!puerto->writeport(c)) { cambiaPuerto(); return 1; } return 0; } int SisLocoMotor::retrocede() { char c; c= 0x66; //2 ON if (!puerto->writeport(c)) { cambiaPuerto(); puerto->writeport(c); } c= 0x6F; //1 OFF if (!puerto->writeport(c)) { cambiaPuerto(); return 1; } return 0; } int SisLocoMotor::para() { char c; c= 0x70; //2 OFF if (!puerto->writeport(c)) { cambiaPuerto(); puerto->writeport(c); } c= 0x6F; //1 OFF if (!puerto->writeport(c)) { cambiaPuerto(); return 1; } return 0; //Correcto } int SisLocoMotor::izquierda() { char c; c= 0x6C; //8 ON if (!puerto->writeport(c)) { cambiaPuerto(); puerto->writeport(c); } c= 0x75; //7 OFF if (!puerto->writeport(c)) { cambiaPuerto(); return 1; } return 0; //Correcto } int SisLocoMotor::derecha() { char c; c= 0x6B; //7 ON if (!puerto->writeport(c)) { cambiaPuerto(); puerto->writeport(c); } c= 0x76; //8 OFF if (!puerto->writeport(c)) { cambiaPuerto(); return 1; } return 0; //Correcto } int SisLocoMotor::recto() { char c; c= 0x75; //7 OFF if (!puerto->writeport(c)) { cambiaPuerto(); puerto->writeport(c); } c= 0x76; //8 OFF if (!puerto->writeport(c)) { cambiaPuerto(); return 1; } return 0; //Correcto } int SisLocoMotor::arriba() { char c; c= 0x68; //ON Relà 4 if (!puerto->writeport(c)) { cambiaPuerto(); return 1; } c= 0x71; //OFF RelÃ3 if (!puerto->writeport(c)) { cambiaPuerto(); return 1; } usleep(9000); c= 0x71; //OFF Rel 3 if (!puerto->writeport(c)) { cambiaPuerto(); return 1; } c= 0x72; //OFF Rel 4 if (!puerto->writeport(c)) { cambiaPuerto(); return 1; } return 0; } int SisLocoMotor::abajo() { char c; c= 0x67; //ON Relà 3 if (!puerto->writeport(c)) { cambiaPuerto(); return 1; } c= 0x72; //OFF Relà 4 if (!puerto->writeport(c)) { cambiaPuerto(); return 1; } usleep(9000); c= 0x71; //OFF Rel 3 if (!puerto->writeport(c)) { cambiaPuerto(); return 1; } c= 0x72; //OFF Rel 4 if (!puerto->writeport(c)) { cambiaPuerto(); return 1; } return 0; //Correcto } int SisLocoMotor::izquierdacam() { char c; c= 0x69; //ON Relà 5 if (!puerto->writeport(c)) { cambiaPuerto(); return 1; } c= 0x74; //OFF Relà 6 if (!puerto->writeport(c)) { cambiaPuerto(); return 1; } usleep(9000); c= 0x73; //OFF Rel 5 if (!puerto->writeport(c)) { cambiaPuerto(); return 1; } c= 0x74; //OFF Rel 6 if (!puerto->writeport(c)) { cambiaPuerto(); return 1; } return 0; //Correcto } int SisLocoMotor::derechacam() { char c; c= 0x6A; //ON Relà 6 if (!puerto->writeport(c)) { cambiaPuerto(); return 1; } c= 0x73; //OFF Relà 5 if (!puerto->writeport(c)) { cambiaPuerto(); return 1; } usleep(9000); c= 0x73; //OFF Rel 5 if (!puerto->writeport(c)) { cambiaPuerto(); return 1; } c= 0x74; //OFF Rel 6 if (!puerto->writeport(c)) { cambiaPuerto(); return 1; } return 0; //Correcto } //sCmd[0]= 0x64; //ON Todos //sCmd[0]= 0x65; //1 //sCmd[0]= 0x66; //2 //sCmd[0]= 0x67; //3 //sCmd[0]= 0x68; //4 //sCmd[0]= 0x69; //5 //sCmd[0]= 0x6A; //6 //sCmd[0]= 0x6B; //7 //sCmd[0]= 0x6C; //8 // //sCmd[0]= 0x6E; //OFF Todos //sCmd[0]= 0x6F; //OFF 1 //sCmd[0]= 0x70; //OFF 2 //sCmd[0]= 0x71; //OFF 3 //sCmd[0]= 0x72; //OFF 4 //sCmd[0]= 0x73; //OFF 5 //sCmd[0]= 0x74; //OFF 6 //sCmd[0]= 0x75; //OFF 7 //sCmd[0]= 0x76; //OFF 8 ------------------------------------------------------------------- // Proyecto Robot Deimos 2000 // Creado por Carlos Boto Coto // Archivo sistemas/SisNeuronal.h #ifndef SISNEURONAL_H_ #define SISNEURONAL_H_ #include "SisLocoMotor.h" #include <string> using namespace std; class SisNeuronal { private: SisLocoMotor *sisLocoMotor; void removeAllWhite (std::string &str); public: SisNeuronal(); SisNeuronal(SisLocoMotor *sisMotor); virtual ~SisNeuronal(); int procesaComando(int* a); }; #endif /*SISNEURONAL_H_*/ ----------------------------------------------------------- // Proyecto Robot Deimos 2000 // Creado por Carlos Boto Coto // Archivo sistemas/SisNeuronal.cpp #include "SisNeuronal.h" #include <iostream> #include <string> #include "string.h" #include "stdio.h" using namespace std; SisNeuronal::SisNeuronal() { } SisNeuronal::~SisNeuronal() { } void SisNeuronal::removeAllWhite (std::string &str) { std::string temp; for (unsigned int i = 0; i < str.length(); i++) if (str[i] != '\n') temp += str[i]; str = temp; } SisNeuronal::SisNeuronal(SisLocoMotor *sisMotor) { sisLocoMotor=sisMotor; } int SisNeuronal::procesaComando(int *aAux) { if (aAux[0]==0) sisLocoMotor->para(); else if (aAux[0]==1) sisLocoMotor->avanza(); else if (aAux[0]==2) sisLocoMotor->retrocede(); if (aAux[1]==0) sisLocoMotor->recto(); else if (aAux[1]==1) sisLocoMotor->izquierda(); else if (aAux[1]==2) sisLocoMotor->derecha(); if (aAux[2]==2) sisLocoMotor->arriba(); else if (aAux[2]==1) sisLocoMotor->abajo(); if (aAux[3]==2) sisLocoMotor->izquierdacam(); else if (aAux[3]==1) sisLocoMotor->derechacam(); return 1; } ------------------------------------------------------------------------------- // Proyecto Robot Deimos 2000 // Creado por Carlos Boto Coto // Archivo sistemas/SisSensorial.h #ifndef SISSENSORIAL_H_ #define SISSENSORIAL_H_ #include "../drivers/ReceptorRadio.h" #include <string> class SisSensorial { public: SisSensorial(); virtual ~SisSensorial(); bool getEstimulo(char *sCmd); private: ReceptorRadio *receptorRadio; //ReceptorUltraSonid *radar; }; #endif /*SISSENSORIAL_H_*/ ------------------------------------------------ // Proyecto Robot Deimos 2000 // Creado por Carlos Boto Coto // Archivo sistemas/SisSensorial.cpp #include "SisSensorial.h" #include "../drivers/ReceptorRadio.h" #include <string> #include <iostream> using namespace std; SisSensorial::SisSensorial() { receptorRadio=new ReceptorRadio(); } SisSensorial::~SisSensorial() { delete receptorRadio; } bool SisSensorial::getEstimulo(char *sCmd) { //cout << "SisSensorial: Recogiendo estímulo: "<< sCmd <<"\n"; if (receptorRadio->getOrden(sCmd)) { //cout << *sCmd << "\n"; return true; }else return false; } ------------------------------------------------ // Proyecto Robot Deimos 2000 // Creado por Carlos Boto Coto // Archivo drivers/ReceptorRadio.h #ifndef RECEPTORRADIO_H_ #define RECEPTORRADIO_H_ #include <netinet/in.h> #include <stdio.h> using namespace std; class ReceptorRadio { public: ReceptorRadio(); virtual ~ReceptorRadio(); bool getOrden(char *sCmd); private: int m_sock; int m_conexion; FILE * m_fd; struct sockaddr_in m_server; int adrl; int readline(int fd, char *str, int maxlen); void obtieneHora(char*); int obtieneCobertura(); }; #endif /*RECEPTORRADIO_H_*/ ------------------------------------------------ // Proyecto Robot Deimos 2000 // Creado por Carlos Boto Coto // Archivo drivers/ReceptorRadio.cpp #include "ReceptorRadio.h" #include <sys/socket.h> #include <sys/types.h> #include <fcntl.h> #include <netinet/in.h> #include <arpa/inet.h> #include <netdb.h> #include <stdio.h> #include <stdlib.h> #include <ctype.h> #include <string.h> #include <unistd.h> /* UNIX standard function definitions */ #include <iostream> using namespace std; ReceptorRadio::ReceptorRadio() { int x; m_sock= socket(AF_INET, SOCK_STREAM, 0); if (m_sock < 0) { cout << "ReceptorRadio: No se puede crear socket\n"; return; } bzero(&m_server, sizeof(m_server)); m_server.sin_family = AF_INET; m_server.sin_addr.s_addr = INADDR_ANY; m_server.sin_port = htons(9000); x = bind(m_sock, (struct sockaddr *)&m_server, sizeof(m_server)); if (x<0) { //close(sock); cout << "ReceptorRadio: no se puede hacer bind del socket\n"; return; } if (listen(m_sock, 5) < 0) { cout << "no listen"; return; } adrl = sizeof (struct sockaddr_in); cout << "ReceptorRadio: Servidor activo\n"; m_conexion=-1; } ReceptorRadio::~ReceptorRadio() { close(m_conexion); } bool ReceptorRadio::getOrden(char *bufer) { int l=0; //cout << "ReceptorRadio: Esperando orden...\n"; if (m_conexion==-1) { m_conexion = accept(m_sock, (struct sockaddr *)&m_server, (socklen_t*) &adrl); //if (conexion == -1) return true; char sAux[255]; strcpy(sAux, "ROBOT DEIMOS V1.0. By Carlos Boto 2008.\r\n"); send(m_conexion, sAux, strlen(sAux), 0); cout << "ReceptorRadio: Conexion estableciza\n"; } //if((l=recv(conexion, bufer, sizeof(bufer),0))>0) if((l=readline(m_conexion, bufer, 80))>0) { char sAux[50]; char sHora[20]; obtieneHora(sHora); sprintf(sAux,"[%s] #%d# \r\n",sHora,obtieneCobertura()); send(m_conexion, sAux, strlen(sAux), 0); return true; } else { cout << "ReceptorRadio: Fin de la conexión\n"; close(m_conexion); m_conexion=-1; return true; } } int ReceptorRadio::readline(int fd, char *str, int maxlen) { int n; /* no. of chars */ int readcount; /* no. characters read */ char c; for (n = 1; n < maxlen; n++) { /* read 1 character at a time */ readcount = read(fd, &c, 1); /* store result in readcount */ if (readcount == 1) /* 1 char read? */ { *str = c; /* copy character to buffer */ str++; /* increment buffer index */ if (c == '\n') /* is it a newline character? */ break; /* then exit for loop */ } else if (readcount == 0) /* no character read? */ { if (n == 1) /* no character read? */ return (0); /* then return 0 */ else break; /* else simply exit loop */ } else return (-1); /* error in read() */ } *str=0; /* null-terminate the buffer */ return (n); /* return number of characters read */ } /* readline() */ void ReceptorRadio::obtieneHora(char *sHora) { time_t t; struct tm *stmh; t = time(NULL); stmh = localtime(&t); strftime(sHora,20,"%H:%M:%S",stmh); } int ReceptorRadio::obtieneCobertura() { m_fd=fopen("/proc/net/wireless","r"); char sCobertura[3]; fseek(m_fd,177, SEEK_SET); fgets(sCobertura,3,m_fd); sCobertura[2]=''; fclose(m_fd); return atoi(sCobertura); }
------------------------------------------------ // Proyecto Robot Deimos 2000 // Creado por Carlos Boto Coto // Archivo drivers/SerialDriver.h #ifndef SERIALDRIVER_H_ #define SERIALDRIVER_H_ class SerialDriver { public: SerialDriver(); SerialDriver(char *sDevice); virtual ~SerialDriver(); int writeport(char); int readport(char *result); private: int fd; int getbaud(int fd); int initport(int fd); }; #endif /*SERIALDRIVER_H_*/
------------------------------------------------ // Proyecto Robot Deimos 2000 // Creado por Carlos Boto Coto // Archivo drivers/SerialDriver.cpp #include "SerialDriver.h" #include <stdio.h> /* Standard input/output definitions */ #include <string.h> /* String function definitions */ #include <unistd.h> /* UNIX standard function definitions */ #include <fcntl.h> /* File control definitions */ #include <errno.h> /* Error number definitions */ #include <termios.h> /* POSIX terminal control definitions */ SerialDriver::SerialDriver() { } SerialDriver::~SerialDriver() { close(fd); } SerialDriver::SerialDriver(char *sDevice) { fd = open(sDevice, O_RDWR | O_NOCTTY | O_NDELAY | O_NONBLOCK); if (fd == -1) { perror("open_port: Unable to open /dev/ttyS0 - "); return; } else { fcntl(fd, F_SETFL, 0); } printf("baud=%d\n", getbaud(fd)); initport(fd); printf("baud=%d\n", getbaud(fd)); } int SerialDriver::initport(int fd) { struct termios options; // Get the current options for the port... tcgetattr(fd, &options); // Set the baud rates to 19200... cfsetispeed(&options, B19200); cfsetospeed(&options, B19200); // cfsetospeed(&options, B9600); // Enable the receiver and set local mode... options.c_cflag |= (CLOCAL | CREAD); /*options.c_cflag &= ~PARENB; options.c_cflag &= ~CSTOPB; options.c_cflag &= ~CSIZE; options.c_cflag |= CS8;*/ options.c_cflag = (options.c_cflag & ~CSIZE) | CS8; options.c_cflag = options.c_cflag | CSTOPB; options.c_cflag = options.c_cflag & ~(PARENB | PARODD); // Set the new options for the port... tcsetattr(fd, TCSANOW, &options); return 1; } /*int SerialDriver::writeport(char *chars) { int len = strlen(chars); chars[len] = 0x0d; // stick a <CR> after the command chars[len+1] = 0x00; // terminate the string properly int n = write(fd, chars, strlen(chars)); if (n < 0) { fputs("write failed!\n", stderr); return 0; } return 1; }*/ int SerialDriver::writeport(char c) { char sCmd[1]; sCmd[0]=c; int n=write(fd,sCmd,(size_t)(1)); if (n < 0) { fputs("write failed!\n", stderr); return 0; } return 1; } int SerialDriver::readport(char *result) { int iIn = read(fd, result, 254); result[iIn-1] = 0x00; if (iIn < 0) { if (errno == EAGAIN) { printf("SERIAL EAGAIN ERROR\n"); return 0; } else { printf("SERIAL read error %d %s\n", errno, strerror(errno)); return 0; } } return 1; } int SerialDriver::getbaud(int fd) { struct termios termAttr; int inputSpeed = -1; speed_t baudRate; tcgetattr(fd, &termAttr); /* Get the input speed. */ baudRate = cfgetispeed(&termAttr); switch (baudRate) { case B0: inputSpeed = 0; break; case B50: inputSpeed = 50; break; case B110: inputSpeed = 110; break; case B134: inputSpeed = 134; break; case B150: inputSpeed = 150; break; case B200: inputSpeed = 200; break; case B300: inputSpeed = 300; break; case B600: inputSpeed = 600; break; case B1200: inputSpeed = 1200; break; case B1800: inputSpeed = 1800; break; case B2400: inputSpeed = 2400; break; case B4800: inputSpeed = 4800; break; case B9600: inputSpeed = 9600; break; case B19200: inputSpeed = 19200; break; case B38400: inputSpeed = 38400; break; } return inputSpeed; }
Robot Deimos 2000 