trunk/src/main.cpp

   1
   2 #include "main.h"
   3 #include <iostream>
   4
   5 //
   6 void CMiEntorno::inicializar()
   7 {
   8         // Inicializo el Entorno
   9         datos.add("robot.sensor_adelante",      1) ;
  10         datos.add("robot.sensor_atras",         0) ;
  11         datos.add("robot.sensor_derecha",       0) ;
  12         datos.add("robot.sensor_izquierda", 0) ;
  13         datos.add("robot.avanzo_adelante",      0) ;
  14         datos.add("robot.avanzo_atras",         0) ;
  15         datos.add("robot.avanzo_derecha",       0) ;
  16         datos.add("robot.avanzo_izquierda", 0) ;
  17 /*
  18         // Inicializo el Entorno
  19         datos.add("robot.avanzo_en_x", 0) ;
  20         // old datos.add("robot.avanzo_en_y", 0) ;
  21         // old datos.add("robot.avanzo_en_z", 0) ;
  22
  23         datos.add("robot.sensor_1", 0) ;
  24         datos.add("robot.sensor_2", 0) ;
  25         datos.add("robot.sensor_3", 0) ;
  26         datos.add("robot.sensor_4", 0) ; // new
  27 */
  28 }
  29
  30
  31 void CMiEntorno::actualizar()
  32 {
  33         datos.find("robot.avanzo_adelante")             = 0 ;
  34         datos.find("robot.avanzo_atras")                = 0 ;
  35         datos.find("robot.avanzo_derecha")              = 0 ;
  36         datos.find("robot.avanzo_izquierda")            = 0 ;
  37         datos.find("robot.sensor_adelante")             = 0 ;
  38         datos.find("robot.sensor_atras")                = 0 ;
  39         datos.find("robot.sensor_derecha")              = 0 ;
  40         datos.find("robot.sensor_izquierda")            = 0 ;
  41
  42 /*
  43         datos.find("robot.sensor_1") = sensor_1 ;
  44         datos.find("robot.sensor_2") = sensor_2 ;
  45         datos.find("robot.sensor_3") = sensor_3 ;
  46         datos.find("robot.sensor_4") = 20 ; // new
  47
  48         datos.find("robot.avanzo_en_x") = avanzo_en_x?1:0 ;
  49         datos.find("robot.avanzo_en_y") = avanzo_en_y?1:0 ;
  50         datos.find("robot.avanzo_en_z") = avanzo_en_z?1:0 ;
  51 */
  52
  53 }
  54
  55
  56
  57 //
  58 int main(int argc, char* argv[])
  59 {
  60         //
  61         CMiEntorno*                     e = new CMiEntorno;
  62         CSistemaAutonomo                a ;
  63
  64         e->inicializar() ;
  65
  66 /* OLD
  67         // Inicializo las teorias
  68         CTeoria t1("Avanzar_X_1", "AvanzarX", 1, 1) ;
  69         t1.datos_iniciales.add ("robot.sensor_1", 0) ;
  70         t1.datos_iniciales.add ("robot.sensor_2", ANY) ;
  71         t1.datos_iniciales.add ("robot.sensor_3", ANY) ;
  72         t1.datos_iniciales.add ("robot.avanzo_en_x", ANY) ;
  73         t1.datos_iniciales.add ("robot.avanzo_en_y", ANY) ;
  74         t1.datos_iniciales.add ("robot.avanzo_en_z", ANY) ;
  75         t1.datos_finales.add ("robot.avanzo_en_x", 1) ;
  76         t1.datos_finales.add ("robot.avanzo_en_y", ANY) ;
  77         t1.datos_finales.add ("robot.avanzo_en_z", ANY) ;
  78         t1.datos_finales.add ("robot.sensor_1", ANY) ;
  79         t1.datos_finales.add ("robot.sensor_2", ANY) ;
  80         t1.datos_finales.add ("robot.sensor_3", ANY) ;
  81
  82         CTeoria t2("Avanzar_Y_1", "AvanzarY", 1, 1) ;
  83         t2.datos_iniciales.add ("robot.sensor_1", ANY) ;
  84         t2.datos_iniciales.add ("robot.sensor_2", 0) ;
  85         t2.datos_iniciales.add ("robot.sensor_3", ANY) ;
  86         t2.datos_iniciales.add ("robot.avanzo_en_x", ANY) ;
  87         t2.datos_iniciales.add ("robot.avanzo_en_y", ANY) ;
  88         t2.datos_iniciales.add ("robot.avanzo_en_z", ANY) ;
  89         t2.datos_finales.add ("robot.avanzo_en_x", 0) ;
  90         t2.datos_finales.add ("robot.avanzo_en_y", 1) ;
  91         t2.datos_finales.add ("robot.avanzo_en_z", 0) ;
  92         t2.datos_finales.add ("robot.sensor_1", ANY) ;
  93         t2.datos_finales.add ("robot.sensor_2", ANY) ;
  94         t2.datos_finales.add ("robot.sensor_3", ANY) ;
  95
  96         CTeoria t3("Avanzar_Z_1", "AvanzarZ", 1, 1) ;
  97         t3.datos_iniciales.add ("robot.sensor_1", ANY) ;
  98         t3.datos_iniciales.add ("robot.sensor_2", ANY) ;
  99         t3.datos_iniciales.add ("robot.sensor_3", 0) ;
 100         t3.datos_iniciales.add ("robot.avanzo_en_x", ANY) ;
 101         t3.datos_iniciales.add ("robot.avanzo_en_y", ANY) ;
 102         t3.datos_iniciales.add ("robot.avanzo_en_z", ANY) ;
 103         t3.datos_finales.add ("robot.avanzo_en_x", 0) ;
 104         t3.datos_finales.add ("robot.avanzo_en_y", 0) ;
 105         t3.datos_finales.add ("robot.avanzo_en_z", 1) ;
 106         t3.datos_finales.add ("robot.sensor_1", ANY) ;
 107         t3.datos_finales.add ("robot.sensor_2", ANY) ;
 108         t3.datos_finales.add ("robot.sensor_3", ANY) ;
 109 */
 110
 111 /*
 112         // Inicializo las teorias
 113         CTeoria t1("Avanzar_X_1", "AvanzarX", 1, 1) ;
 114         t1.datos_iniciales.add ("robot.sensor_1", ANY) ;
 115         t1.datos_iniciales.add ("robot.sensor_2", ANY) ;
 116         t1.datos_iniciales.add ("robot.sensor_3", ANY) ;
 117         t1.datos_iniciales.add ("robot.sensor_4", ANY) ;
 118         t1.datos_iniciales.add ("robot.avanzo_en_x", ANY) ;
 119         t1.datos_finales.add ("robot.avanzo_en_x", ANY) ;
 120         t1.datos_finales.add ("robot.sensor_1", ANY) ;
 121         t1.datos_finales.add ("robot.sensor_2", ANY) ;
 122         t1.datos_finales.add ("robot.sensor_3", ANY) ;
 123         t1.datos_finales.add ("robot.sensor_4", ANY) ;
 124
 125         // Inicializo las teorias
 126         CTeoria t2("Avanzar_X_2", "AvanzarX", 1, 1) ;
 127         t2.datos_iniciales.add ("robot.sensor_1", ANY) ;
 128         t2.datos_iniciales.add ("robot.sensor_2", ANY) ;
 129         t2.datos_iniciales.add ("robot.sensor_3", ANY) ;
 130         t2.datos_iniciales.add ("robot.sensor_4", ANY) ;
 131         t2.datos_iniciales.add ("robot.avanzo_en_x", ANY) ;
 132         t2.datos_finales.add ("robot.avanzo_en_x", ANY) ;
 133         t2.datos_finales.add ("robot.sensor_1", ANY) ;
 134         t2.datos_finales.add ("robot.sensor_2", ANY) ;
 135         t2.datos_finales.add ("robot.sensor_3", ANY) ;
 136         t2.datos_finales.add ("robot.sensor_4", ANY) ;
 137
 138
 139         a.teorias.add(t1.nombre.c_str(), t1) ;
 140         a.teorias.add(t2.nombre.c_str(), t2) ;
 141         // OLD a.teorias.add(t3.nombre.c_str(), t3) ;
 142 #ifdef DEBUG
 143         std::cout << "Agrega teoria " << t1.nombre << ":\n";
 144         std::cout << "  .datos_iniciales:\n" << t1.datos_iniciales << "\n";
 145         std::cout << "  .datos_finales:\n" << t1.datos_finales << "\n";
 146         std::cout << "Agrega teoria " << t2.nombre << ":\n";
 147         std::cout << "  .datos_iniciales:\n" << t2.datos_iniciales << "\n";
 148         std::cout << "  .datos_finales:\n" << t2.datos_finales << "\n";
 149 #endif // DEBUG
 150
 151 */
 152
 153         // Inicializo las teorias
 154         CTeoria t1("Avanzar", "avanzar", 1, 1) ;
 155         t1.datos_iniciales.add ("robot.sensor_adelante",        0) ;
 156         t1.datos_iniciales.add ("robot.sensor_atras",           ANY) ;
 157         t1.datos_iniciales.add ("robot.sensor_derecha",         ANY) ;
 158         t1.datos_iniciales.add ("robot.sensor_izquierda",       ANY) ;
 159         t1.datos_iniciales.add ("robot.avanzo_adelante",        ANY) ;
 160         t1.datos_iniciales.add ("robot.avanzo_atras",           ANY) ;
 161         t1.datos_iniciales.add ("robot.avanzo_derecha",         ANY) ;
 162         t1.datos_iniciales.add ("robot.avanzo_izquierda",       ANY) ;
 163         t1.datos_finales.add ("robot.sensor_adelante",          ANY) ;
 164         t1.datos_finales.add ("robot.sensor_atras",             ANY) ;
 165         t1.datos_finales.add ("robot.sensor_derecha",           ANY) ;
 166         t1.datos_finales.add ("robot.sensor_izquierda",         ANY) ;
 167         t1.datos_finales.add ("robot.avanzo_adelante",          1) ;
 168         t1.datos_finales.add ("robot.avanzo_atras",             0) ;
 169         t1.datos_finales.add ("robot.avanzo_derecha",           0) ;
 170         t1.datos_finales.add ("robot.avanzo_izquierda",         0) ;
 171
 172
 173         CTeoria t2("Retroceder", "retroceder", 1, 1) ;
 174         t2.datos_iniciales.add ("robot.sensor_adelante",        ANY) ;
 175         t2.datos_iniciales.add ("robot.sensor_atras",           0) ;
 176         t2.datos_iniciales.add ("robot.sensor_derecha",         ANY) ;
 177         t2.datos_iniciales.add ("robot.sensor_izquierda",       ANY) ;
 178         t2.datos_iniciales.add ("robot.avanzo_adelante",        ANY) ;
 179         t2.datos_iniciales.add ("robot.avanzo_atras",           ANY) ;
 180         t2.datos_iniciales.add ("robot.avanzo_derecha",         ANY) ;
 181         t2.datos_iniciales.add ("robot.avanzo_izquierda",       ANY) ;
 182         t2.datos_finales.add ("robot.sensor_adelante",          ANY) ;
 183         t2.datos_finales.add ("robot.sensor_atras",             ANY) ;
 184         t2.datos_finales.add ("robot.sensor_derecha",           ANY) ;
 185         t2.datos_finales.add ("robot.sensor_izquierda",         ANY) ;
 186         t2.datos_finales.add ("robot.avanzo_adelante",          0) ;
 187         t2.datos_finales.add ("robot.avanzo_atras",             1) ;
 188         t2.datos_finales.add ("robot.avanzo_derecha",           0) ;
 189         t2.datos_finales.add ("robot.avanzo_izquierda",         0) ;
 190
 191
 192         CTeoria t3("Derecha", "derecha", 1, 1) ;
 193         t3.datos_iniciales.add ("robot.sensor_adelante",        ANY) ;
 194         t3.datos_iniciales.add ("robot.sensor_atras",           ANY) ;
 195         t3.datos_iniciales.add ("robot.sensor_derecha",         0) ;
 196         t3.datos_iniciales.add ("robot.sensor_izquierda",       ANY) ;
 197         t3.datos_iniciales.add ("robot.avanzo_adelante",        ANY) ;
 198         t3.datos_iniciales.add ("robot.avanzo_atras",           ANY) ;
 199         t3.datos_iniciales.add ("robot.avanzo_derecha",         ANY) ;
 200         t3.datos_iniciales.add ("robot.avanzo_izquierda",       ANY) ;
 201         t3.datos_finales.add ("robot.sensor_adelante",          ANY) ;
 202         t3.datos_finales.add ("robot.sensor_atras",             ANY) ;
 203         t3.datos_finales.add ("robot.sensor_derecha",           ANY) ;
 204         t3.datos_finales.add ("robot.sensor_izquierda",         ANY) ;
 205         t3.datos_finales.add ("robot.avanzo_adelante",          0) ;
 206         t3.datos_finales.add ("robot.avanzo_atras",             0) ;
 207         t3.datos_finales.add ("robot.avanzo_derecha",           1) ;
 208         t3.datos_finales.add ("robot.avanzo_izquierda",         0) ;
 209
 210
 211         CTeoria t4("Eludir.1", "derecha", 1, 1) ;
 212         t4.datos_iniciales.add ("robot.sensor_adelante",        1) ;
 213         t4.datos_iniciales.add ("robot.sensor_atras",           ANY) ;
 214         t4.datos_iniciales.add ("robot.sensor_derecha",         0) ;
 215         t4.datos_iniciales.add ("robot.sensor_izquierda",       ANY) ;
 216         t4.datos_iniciales.add ("robot.avanzo_adelante",        ANY) ;
 217         t4.datos_iniciales.add ("robot.avanzo_atras",           ANY) ;
 218         t4.datos_iniciales.add ("robot.avanzo_derecha",         ANY) ;
 219         t4.datos_iniciales.add ("robot.avanzo_izquierda",       ANY) ;
 220         t4.datos_finales.add ("robot.sensor_adelante",          0) ;
 221         t4.datos_finales.add ("robot.sensor_atras",             ANY) ;
 222         t4.datos_finales.add ("robot.sensor_derecha",           ANY) ;
 223         t4.datos_finales.add ("robot.sensor_izquierda",         ANY) ;
 224         t4.datos_finales.add ("robot.avanzo_adelante",          0) ;
 225         t4.datos_finales.add ("robot.avanzo_atras",             ANY) ;
 226         t4.datos_finales.add ("robot.avanzo_derecha",           1) ;
 227         t4.datos_finales.add ("robot.avanzo_izquierda",         ANY) ;
 228
 229
 230         a.teorias.add(t1.nombre, &t1) ;
 231         a.teorias.add(t2.nombre, &t2) ;
 232         a.teorias.add(t3.nombre, &t3) ;
 233         a.teorias.add(t4.nombre, &t4) ;
 234 #ifdef DEBUG
 235         std::cout << "Agrega teoria: " << t1 << "\n";
 236         std::cout << "\tdatos_iniciales:\n" << t1.datos_iniciales << "\n";
 237         std::cout << "\tdatos_finales:\n" << t1.datos_finales << "\n";
 238         std::cout << "Agrega teoria: " << t2 << "\n";
 239         std::cout << "\tdatos_iniciales:\n" << t2.datos_iniciales << "\n";
 240         std::cout << "\tdatos_finales:\n" << t2.datos_finales << "\n";
 241         std::cout << "Agrega teoria: " << t3 << "\n";
 242         std::cout << "\tdatos_iniciales:\n" << t3.datos_iniciales << "\n";
 243         std::cout << "\tdatos_finales:\n" << t3.datos_finales << "\n";
 244         std::cout << "Agrega teoria: " << t4 << "\n";
 245         std::cout << "\tdatos_iniciales:\n" << t4.datos_iniciales << "\n";
 246         std::cout << "\tdatos_finales:\n" << t4.datos_finales << "\n";
 247 #endif // DEBUG
 248
 249         // Inicializo el SA
 250         delete a.p_entorno;
 251         a.p_entorno = e;
 252
 253         // Obtengo un plan
 254         CIndiceMagico<t_dato> datos_finales;
 255
 256         a.m_datos_finales.add ("robot.avanzo_adelante", 1) ;
 257         //a.m_datos_finales.add ("robot.avanzo_en_x", 1) ;
 258
 259         for (int i = 0; i < 3; ++i)
 260         {
 261
 262                 std::cout << "Teorias:\n" << a.teorias << "\n";
 263
 264                 a.plan();
 265
 266                 while (a.has_next_theory())
 267                 {
 268                         CTeoria* t = a.get_next_theory();
 269                         if (a.validate_theory(t))
 270                         {
 271                                 std::cout << "Valida\n";
 272                         }
 273                         else
 274                         {
 275                                 std::cout << "No valida, planificamos de nuevo\n";
 276                                 break;
 277                         }
 278                 }
 279
 280         }
 281
 282         return 0 ;
 283 }
 284
 285