00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022 #include "core/control/controlador.h"
00023 #include "lista.cc"
00024 #include "matriz4.h"
00025
00026 #include <string.h>
00027
00028
00029
00030
00031
00032 #define ESPERO(tipo, msg) \
00033 if ( tipo_token != (tipo) ) { \
00034 fprintf(stderr, "<CONTROLADOR> ERROR: Esperaba %s y recibi [%s].\n", \
00035 (msg), cad); fflush(stderr); return FALSE; \
00036 }
00037
00038 #define VERIFICAR(obj, cad) \
00039 if ( !(obj) ) { \
00040 fprintf(stderr, "<CONTROLADOR> - ERROR: " \
00041 "%s\n", (cad)); \
00042 fflush(stderr); \
00043 return FALSE; \
00044 }
00045
00046 #define CREAR_NOMBRE(s) \
00047 tipo_token = Sabiondo->siguiente_token(cad); \
00048 ESPERO(TK_CADENA, "una cadena"); \
00049 des_comille(cad); \
00050 (s) = new char[strlen(cad) + 1]; \
00051 VERIFICAR((s), "no hay memoria"); \
00052 strcpy((s), cad);
00053
00054 #define VERIFICAR_TIPO(ttt) \
00055 if ( (*sensores)[i]->tipo() != (ttt) ) { \
00056 fprintf(stderr, "<CONTROLADOR_DE_VUELO> ERROR: Type mismatch en %s.\n"\
00057 ,(*sensores)[i]->nombre()); \
00058 fflush(stderr); \
00059 return FALSE; \
00060 }
00061
00062 #define VERIFICAR_TIPOA(ttt) \
00063 if ( (*actuadores)[i]->tipo() != (ttt) ) { \
00064 fprintf(stderr, "<CONTROLADOR_DE_VUELO> ERROR: Type mismatch en %s.\n"\
00065 ,(*actuadores)[i]->nombre()); \
00066 fflush(stderr); \
00067 return FALSE; \
00068 }
00069
00070 #define COMPARAR_SENSOR(nomb, variable, tipo, tt) \
00071 if ( strcmp((nomb), (*sensores)[i]->nombre()) == 0 ) { \
00072 VERIFICAR_TIPO((tipo)); \
00073 (variable) = (tt*)((*sensores)[i]); \
00074 }
00075
00076 #define COMPARAR_ACTUADOR(nomb, variable, tipo, tt) \
00077 if ( strcmp((nomb), (*actuadores)[i]->nombre()) == 0 ) { \
00078 VERIFICAR_TIPOA((tipo)); \
00079 (variable) = ( tt* ) ( (*actuadores)[i]); \
00080 }
00081
00082
00083
00084
00085
00086 CONTROLADOR_BASICO_COPIA_E::CONTROLADOR_BASICO_COPIA_E()
00092 {
00093 Sensor_origen = NULL;
00094 Actuador_destino = NULL;
00095 Nombre_sensor_origen = NULL;
00096 Nombre_actuador_destino = NULL;
00097 }
00098
00099 BOOLEAN
00100 CONTROLADOR_BASICO_COPIA_E::leer(TOKENIZADOR *Sabiondo)
00107 {
00108 char cad[1000];
00109 int tipo_token = TK_DESCONOCIDO, pos;
00110
00111 pos = 1;
00112 while ( tipo_token != TK_CERRAR) {
00113 tipo_token = Sabiondo->siguiente_token(cad);
00114 switch ( pos ) {
00115 case 1: ESPERO(TK_ABRIR, "\"{\""); pos++; break;
00116 default:
00117 if ( tipo_token == TK_CERRAR ) break;
00118 ESPERO(TK_IDENTIFICADOR, "un identificador");
00119
00120 if ( strcmp(cad, "origen") == 0 ) {
00121 CREAR_NOMBRE(Nombre_sensor_origen);
00122 }
00123 else if ( strcmp(cad, "destino") == 0 ) {
00124 CREAR_NOMBRE(Nombre_actuador_destino);
00125 }
00126 else {
00127 fprintf(stderr,
00128 "<CONTROLADOR_BASICO_COPIA_E> - "
00129 "Parse error en %s\n", cad);
00130 fflush(stderr);
00131 return FALSE;
00132 }
00133 break;
00134 }
00135 }
00136
00137 return TRUE;
00138 }
00139
00140 BOOLEAN
00141 CONTROLADOR_BASICO_COPIA_E::resolver(LISTA<SENSOR *> *sensores,
00142 LISTA<ACTUADOR *> *actuadores)
00164 {
00165
00166 VERIFICAR(Nombre_sensor_origen, "Falta especificar sensor_origen");
00167 VERIFICAR(Nombre_actuador_destino,
00168 "Falta especificar actuador_destino");
00169
00170 int i;
00171
00172
00173 for ( i = 0; i < sensores->tam(); i++ ) {
00174 COMPARAR_SENSOR(Nombre_sensor_origen, Sensor_origen,
00175 T_FLOAT, SENSOR_FLOAT);
00176 }
00177
00178
00179 for ( i = 0; i < actuadores->tam(); i++ ) {
00180 COMPARAR_ACTUADOR(Nombre_actuador_destino, Actuador_destino,
00181 T_FLOAT, ACTUADOR_FLOAT);
00182 }
00183
00184
00185 VERIFICAR(Sensor_origen, "No se encuentra el origen FLOAT");
00186 VERIFICAR(Actuador_destino, "No se encuentra el destino FLOAT");
00187
00188 return TRUE;
00189 }
00190
00191 void
00192 CONTROLADOR_BASICO_COPIA_E::ejecutar(void)
00197 {
00198 double d;
00199
00200
00201 #ifdef NONONO
00202
00203 printf("<CONTROLADOR_BASICO_COPIA_E>:\n");
00204 printf("Sensores:\n");
00205 printf(" - Origen: %.2f\n", d);
00206 fflush(stdout);
00207 #endif
00208
00209
00210 d = Sensor_origen->evaluar();
00211
00212
00213 Actuador_destino->actuar(d);
00214 }
00215
00216
00217
00218
00219
00220 CONTROLADOR_BASICO_COPIA_V::CONTROLADOR_BASICO_COPIA_V()
00226 {
00227 Sensor_origen = NULL;
00228 Actuador_destino = NULL;
00229 Nombre_sensor_origen = NULL;
00230 Nombre_actuador_destino = NULL;
00231 }
00232
00233 BOOLEAN
00234 CONTROLADOR_BASICO_COPIA_V::leer(TOKENIZADOR *Sabiondo)
00241 {
00242 char cad[1000];
00243 int tipo_token = TK_DESCONOCIDO, pos;
00244
00245 pos = 1;
00246 while ( tipo_token != TK_CERRAR) {
00247 tipo_token = Sabiondo->siguiente_token(cad);
00248 switch ( pos ) {
00249 case 1: ESPERO(TK_ABRIR, "\"{\""); pos++; break;
00250 default:
00251 if ( tipo_token == TK_CERRAR ) break;
00252 ESPERO(TK_IDENTIFICADOR, "un identificador");
00253
00254 if ( strcmp(cad, "origen") == 0 ) {
00255 CREAR_NOMBRE(Nombre_sensor_origen);
00256 }
00257 else if ( strcmp(cad, "destino") == 0 ) {
00258 CREAR_NOMBRE(Nombre_actuador_destino);
00259 }
00260 else {
00261 fprintf(stderr,
00262 "<CONTROLADOR_BASICO_COPIA_V> - "
00263 "Parse error en %s\n", cad);
00264 fflush(stderr);
00265 return FALSE;
00266 }
00267 break;
00268 }
00269 }
00270
00271 return TRUE;
00272 }
00273
00274 BOOLEAN
00275 CONTROLADOR_BASICO_COPIA_V::resolver(LISTA<SENSOR *> *sensores,
00276 LISTA<ACTUADOR *> *actuadores)
00298 {
00299
00300 VERIFICAR(Nombre_sensor_origen, "Falta especificar sensor_origen");
00301 VERIFICAR(Nombre_actuador_destino,
00302 "Falta especificar actuador_destino");
00303
00304 int i;
00305
00306
00307 for ( i = 0; i < sensores->tam(); i++ ) {
00308 COMPARAR_SENSOR(Nombre_sensor_origen, Sensor_origen,
00309 T_VECTOR, SENSOR_VECTOR);
00310 }
00311
00312
00313 for ( i = 0; i < actuadores->tam(); i++ ) {
00314 COMPARAR_ACTUADOR(Nombre_actuador_destino, Actuador_destino,
00315 T_VECTOR, ACTUADOR_VECTOR);
00316 }
00317
00318
00319 VERIFICAR(Sensor_origen, "No se encuentra el origen VECTOR");
00320 VERIFICAR(Actuador_destino, "No se encuentra el destino VECTOR");
00321
00322 return TRUE;
00323 }
00324
00325 void
00326 CONTROLADOR_BASICO_COPIA_V::ejecutar(void)
00331 {
00332
00333 VECTOR d;
00334
00335 d = Sensor_origen->evaluar();
00336
00337
00338 Actuador_destino->actuar(d);
00339
00340
00341 #ifdef NONONO
00342
00343 printf("<CONTROLADOR_BASICO_COPIA_V>:\n");
00344 printf("Sensores:\n");
00345 printf(" - Origen: <%.2f, %.2f, %.2f>\n", d.x, d.y, d.z);
00346 fflush(stdout);
00347 #endif
00348 }
00349
00350
00351
00352
00353
00354 CONTROLADOR_DE_VUELO::CONTROLADOR_DE_VUELO()
00360 {
00361 Sensor_velocidad = NULL;
00362 Sensor_drapidez = NULL;
00363 Sensor_dyaw = NULL;
00364 Sensor_dpitch = NULL;
00365 Sensor_droll = NULL;
00366 Actuador_velocidad = NULL;
00367 Nombre_sensor_velocidad = NULL;
00368 Nombre_sensor_orientacion = NULL;
00369 Nombre_sensor_drapidez = NULL;
00370 Nombre_sensor_dyaw = NULL;
00371 Nombre_sensor_dpitch = NULL;
00372 Nombre_sensor_droll = NULL;
00373 Nombre_actuador_velocidad = NULL;
00374 }
00375
00376 BOOLEAN
00377 CONTROLADOR_DE_VUELO::leer(TOKENIZADOR *Sabiondo)
00384 {
00385 char cad[1000];
00386 int tipo_token = TK_DESCONOCIDO, pos;
00387
00388 pos = 1;
00389 while ( tipo_token != TK_CERRAR) {
00390 tipo_token = Sabiondo->siguiente_token(cad);
00391 switch ( pos ) {
00392 case 1: ESPERO(TK_ABRIR, "\"{\""); pos++; break;
00393 default:
00394 if ( tipo_token == TK_CERRAR ) break;
00395 ESPERO(TK_IDENTIFICADOR, "un identificador");
00396
00397 if ( strcmp(cad, "velocidad_in") == 0 ) {
00398 CREAR_NOMBRE(Nombre_sensor_velocidad);
00399 }
00400 else if ( strcmp(cad, "orientacion_in") == 0 ) {
00401 CREAR_NOMBRE(Nombre_sensor_orientacion);
00402 }
00403 else if ( strcmp(cad, "delta_rapidez") == 0 ) {
00404 CREAR_NOMBRE(Nombre_sensor_drapidez);
00405 }
00406 else if ( strcmp(cad, "delta_yaw") == 0 ) {
00407 CREAR_NOMBRE(Nombre_sensor_dyaw);
00408 }
00409 else if ( strcmp(cad, "delta_pitch") == 0 ) {
00410 CREAR_NOMBRE(Nombre_sensor_dpitch);
00411 }
00412 else if ( strcmp(cad, "delta_roll") == 0 ) {
00413 CREAR_NOMBRE(Nombre_sensor_droll);
00414 }
00415 else if ( strcmp(cad, "velocidad_out") == 0 ) {
00416 CREAR_NOMBRE(Nombre_actuador_velocidad);
00417 }
00418 else if ( strcmp(cad, "orientacion_out") == 0 ) {
00419 CREAR_NOMBRE(Nombre_actuador_orientacion);
00420 }
00421 else {
00422 fprintf(stderr,
00423 "<CONTROLADOR_DE_VUELO> - Parse error en %s\n", cad);
00424 fflush(stderr);
00425 return FALSE;
00426 }
00427 break;
00428 }
00429 }
00430
00431 return TRUE;
00432 }
00433
00434 BOOLEAN
00435 CONTROLADOR_DE_VUELO::resolver(LISTA<SENSOR *> *sensores,
00436 LISTA<ACTUADOR *> *actuadores)
00458 {
00459
00460 VERIFICAR(Nombre_sensor_velocidad, "Falta especificar velocidad");
00461 VERIFICAR(Nombre_sensor_orientacion, "Falta especificar orientacion");
00462 VERIFICAR(Nombre_sensor_drapidez, "Falta especificar acelerador");
00463 VERIFICAR(Nombre_sensor_dyaw, "Falta especificar delta_yaw");
00464 VERIFICAR(Nombre_sensor_dpitch, "Falta especificar delta_pitch");
00465 VERIFICAR(Nombre_sensor_droll, "Falta especificar delta_roll");
00466 VERIFICAR(Nombre_actuador_velocidad,
00467 "Falta especificar actuador_velocidad");
00468 VERIFICAR(Nombre_actuador_orientacion,
00469 "Falta especificar actuador_orientacion");
00470
00471 int i;
00472
00473
00474 for ( i = 0; i < sensores->tam(); i++ ) {
00475 COMPARAR_SENSOR(Nombre_sensor_velocidad, Sensor_velocidad,
00476 T_VECTOR, SENSOR_VECTOR);
00477 COMPARAR_SENSOR(Nombre_sensor_orientacion, Sensor_orientacion,
00478 T_VECTOR,SENSOR_VECTOR);
00479 COMPARAR_SENSOR(Nombre_sensor_drapidez, Sensor_drapidez,
00480 T_FLOAT, SENSOR_FLOAT);
00481 COMPARAR_SENSOR(Nombre_sensor_dyaw, Sensor_dyaw,
00482 T_FLOAT, SENSOR_FLOAT);
00483 COMPARAR_SENSOR(Nombre_sensor_dpitch, Sensor_dpitch,
00484 T_FLOAT, SENSOR_FLOAT);
00485 COMPARAR_SENSOR(Nombre_sensor_droll, Sensor_droll,
00486 T_FLOAT, SENSOR_FLOAT);
00487 }
00488
00489
00490 for ( i = 0; i < actuadores->tam(); i++ ) {
00491 COMPARAR_ACTUADOR(Nombre_actuador_velocidad, Actuador_velocidad,
00492 T_VECTOR, ACTUADOR_VECTOR);
00493 COMPARAR_ACTUADOR(Nombre_actuador_orientacion, Actuador_orientacion,
00494 T_VECTOR, ACTUADOR_VECTOR);
00495 }
00496
00497
00498 VERIFICAR(Sensor_velocidad, "No se encuentra la velocidad");
00499 VERIFICAR(Sensor_drapidez, "No se encuentra la aceleracion");
00500 VERIFICAR(Sensor_dyaw, "No se encuentra delta_yaw");
00501 VERIFICAR(Sensor_dpitch, "No se encuentra delta pitch");
00502 VERIFICAR(Sensor_droll, "No se encuentra delta_roll");
00503 VERIFICAR(Actuador_velocidad, "No se encuentra el motor");
00504 VERIFICAR(Actuador_orientacion, "No se encuentran los alerones");
00505
00506 return TRUE;
00507 }
00508
00509 void
00510 CONTROLADOR_DE_VUELO::ejecutar(void)
00541 {
00542
00543 VECTOR v, o;
00544 double delta_rapidez, delta_yaw, delta_pitch, delta_roll;
00545
00546 v = Sensor_velocidad->evaluar();
00547 o = Sensor_orientacion->evaluar();
00548 delta_yaw = Sensor_dyaw->evaluar();
00549 delta_pitch = Sensor_dpitch->evaluar();
00550 delta_roll = Sensor_droll->evaluar();
00551 delta_rapidez = Sensor_drapidez->evaluar();
00552
00553
00554 VECTOR nueva_v(0.01, 0.01, 0.01);
00555 VECTOR nueva_o;
00556 MATRIZ_4x4 R, delta_R;
00557 VECTOR front, right, up;
00558 double nueva_rapidez;
00559
00560
00561 R.rotacion_angulos_euler(o.x, o.y, o.z);
00562
00563
00564 up.x = R.M[0][2]; up.y = R.M[1][2]; up.z = R.M[2][2];
00565 delta_R.rotacion_eje(DEG2RAD(-3) * delta_yaw, up.x, up.y, up.z);
00566 R = delta_R * R;
00567
00568
00569 right.x = R.M[0][0]; right.y = R.M[1][0]; right.z = R.M[2][0];
00570 delta_R.rotacion_eje(DEG2RAD(5)*delta_roll, right.x, right.y, right.z);
00571 R = delta_R * R;
00572
00573
00574 front.x = R.M[0][1]; front.y = R.M[1][1]; front.z = R.M[2][1];
00575 delta_R.rotacion_eje(DEG2RAD(3)*delta_pitch, front.x, front.y, front.z);
00576 R = delta_R * R;
00577
00578
00579 R.exportar_angulos_euler(&nueva_o.x, &nueva_o.y, &nueva_o.z);
00580
00581
00582 nueva_v.y = -R.M[0][1]; nueva_v.x = R.M[1][1]; nueva_v.z = R.M[2][1];
00583 nueva_v.normalizar();
00584 nueva_rapidez = v.norma() + delta_rapidez/10;
00585 if ( nueva_rapidez < 0 ) nueva_rapidez = 0;
00586 if ( nueva_rapidez > 1 ) nueva_rapidez = 1;
00587 nueva_v = nueva_v * nueva_rapidez;
00588
00589
00590 Actuador_velocidad->actuar(nueva_v);
00591 Actuador_orientacion->actuar(nueva_o);
00592
00593
00594 #ifdef NONONO
00595 clrscr();
00596 printf("<CONTROLADOR_DE_VUELO>:\n");
00597 printf("Sensores del mundo virtual:\n");
00598 printf(" - Velocidad: <%.2f, %.2f, %.2f>\n", v.x, v.y, v.z);
00599 printf(" - Orientacion: <%.2f, %.2f, %.2f>\n", o.x, o.y, o.z);
00600 printf("Sensores del mundo real:\n");
00601 printf(" - Aceleracion: %.2f\n", delta_rapidez);
00602 printf(" - Delta yaw: %.2f\n", delta_yaw);
00603 printf(" - Delta pitch: %.2f\n", delta_pitch);
00604 printf(" - Delta roll: %.2f\n", delta_roll);
00605 fflush(stdout);
00606 #endif
00607 }
00608
00609
00610
00611
00612