base_motor_driver.cpp
Go to the documentation of this file.
00001 
00031 #include "base_motor_driver.h"
00032 
00034 
00035 BaseMotor::BaseMotor() :
00036     motor_order("M\n\r"), _corrected_data(0), _distance1(0.0), _distance2(0.0), _before_distance1(0.0),
00037     _before_distance2(0.0), _fd_motor1(-1), _fd_motor2(-1)
00038 {
00039 
00040 }
00041 
00043 
00044 BaseMotor::~BaseMotor()
00045 {
00046 }
00047 
00049 
00050 int BaseMotor::init_communication()
00051 {
00052     int result;
00053     struct termios newtio1, newtio2;
00054     char DEFINE0[] = "HO\n\r";
00055 
00056     struct sembuf sem_in[] = {0, -1, 0};
00057     struct sembuf sem_out[] = {0, 1, 0};
00058 
00059     // Semaforo para el puerto de comuniciaciones
00060     int semFD;
00061 
00062     _fd_motor1 = open(DEVICE_NAME1, O_RDWR | O_NOCTTY);
00063     if (_fd_motor1 < 0) {
00064         ROS_INFO("[BASE_MOTOR_DRIVER] Error while opening puerto serie 0 del motor %s:'%s'", DEVICE_NAME1,
00065                  strerror(errno));
00066         return (1);
00067     }
00068 
00069     _fd_motor2 = open(DEVICE_NAME2, O_RDWR | O_NOCTTY);
00070     if (_fd_motor2 < 0) {
00071         ROS_INFO("[BASE_MOTOR_DRIVER] Error while opening puerto serie 0 del motor %s:'%s'", DEVICE_NAME2,
00072                  strerror(errno));
00073         return (1);
00074     }
00075 
00076     // MOTOR 1
00077     bzero((void *) &newtio1, sizeof(newtio1));
00078 
00079     // Velocidad, paridad, stop, bit, echo ..
00080     newtio1.c_cflag = BAUDRATE | CS8 | CLOCAL | CREAD;
00081     newtio1.c_iflag = IGNPAR | ICRNL;
00082     newtio1.c_oflag = 0;
00083     newtio1.c_lflag = ECHO | ICANON;
00084 
00085     // Caracteres de control
00086     newtio1.c_cc[VINTR] = 0;
00087     newtio1.c_cc[VQUIT] = 0;
00088     newtio1.c_cc[VERASE] = 0;
00089     newtio1.c_cc[VKILL] = 0;
00090     newtio1.c_cc[VEOF] = 4;
00091     newtio1.c_cc[VSWTC] = 0;
00092     newtio1.c_cc[VSTART] = 0;
00093     newtio1.c_cc[VSTOP] = 0;
00094     newtio1.c_cc[VSUSP] = 0;
00095     newtio1.c_cc[VEOL] = 0;
00096     newtio1.c_cc[VREPRINT] = 0;
00097     newtio1.c_cc[VDISCARD] = 0;
00098     newtio1.c_cc[VWERASE] = 0;
00099     newtio1.c_cc[VLNEXT] = 0;
00100     newtio1.c_cc[VEOL2] = 0;
00101 
00102     // Bloqueo por caracter con tiempo de espera infinito.
00103     newtio1.c_cc[VTIME] = 0;
00104     newtio1.c_cc[VMIN] = 1;
00105 
00106     // Activacion de configuracion.
00107     tcflush(_fd_motor1, TCIFLUSH);
00108     result = tcsetattr(_fd_motor1, TCSANOW, &newtio1);
00109 
00110     if (result) {
00111         ROS_INFO("[BASE_MOTOR_DRIVER] Configuando puerto serie del motor %s:'%s'", DEVICE_NAME1, strerror(errno));
00112         return (2);
00113     }
00114 
00115     // MOTOR 2
00116     bzero((void *) &newtio2, sizeof(newtio2));
00117 
00118     // Velocidad, paridad, stop, bit, echo ..
00119     newtio2.c_cflag = BAUDRATE | CS8 | CLOCAL | CREAD;
00120     newtio2.c_iflag = IGNPAR | ICRNL;
00121     newtio2.c_oflag = 0;
00122     newtio2.c_lflag = ECHO | ICANON;
00123 
00124     // Caracteres de control
00125     newtio2.c_cc[VINTR] = 0;
00126     newtio2.c_cc[VQUIT] = 0;
00127     newtio2.c_cc[VERASE] = 0;
00128     newtio2.c_cc[VKILL] = 0;
00129     newtio2.c_cc[VEOF] = 4;
00130     newtio2.c_cc[VSWTC] = 0;
00131     newtio2.c_cc[VSTART] = 0;
00132     newtio2.c_cc[VSTOP] = 0;
00133     newtio2.c_cc[VSUSP] = 0;
00134     newtio2.c_cc[VEOL] = 0;
00135     newtio2.c_cc[VREPRINT] = 0;
00136     newtio2.c_cc[VDISCARD] = 0;
00137     newtio2.c_cc[VWERASE] = 0;
00138     newtio2.c_cc[VLNEXT] = 0;
00139     newtio2.c_cc[VEOL2] = 0;
00140 
00141     // Bloqueo por caracter con tiempo de espera infinito.
00142     newtio2.c_cc[VTIME] = 0;
00143     newtio2.c_cc[VMIN] = 1;
00144 
00145     // Activacion de configuracion.
00146     tcflush(_fd_motor2, TCIFLUSH);
00147     result = tcsetattr(_fd_motor2, TCSANOW, &newtio2);
00148 
00149     if (result) {
00150         ROS_INFO("[BASE_MOTOR_DRIVER] Configuando puerto serie del motor %s:'%s'", DEVICE_NAME2, strerror(errno));
00151         return (2);
00152     }
00153 
00154     // create the key SEMFDID for the semaphore
00155     int returnValue = access(SEM_FILENAME, F_OK);
00156     if (returnValue != 0) {
00157         ROS_DEBUG("[BASE_MOTOR_DRIVER] access() returned an error %i='%s'", returnValue, strerror(errno));
00158         if (creat(SEM_FILENAME, S_IRUSR | S_IWUSR) == -1) {
00159             ROS_INFO("[BASE_MOTOR_DRIVER] ACCESS:Error creando el fichero de claves:'%s'", strerror(errno));
00160         }
00161     }
00162     SEMFDID = ftok(SEM_FILENAME, 'S');
00163     if (SEMFDID == (key_t) -1) {
00164         ROS_INFO("[BASE_MOTOR_DRIVER] Errro while creating the key SEMFDID:'%s'", strerror(errno));
00165         exit(EXIT_FAILURE);
00166     }
00167 
00168     // Creacion del semaforo de control de acceso del puerto de comunicaciones.
00169     ROS_DEBUG("[BASE_MOTOR_DRIVER] Creacion del semaforo de control de acceso del puerto de comunicaciones:SEMFDID:%i",
00170               SEMFDID);
00171     semFD = semget(SEMFDID, 1, IPC_CREAT | 0666);
00172     if (semFD < 0) {
00173         ROS_INFO("[BASE_MOTOR_DRIVER] Errro while creating el semaforo para el puerto de comunicaciones:semFD:%i, '%s'",
00174                  semFD, strerror(errno));
00175         return (3);
00176     }
00177 
00178     returnValue = semctl(semFD, 0, SETVAL, 1);
00179     if (returnValue != 0) {
00180         ROS_INFO("[BASE_MOTOR_DRIVER] Error while semctl:semFD:%i, '%s'", semFD, strerror(errno));
00181     }
00182 
00183     bzero((void *) &_current_cinematic, sizeof(cinematic_data));
00184     bzero((void *) &_last_cinematic, sizeof(cinematic_data));
00185     bzero((void *) &_current_estimated, sizeof(cinematic_data));
00186     bzero((void *) &_before_estimated, sizeof(cinematic_data));
00187 
00188     _distance1 = _distance2 = _before_distance1 = _before_distance2 = 0.0;
00189 
00190     // INCIO ZONA EXCLUSIVA
00191 
00192     semctl(semFD, 0, SETVAL, 1);
00193 
00194     ROS_DEBUG("[BASE_MOTOR_DRIVER] SEM en inicioComunicacionMotores: %d", semctl( semFD, 0,GETVAL,0));
00195 
00196     semop(semFD, sem_in, 1);
00197 
00198     write(_fd_motor1, DEFINE0, 4);
00199     write(_fd_motor2, DEFINE0, 4);
00200     ROS_DEBUG("[BASE_MOTOR_DRIVER] Enviado M1: '%s' Enviado M2: '%s'", DEFINE0, DEFINE0);
00201 
00202     write(_fd_motor1, ACCELERATION, 6);
00203     write(_fd_motor2, ACCELERATION, 6);
00204     ROS_DEBUG("[BASE_MOTOR_DRIVER] Enviado M1: '%s' Enviado M2: '%s'", ACCELERATION, ACCELERATION);
00205 
00206     write(_fd_motor1, DECELERATION, 7);
00207     write(_fd_motor2, DECELERATION, 7);
00208     ROS_DEBUG("[BASE_MOTOR_DRIVER] Enviado M1: '%s' Enviado M2: '%s'", DECELERATION, DECELERATION);
00209 
00210     write(_fd_motor1, VELOCITY_MAX, 8);
00211     write(_fd_motor2, VELOCITY_MAX, 8);
00212     ROS_DEBUG("[BASE_MOTOR_DRIVER] Enviado M1: '%s' Enviado M2: '%s'", VELOCITY_MAX, VELOCITY_MAX);
00213 
00214 #ifdef DEBUG_ODO
00215     char buf1[64];
00216     int res;
00217     int i;
00218     static const char POSM1[] = "GSP\n\r";
00219     bzero((void *) buf1, 64);
00220     write(_fd_motor1, POSM1, 5);
00221     ROS_DEBUG("[BASE_MOTOR_DRIVER] Leido: ");
00222     i = 0;
00223     res = read(_fd_motor1, buf1, 62);
00224     read(_fd_motor1, &buf1[63], 1);
00225     ROS_DEBUG("[BASE_MOTOR_DRIVER] Velocidad máxima :'%s':%d", buf1, res);
00226 #endif
00227 
00228     // FIN ZONA EXCLUSIVA
00229     semop(semFD, sem_out, 1);
00230 
00231     // El puerto acepta la configuracion y el semaforo esestavo.
00232     return (0);
00233 }
00234 
00236 
00237 int BaseMotor::end_communication()
00238 {
00239     int semFD;
00240     struct sembuf sem_in[] = {0, -1, 0};
00241     struct sembuf arg[] = {0, 0, 0};
00242     char buf[] = "V0\n\r";
00243 
00244     if ((semFD = semget(SEMFDID, 1, 0)) == -1) {
00245         ROS_INFO("[BASE_MOTOR_DRIVER] finComunicacionesMotores semget:'%s'", strerror(errno));
00246         return (1);
00247     }
00248 
00249     // INCIO ZONA EXCLUSIVA
00250     ROS_DEBUG("[BASE_MOTOR_DRIVER] SEM en finComunicacionMotores: %d", semctl( semFD, 0,GETVAL,0));
00251     semop(semFD, sem_in, 1);
00252 
00253     write(_fd_motor1, buf, 4);
00254     write(_fd_motor2, buf, 4);
00255     ROS_DEBUG("[BASE_MOTOR_DRIVER] Enviado M1: '%s' Enviado M2: '%s'", buf, buf);
00256 
00257     // FIN ZONA EXCLUSIVA
00258 
00259     close(_fd_motor1);
00260     close(_fd_motor2);
00261 
00262     /* remove it: */
00263     if (semctl(semFD, 0, IPC_RMID, arg) == -1) {
00264         ROS_INFO("[BASE_MOTOR_DRIVER] finComunicacionesMotores semctl:'%s'", strerror(errno));
00265 
00266         return (2);
00267     }
00268     ROS_DEBUG("[BASE_MOTOR_DRIVER] Saliendo de fin comunicaciones");
00269 
00270     return 0;
00271 }
00272 
00274 
00275 int BaseMotor::enable_motors()
00276 {
00277     int r;
00278     if ((r = enable_disable_motor(&_fd_motor1, "EN\r")) != 0) {
00279         ROS_INFO("[BASE_MOTOR_DRIVER] Error habilitando motor 1! Error %d", r);
00280         return r;
00281     }
00282     if ((r = enable_disable_motor(&_fd_motor2, "EN\r")) != 0) {
00283         ROS_INFO("[BASE_MOTOR_DRIVER] Error habilitando motor 2! Error %d", r);
00284         return r;
00285     }
00286 
00287     return 0;
00288 }
00289 
00291 
00292 int BaseMotor::disable_motors()
00293 {
00294     int r;
00295     if ((r = enable_disable_motor(&_fd_motor1, "DI\r")) != 0) {
00296         ROS_INFO("[BASE_MOTOR_DRIVER] Error inhabilitando motor 1! Error %d", r);
00297         return r;
00298     }
00299     if ((r = enable_disable_motor(&_fd_motor2, "DI\r")) != 0) {
00300         ROS_INFO("[BASE_MOTOR_DRIVER] Error inhabilitando motor 2! Error %d", r);
00301         return r;
00302     }
00303 
00304     return 0;
00305 }
00306 
00308 
00309 int BaseMotor::enable_disable_motor(int *fd, const char *order)
00310 {
00311     char bufOut[64], bufIn[64];
00312     // size 4 because commands to send: EN\n\r or DI\n\r
00313     char comando[4];
00314     const char comandoGST[] = "GST\n\r";
00315     struct sembuf sem_in[] = {0, -1, 0};
00316     struct sembuf sem_out[] = {0, 1, 0};
00317     int res, l, semFD;
00318     // times to try to send the command
00319     int intentos = 3;
00320     char c;
00321 
00322     if (strlen(order) > 3) {
00323         ROS_INFO("[BASE_MOTOR_DRIVER] Error en la longitud del comando EN/DI");
00324         return -3;
00325     }
00326     bzero((void *) comando, 4);
00327     strcat(comando, order);
00328     if (comando[2] != '\r') {
00329         comando[2] = '\r';
00330     }
00331     l = strlen(comando);
00332 
00333     bzero((void *) bufOut, 64);
00334     bzero((void *) bufIn, 64);
00335 
00336     if ((semFD = semget(SEMFDID, 1, 0)) < 0) {
00337         ROS_INFO("[BASE_MOTOR_DRIVER] Error adquiriendo el semaforo en enable_disable_motor:'%s'", strerror(errno));
00338         return (-4);
00339     }
00340 
00341     while(intentos >= 0) {
00342         intentos--;
00343         l = strlen(comando);
00344 
00345         // escribimos el comando al driver
00346         if (semop(semFD, sem_in, 1) != 0) {
00347             ROS_INFO("[BASE_MOTOR_DRIVER] Error inicio zona exclusiva write enable_disable_motor -> (%i) \"'%s'\"",
00348                      errno, strerror(errno));
00349             return -4;
00350         }
00351 
00352         res = write(*fd, comando, l);
00353         if (res != l) {
00354             ROS_INFO("[BASE_MOTOR_DRIVER] Error escribiendo a _fd_motor1: escritos %d bytes de %d", res, l);
00355             ROS_DEBUG("[BASE_MOTOR_DRIVER] (%i) \"'%s'\"/n/n", errno, strerror(errno));
00356             continue;
00357         }
00358 
00359         // nos aseguramos que lo ha escrito correctamente preguntando al driver su estado...
00360         l = strlen(comandoGST);
00361         res = write(*fd, comandoGST, l);
00362         if (res != l) {
00363             ROS_INFO("[BASE_MOTOR_DRIVER] Error escribiendo a _fd_motor1: escritos %d bytes de %d", res, l);
00364             ROS_DEBUG("[BASE_MOTOR_DRIVER] (%i) \"'%s'\"", errno, strerror(errno));
00365             continue;
00366         }
00367 
00368         // reading the answer
00369 
00370         res = read(*fd, bufIn, 62);
00371         read(*fd, &bufIn[63], 1);
00372         if (semop(semFD, sem_out, 1) != 0) {
00373             ROS_INFO("[BASE_MOTOR_DRIVER] Error fin zona exclusiva read enable_disable_motor -> (%i) \"'%s'\"", errno,
00374                      strerror(errno));
00375             return -4;
00376         }
00377 
00378         if (res < SIZE_REPORT_GST) {
00379             ROS_INFO("[BASE_MOTOR_DRIVER] Error leyendo a fd: leidos %d bytes", res);
00380             ROS_DEBUG("[BASE_MOTOR_DRIVER] (%i) \"'%s'\"", errno, strerror(errno));
00381             continue;
00382         }
00383 
00384         // en c se guarda el estado del driver
00385         c = bufIn[3];
00386         // encuentra "EN" => hemos mandado habilitar el motor
00387         if (strstr(comando, "EN") != NULL) {
00388             // operación realizada correctamente
00389             if (c == '1') {
00390                 return 0;
00391             }
00392             else {
00393                 ROS_DEBUG("[BASE_MOTOR_DRIVER] Intento fallido. Restan %d intentos", intentos);
00394             }
00395         }
00396         // encuentra "DI" => hemos mandado inhabilitar el motor
00397         else if (strstr(comando, "DI") != NULL) {
00398             // operación realizada correctamente
00399             if (c == '0') {
00400                 return 0;
00401             }
00402             else {
00403                 ROS_DEBUG("[BASE_MOTOR_DRIVER] Intento fallido. Restan %d intentos", intentos);
00404             }
00405         }
00406         // hemos mandado una cosa rara
00407         else {
00408             ROS_INFO("[BASE_MOTOR_DRIVER] Error comprobando el éxito de nuestro comando inapropiado: \"'%s'\"",
00409                      comando);
00410             return -3;
00411         }
00412     }
00413 
00414     ROS_DEBUG("[BASE_MOTOR_DRIVER] ¡INTENTOS AGOTADOS!");
00415     return -2;
00416 }
00417 
00419 
00420 int BaseMotor::read_data(cinematic_data* data)
00421 {
00422     return read_data_variable_time_diff(data, PERIOD);
00423 }
00424 
00426 
00427 int BaseMotor::read_data_variable_time_diff(cinematic_data *data, double time_since_last_call)
00428 {
00429     static const char GNM1[] = "GN\n\r";
00430     static const char GNM2[] = "GN\n\r";
00431     static const char POSM1[] = "POS\n\r";
00432     static const char POSM2[] = "POS\n\r";
00433 
00434     char buf1[64], buf2[64], buf3[64], buf4[64];
00435 
00436     struct sembuf sem_in[] = {0, -1, 0};
00437     struct sembuf sem_out[] = {0, 1, 0};
00438 
00439     double v, w, deltaD1, deltaD2, deltaD, deltaTheta;
00440     int semFD;
00441 
00442     bzero((void *) buf1, 64);
00443     bzero((void *) buf2, 64);
00444     bzero((void *) buf3, 64);
00445     bzero((void *) buf4, 64);
00446 
00447     if ((semFD = semget(SEMFDID, 1, 0)) < 0) {
00448         ROS_INFO("[BASE_MOTOR_DRIVER] Adquiriendo el semaforo en leeDatosMotores:'%s'", strerror(errno));
00449         return (1);
00450     }
00451 
00452     // INCIO ZONA EXCLUSIVA
00453     ROS_DEBUG("[BASE_MOTOR_DRIVER] SEM en leeDatosMotores: %d", semctl( semFD, 0,GETVAL,0));
00454     int returnValue = semop(semFD, sem_in, 1);
00455     if (returnValue != 0) {
00456         ROS_INFO("[BASE_MOTOR_DRIVER] Error while semop:'%s'", strerror(errno));
00457     }
00458 
00459     // POS
00460     ROS_DEBUG("[BASE_MOTOR_DRIVER] Sending POSM1:'%s'", POSM1);
00461     write(_fd_motor1, POSM1, 5);
00462 
00463     // buf1 :ticks
00464     read(_fd_motor1, buf1, 62);
00465     read(_fd_motor1, &buf1[63], 1);
00466     ROS_DEBUG("[BASE_MOTOR_DRIVER] Received buf1:'%s'", buf1);
00467 
00468     ROS_DEBUG("[BASE_MOTOR_DRIVER] Sending POSM2:'%s'", POSM2);
00469     write(_fd_motor2, POSM2, 5);
00470 
00471     // buf2 :ticks
00472     read(_fd_motor2, buf2, 62);
00473     read(_fd_motor2, &buf2[63], 1);
00474     ROS_DEBUG("[BASE_MOTOR_DRIVER] Received buf2:'%s'", buf2);
00475 
00476     // VELOCIDAD
00477     ROS_DEBUG("[BASE_MOTOR_DRIVER] Sending GNM1:'%s'", GNM1);
00478     write(_fd_motor1, GNM1, 4);
00479 
00480     // buf3 :RPM (radians / sec)
00481     read(_fd_motor1, buf3, 62);
00482     read(_fd_motor1, &buf3[63], 1);
00483     ROS_DEBUG("[BASE_MOTOR_DRIVER] Received buf3:'%s'", buf3);
00484 
00485     ROS_DEBUG("[BASE_MOTOR_DRIVER] Sending GNM2:'%s'", GNM2);
00486     write(_fd_motor2, GNM2, 4);
00487 
00488     // buf4 :RPM (radians / sec)
00489     read(_fd_motor2, buf4, 62);
00490     read(_fd_motor2, &buf4[63], 1);
00491     ROS_DEBUG("[BASE_MOTOR_DRIVER] Received buf4:'%s'", buf4);
00492 
00493     semop(semFD, sem_out, 1);
00494 
00495     // PULSES_PER_REVOLUTION                 ticks -> 1 rotation of motor
00496     // so PULSES_PER_REVOLUTION * REDUCTION  ticks -> 1 rotation of wheel ( 2 PI )
00497     // so if we have buf1 ticks, the wheel angle is
00498     // angle = (buf1 * 2 PI) / (PULSES_PER_REVOLUTION * REDUCTION)
00499 
00500     // and so the distance is
00501     // d = angle * radius
00502     //   = (buf1 * 2 PI) / (PULSES_PER_REVOLUTION * REDUCTION) * radius
00503     //   = (diameter * buf1 * PI) / (PULSES_PER_REVOLUTION * REDUCTION)
00504     _before_distance1 = _distance1;
00505     // dimension check: (tick * no_dim * mm) / (tick * no_dim * no_dim) = mm
00506     _distance1 = (atof(buf1) * M_PI * WHEEL_DIAMETER) / (PULSES_PER_REVOLUTION * REDUCTION);
00507     _before_distance2 = _distance2;
00508     _distance2 = (atof(buf2) * M_PI * WHEEL_DIAMETER) / (PULSES_PER_REVOLUTION * REDUCTION);
00509 
00510     deltaD1 = _distance1 - _before_distance1; // mm
00511     deltaD2 = _distance2 - _before_distance2; // mm
00512 
00513     deltaD = (deltaD1 + deltaD2) * 0.5; // mm
00514     deltaTheta = (deltaD2 - deltaD1) / (AXIS_LENGTH); // no_dim
00515 
00516     // mm . sec-1
00517     v = deltaD / time_since_last_call;
00518 
00519     // rad. sec-1
00520     w = deltaTheta / time_since_last_call;
00521 
00522     _current_cinematic.theta = _last_cinematic.theta + deltaTheta;
00523     _current_cinematic.x = _last_cinematic.x + deltaD * cos(_current_cinematic.theta);
00524     _current_cinematic.y = _last_cinematic.y + deltaD * sin(_current_cinematic.theta);
00525     _current_cinematic.v = v;
00526     _current_cinematic.w = w;
00527 
00528     ROS_DEBUG("[BASE_MOTOR_DRIVER] x:%.4f mm   y:%.4f mm   theta:%.4f rad   v:%.4f  mm.sec-1   w:%.4f rad.sec-1",
00529               _current_cinematic.x, _current_cinematic.y, _current_cinematic.theta, _current_cinematic.v,
00530               _current_cinematic.w);
00531 
00532     // copy actual -> anterior
00533     _last_cinematic.theta = _current_cinematic.theta;
00534     _last_cinematic.x = _current_cinematic.x;
00535     _last_cinematic.y = _current_cinematic.y;
00536     _last_cinematic.v = _current_cinematic.v;
00537     _last_cinematic.w = _current_cinematic.w;
00538 
00539     // copy actual -> data
00540     data->theta = _current_cinematic.theta;
00541     data->x = _current_cinematic.x;
00542     data->y = _current_cinematic.y;
00543     data->v = _current_cinematic.v;
00544     data->w = _current_cinematic.w;
00545 
00546     // convertimos a grados
00547     data->w = data->w * 360.0 * (0.5) / M_PI; // deg . sec-1
00548     data->theta = data->theta * 360.0 * (0.5) / M_PI; // deg
00549 
00550     ROS_DEBUG("[BASE_MOTOR_DRIVER] data: x=%.4f y=%.4f  theta=%.4f grados, v=%.4f w=%.4f", data->x, data->y,
00551               data->theta, data->v, data->w);
00552 
00553     if (_corrected_data) {
00554         _current_estimated.theta = _before_estimated.theta + deltaTheta;
00555         _current_estimated.x = _before_estimated.x + deltaD * cos(_current_estimated.theta);
00556         _current_estimated.y = _before_estimated.y + deltaD * sin(_current_estimated.theta);
00557         _current_estimated.v = v;
00558         _current_estimated.w = w;
00559 
00560         _before_estimated.theta = _current_estimated.theta;
00561         _before_estimated.x = _current_estimated.x;
00562         _before_estimated.y = _current_estimated.y;
00563         _before_estimated.v = _current_estimated.v;
00564         _before_estimated.w = _current_estimated.w;
00565     }
00566     else {
00567         _current_estimated.theta = _current_cinematic.theta;
00568         _current_estimated.x = _current_cinematic.x;
00569         _current_estimated.y = _current_cinematic.y;
00570         _current_estimated.v = v;
00571         _current_estimated.w = w;
00572 
00573         _before_estimated.theta = _last_cinematic.theta;
00574         _before_estimated.x = _last_cinematic.x;
00575         _before_estimated.y = _last_cinematic.y;
00576         _before_estimated.v = _last_cinematic.v;
00577         _before_estimated.w = _last_cinematic.w;
00578     }
00579 
00580     return (0);
00581 }
00582 
00584 
00585 int BaseMotor::set_velocity(double v, double w)
00586 {
00587     char buf1[64], buf2[64];
00588     long int vm1, vm2;
00589     int l1, l2;
00590     int semFD;
00591     struct sembuf sem_in[] = {0, -1, 0};
00592     struct sembuf sem_out[] = {0, 1, 0};
00593 
00594     // conversion w a rad/seg
00595     w = w * 2.0 * M_PI / (360.0);
00596 
00597     // Modificados los signos para ajustar esquema con player
00598     double v1 = v - w * AXIS_LENGTH * 0.5;
00599     double v2 = v + w * AXIS_LENGTH * 0.5;
00600 
00601     // El 60 es para llevar segundos a minutos
00602     vm1 = (long int) ((v1 * 60 * REDUCTION) / (M_PI * WHEEL_DIAMETER));
00603     vm2 = (long int) ((v2 * 60 * REDUCTION) / (M_PI * WHEEL_DIAMETER));
00604     ROS_DEBUG_NAMED("base_motor_driver", "[BASE_MOTOR_DRIVER] Motor instructions: %li, %li", vm1, vm2);
00605 
00606     bzero((void *) buf1, 64);
00607     bzero((void *) buf2, 64);
00608 
00609     sprintf(buf1, "V%ld\n\r", vm1);
00610     sprintf(buf2, "V%ld\n\r", vm2);
00611 
00612     // find the end of the buf1 buffer
00613     for (l1 = 0; (buf1[l1] != '\0') && (l1 < 63); l1++) {
00614     }
00615     // find the end of the buf2 buffer
00616     for (l2 = 0; (buf2[l2] != '\0') && (l2 < 63); l2++) {
00617     }
00618 
00619     if ((semFD = semget(SEMFDID, 1, 0)) < 0) {
00620         ROS_INFO("[BASE_MOTOR_DRIVER] Adquiriendo el semaforo en leeDatosMotores:'%s'", strerror(errno));
00621         return (1);
00622     }
00623 
00624     // INCIO ZONA EXCLUSIVA
00625     ROS_DEBUG_NAMED("base_motor_driver", "[BASE_MOTOR_DRIVER] SEM en colocaVelocidadMotores: %d",
00626                     semctl( semFD, 0,GETVAL,0));
00627     semop(semFD, sem_in, 1);
00628 
00629     // recuperamos la velocidad máxima
00630     write(_fd_motor1, VELOCITY_MAX, 9);
00631     write(_fd_motor2, VELOCITY_MAX, 9);
00632     ROS_DEBUG_NAMED("base_motor_driver", "[BASE_MOTOR_DRIVER] Enviado M1: '%s' Enviado M2: '%s'", VELOCITY_MAX,
00633                     VELOCITY_MAX);
00634 
00635     write(_fd_motor1, buf1, l1);
00636     write(_fd_motor2, buf2, l2);
00637     ROS_DEBUG_NAMED("base_motor_driver", "[BASE_MOTOR_DRIVER] Enviado M1: '%s' Enviado M2: '%s'", buf1, buf2);
00638 
00639     int returnValue = semop(semFD, sem_out, 1);
00640     if (returnValue != 0) {
00641         ROS_INFO("[BASE_MOTOR_DRIVER] Error while semop:semFD:%i, '%s'", semFD, strerror(errno));
00642     }
00643 
00644     ROS_DEBUG_NAMED("base_motor_driver", "[BASE_MOTOR_DRIVER] v %f w %f vm1 %ld vm2 %ld, v * w: %f", v, w, vm1, vm2,
00645                     v * w);
00646 
00647     return (0);
00648 }
00649 
00651 
00652 int BaseMotor::set_displacement_velocity(double d, double v, double w)
00653 {
00654     char buf[64], buf1[64], buf2[64];
00655     long int dm;
00656     int l, l1, l2;
00657     double v1, v2;
00658     long int vm1, vm2;
00659     int grados;
00660     int semFD;
00661     struct sembuf sem_in[] = {0, -1, 0};
00662     struct sembuf sem_out[] = {0, 1, 0};
00663     cinematic_data dVelocidad;
00664 
00665     // Desplazamiento:
00666     grados = (int) ((d * PULSES_PER_REVOLUTION * REDUCTION) / (360));
00667     dm = (long int) (grados * 0.713);
00668     bzero((void *) buf, 64);
00669     sprintf(buf, "LR%ld\n\r", dm);
00670     for (l = 0; (buf[l] != '\0') && (l < 63); l++)
00671         ;
00672 
00673     // Velocidad:
00674     w = w * 2.0 * M_PI / (360.0);    // convertimos w a rad/seg
00675 
00676     // Cambia los signos de la velocidad para ajustar con player
00677     v1 = v - w * AXIS_LENGTH * 0.5;
00678     v2 = v + w * AXIS_LENGTH * 0.5;
00679     vm1 = (long int) ((v1 * 60 * REDUCTION) / (M_PI * WHEEL_DIAMETER));    // El 60 es para llevar segundos a minutos
00680     vm2 = (long int) ((v2 * 60 * REDUCTION) / (M_PI * WHEEL_DIAMETER));
00681     bzero((void *) buf1, 64);
00682     bzero((void *) buf2, 64);
00683     sprintf(buf1, "SP%ld\n\r", vm1);
00684     sprintf(buf2, "SP%ld\n\r", vm2);
00685     for (l1 = 0; (buf1[l1] != '\0') && (l1 < 63); l1++)
00686         ;
00687     for (l2 = 0; (buf2[l2] != '\0') && (l2 < 63); l2++)
00688         ;
00689 
00690     if ((semFD = semget(SEMFDID, 1, 0)) < 0) {
00691         ROS_INFO("[BASE_MOTOR_DRIVER] Adquiriendo el semaforo en leeDatosMotores:'%s'", strerror(errno));
00692         return (1);
00693     }
00694 
00695     // INCIO ZONA EXCLUSIVA
00696     ROS_DEBUG("[BASE_MOTOR_DRIVER] SEM en colocaVelocidadDesplazamiento: %d", semctl( semFD, 0,GETVAL,0));
00697     semop(semFD, sem_in, 1);
00698 
00699     //Ajustamos las velocidades máximas
00700     write(_fd_motor1, buf1, l1);
00701     write(_fd_motor2, buf2, l2);
00702     ROS_DEBUG("[BASE_MOTOR_DRIVER] Enviado M1: '%s' Enviado M2: '%s'", buf1, buf2);
00703 
00704     //Indicamos cuanto va a moverse
00705     write(_fd_motor1, buf, l);
00706     write(_fd_motor2, buf, l);
00707     ROS_DEBUG("[BASE_MOTOR_DRIVER] Enviado M1: '%s' Enviado M2: '%s'", buf, buf);
00708 
00709     //Comienza a moverse
00710     write(_fd_motor1, motor_order.c_str(), 3);
00711     write(_fd_motor2, motor_order.c_str(), 3);
00712     ROS_DEBUG("[BASE_MOTOR_DRIVER] Enviado M1: '%s' Enviado M2: '%s'", motor_order.c_str(), motor_order.c_str());
00713 
00714     semop(semFD, sem_out, 1);
00715 
00716     ROS_DEBUG("[BASE_MOTOR_DRIVER] colocaVelocidadDesplazamiento");
00717     ROS_DEBUG("[BASE_MOTOR_DRIVER] d=%f  dm=%ld l=%i ", d, dm, l);
00718     ROS_DEBUG("[BASE_MOTOR_DRIVER] v %f w %f v1 %.4f v2 %.4f vm1 %ld vm2 %ld", v, w, v1, v2, vm1, vm2);
00719 
00720     return (0);
00721 }
00722 
00724 
00725 int BaseMotor::move_ahead(double distance)
00726 {
00727     char buf[64];
00728     long int dm;
00729     int l;
00730     int semFD;
00731     struct sembuf sem_in[] = {0, -1, 0};
00732     struct sembuf sem_out[] = {0, 1, 0};
00733 
00734     dm = (long int) ((distance * PULSES_PER_REVOLUTION * REDUCTION) / (M_PI * WHEEL_DIAMETER));
00735 
00736     bzero((void *) buf, 64);
00737 
00738     sprintf(buf, "LR%ld\n\r", dm);
00739 
00740     for (l = 0; (buf[l] != '\0') && (l < 63); l++)
00741         ;
00742 
00743     if ((semFD = semget(SEMFDID, 1, 0)) < 0) {
00744         ROS_INFO("[BASE_MOTOR_DRIVER] Adquiriendo el semaforo en leeDatosMotores:'%s'", strerror(errno));
00745         return (1);
00746     }
00747 
00748     // INCIO ZONA EXCLUSIVA
00749     ROS_DEBUG("[BASE_MOTOR_DRIVER] SEM en moverLineaRecta: %d", semctl( semFD, 0,GETVAL,0));
00750     semop(semFD, sem_in, 1);
00751 
00752     write(_fd_motor1, VELOCITY_MAX, 9);
00753     write(_fd_motor2, VELOCITY_MAX, 9);
00754     ROS_DEBUG("[BASE_MOTOR_DRIVER] Enviado M1: '%s' Enviado M2: '%s'", VELOCITY_MAX, VELOCITY_MAX);
00755 
00756     write(_fd_motor1, buf, l);
00757     write(_fd_motor2, buf, l);
00758     ROS_DEBUG("[BASE_MOTOR_DRIVER] Enviado M1: '%s' Enviado M2: '%s'", buf, buf);
00759 
00760     write(_fd_motor1, motor_order.c_str(), 3);
00761     write(_fd_motor2, motor_order.c_str(), 3);
00762     ROS_DEBUG("[BASE_MOTOR_DRIVER] Enviado M1: '%s' Enviado M2: '%s'", motor_order.c_str(), motor_order.c_str());
00763 
00764     semop(semFD, sem_out, 1);
00765 
00766     ROS_DEBUG("[BASE_MOTOR_DRIVER] distance: %f, dm: %ld, l: %i ", distance, dm, l);
00767 
00768     return (0);
00769 }
00770 
00772 
00773 int BaseMotor::relative_turn(double theta)
00774 {
00775     char buf1[64], buf2[64];
00776     char buf3[] = "M\n\r";
00777     long int dm;
00778     int l1, l2;
00779     int semFD;
00780     struct sembuf sem_in[] = {0, -1, 0};
00781     struct sembuf sem_out[] = {0, 1, 0};
00782     cinematic_data dVelocidad;
00783 
00784     ROS_DEBUG("[BASE_MOTOR_DRIVER] Motores: Me piden girar %f grados", theta);
00785 
00786     theta = theta * REDUCTION * PULSES_PER_REVOLUTION / 360.0;
00787 
00788     dm = (long int) theta * AXIS_LENGTH / WHEEL_DIAMETER;
00789     bzero((void *) buf1, 64);
00790     bzero((void *) buf2, 64);
00791 
00792     sprintf(buf1, "LR%ld\n\r", (-1) * dm);
00793     sprintf(buf2, "LR%ld\n\r", dm);
00794 
00795     for (l1 = 0; (buf1[l1] != '\0') && (l1 < 63); l1++)
00796         ;
00797     for (l2 = 0; (buf2[l2] != '\0') && (l2 < 63); l2++)
00798         ;
00799 
00800     if ((semFD = semget(SEMFDID, 1, 0)) < 0) {
00801         ROS_INFO("[BASE_MOTOR_DRIVER] Adquiriendo el semaforo en leeDatosMotores:'%s'", strerror(errno));
00802         return (1);
00803     }
00804 
00805     // INCIO ZONA EXCLUSIVA
00806     ROS_DEBUG("[BASE_MOTOR_DRIVER] SEM en giroRelativo: %d", semctl( semFD, 0,GETVAL,0));
00807     semop(semFD, sem_in, 1);
00808 
00809     write(_fd_motor1, VELOCITY_MAX, 9);
00810     write(_fd_motor2, VELOCITY_MAX, 9);
00811     ROS_DEBUG("[BASE_MOTOR_DRIVER] Enviado M1: '%s' Enviado M2: '%s'", VELOCITY_MAX, VELOCITY_MAX);
00812 
00813     write(_fd_motor1, buf1, l1);
00814     write(_fd_motor2, buf2, l2);
00815     ROS_DEBUG("[BASE_MOTOR_DRIVER] Enviado M1: '%s' Enviado M2: '%s'", buf1, buf2);
00816 
00817     write(_fd_motor2, buf3, 3);
00818     write(_fd_motor1, buf3, 3);
00819     ROS_DEBUG("[BASE_MOTOR_DRIVER] Enviado M1: '%s' Enviado M2: '%s'", buf3, buf3);
00820 
00821     semop(semFD, sem_out, 1);
00822 
00823     ROS_DEBUG("[BASE_MOTOR_DRIVER] theta %.4f   dm %ld l1 %i b1 '%s' b2 '%s' '%s'", theta, dm, l1, buf1, buf2, buf3);
00824 
00825     return (0);
00826 }
00827 
00829 
00830 int BaseMotor::reset_odometry()
00831 {
00832     char DEFINE0[] = "HO\n\r";
00833     int semFD;
00834     struct sembuf sem_in[] = {0, -1, 0};
00835     struct sembuf sem_out[] = {0, 1, 0};
00836     if ((semFD = semget(SEMFDID, 1, 0)) < 0) {
00837         ROS_INFO("[BASE_MOTOR_DRIVER] Adquiriendo el semaforo en resetOdometry:'%s'", strerror(errno));
00838         return (1);
00839     }
00840 
00841     bzero((void *) &_current_cinematic, sizeof(cinematic_data));
00842     bzero((void *) &_last_cinematic, sizeof(cinematic_data));
00843     _distance1 = _distance2 = _before_distance1 = _before_distance2 = 0.0;
00844 
00845     ROS_DEBUG("[BASE_MOTOR_DRIVER] SEM en resetOdometry: %d", semctl(semFD, 0,GETVAL,0));
00846 
00847     // INCIO ZONA EXCLUSIVA
00848     semop(semFD, sem_in, 1);
00849     write(_fd_motor1, DEFINE0, 4);
00850     write(_fd_motor2, DEFINE0, 4);
00851     ROS_DEBUG("[BASE_MOTOR_DRIVER] Enviado M1: '%s' Enviado M2: '%s'", DEFINE0, DEFINE0);
00852     // FIN ZONA EXCLUSIVA
00853     semop(semFD, sem_out, 1);
00854 
00855     return (0);
00856 }
00857 
00859 
00860 int BaseMotor::update_odometry(double x, double y, double theta)
00861 {
00862     char DEFINE0[] = "HO\n\r";
00863     int semFD;
00864     struct sembuf sem_in[] = {0, -1, 0};
00865     struct sembuf sem_out[] = {0, 1, 0};
00866 
00867     if ((semFD = semget(SEMFDID, 1, 0)) < 0) {
00868         ROS_INFO("[BASE_MOTOR_DRIVER] Adquiriendo el semaforo en actualizarOdometria:'%s'", strerror(errno));
00869         return (1);
00870     }
00871 
00872     _distance1 = _distance2 = _before_distance1 = _before_distance2 = 0.0;
00873 
00874     _current_cinematic.x = x;
00875     _current_cinematic.y = y;
00876     _current_cinematic.theta = theta;
00877     // guess v and w are global variables
00878 
00879     _last_cinematic.x = _current_cinematic.x;
00880     _last_cinematic.y = _current_cinematic.y;
00881     _last_cinematic.theta = _current_cinematic.theta;
00882 
00883     ROS_DEBUG("[BASE_MOTOR_DRIVER] SEM en resetOdometry: %d", semctl( semFD, 0,GETVAL,0));
00884 
00885     // start exclusive zone
00886     semop(semFD, sem_in, 1);
00887 
00888     write(_fd_motor1, DEFINE0, 4);
00889     write(_fd_motor2, DEFINE0, 4);
00890 
00891     ROS_DEBUG("[BASE_MOTOR_DRIVER] Enviado M1: '%s' Enviado M2: '%s'", DEFINE0, DEFINE0);
00892 
00893     semop(semFD, sem_out, 1);
00894     // end exclusive zone
00895 
00896     return (0);
00897 }
00898 
00900 
00901 cinematic_data * BaseMotor::get_estimate_position()
00902 {
00903     cinematic_data *data;
00904 
00905     data->x = _current_estimated.x;
00906     data->y = _current_estimated.y;
00907     data->theta = _current_estimated.theta;
00908     data->v = _current_estimated.v;
00909     data->w = _current_estimated.w;
00910 
00911     return data;
00912 }
00913 
00915 
00916 int BaseMotor::set_estimate_position(double x, double y, double theta)
00917 {
00918     _current_estimated.x = x;
00919     _current_estimated.y = y;
00920     _current_estimated.theta = theta;
00921     // guess v and w are global variables
00922 
00923     _before_estimated.x = _current_estimated.x;
00924     _before_estimated.y = _current_estimated.y;
00925     _before_estimated.theta = _current_estimated.theta;
00926     _corrected_data = 1;
00927 
00928     return (0);
00929 }
00930 


base
Author(s): Raul Perula-Martinez
autogenerated on Wed Apr 1 2015 10:17:15