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.avanzo_en_x", 0) ;
  10         // old datos.add("robot.avanzo_en_y", 0) ;
  11         // old datos.add("robot.avanzo_en_z", 0) ;
  12
  13         datos.add("robot.sensor_1", 0) ;
  14         datos.add("robot.sensor_2", 0) ;
  15         datos.add("robot.sensor_3", 0) ;
  16         datos.add("robot.sensor_4", 0) ; // new
  17 }
  18
  19
  20 void CMiEntorno::actualizar()
  21 {
  22         datos.find("robot.sensor_1") = sensor_1 ;
  23         datos.find("robot.sensor_2") = sensor_2 ;
  24         datos.find("robot.sensor_3") = sensor_3 ;
  25         datos.find("robot.sensor_4") = 20 ; // new
  26
  27         datos.find("robot.avanzo_en_x") = avanzo_en_x?1:0 ;
  28         datos.find("robot.avanzo_en_y") = avanzo_en_y?1:0 ;
  29         datos.find("robot.avanzo_en_z") = avanzo_en_z?1:0 ;
  30
  31
  32 }
  33
  34
  35
  36 //
  37 int main(int argc, char* argv[])
  38 {
  39         //
  40         CMiEntorno*                     e = new CMiEntorno;
  41         CSistemaAutonomo                a ;
  42
  43         e->inicializar() ;
  44
  45 /* OLD
  46         // Inicializo las teorias
  47         CTeoria t1("Avanzar_X_1", "AvanzarX", 1, 1) ;
  48         t1.datos_iniciales.add ("robot.sensor_1", 0) ;
  49         t1.datos_iniciales.add ("robot.sensor_2", ANY) ;
  50         t1.datos_iniciales.add ("robot.sensor_3", ANY) ;
  51         t1.datos_iniciales.add ("robot.avanzo_en_x", ANY) ;
  52         t1.datos_iniciales.add ("robot.avanzo_en_y", ANY) ;
  53         t1.datos_iniciales.add ("robot.avanzo_en_z", ANY) ;
  54         t1.datos_finales.add ("robot.avanzo_en_x", 1) ;
  55         t1.datos_finales.add ("robot.avanzo_en_y", ANY) ;
  56         t1.datos_finales.add ("robot.avanzo_en_z", ANY) ;
  57         t1.datos_finales.add ("robot.sensor_1", ANY) ;
  58         t1.datos_finales.add ("robot.sensor_2", ANY) ;
  59         t1.datos_finales.add ("robot.sensor_3", ANY) ;
  60
  61         CTeoria t2("Avanzar_Y_1", "AvanzarY", 1, 1) ;
  62         t2.datos_iniciales.add ("robot.sensor_1", ANY) ;
  63         t2.datos_iniciales.add ("robot.sensor_2", 0) ;
  64         t2.datos_iniciales.add ("robot.sensor_3", ANY) ;
  65         t2.datos_iniciales.add ("robot.avanzo_en_x", ANY) ;
  66         t2.datos_iniciales.add ("robot.avanzo_en_y", ANY) ;
  67         t2.datos_iniciales.add ("robot.avanzo_en_z", ANY) ;
  68         t2.datos_finales.add ("robot.avanzo_en_x", 0) ;
  69         t2.datos_finales.add ("robot.avanzo_en_y", 1) ;
  70         t2.datos_finales.add ("robot.avanzo_en_z", 0) ;
  71         t2.datos_finales.add ("robot.sensor_1", ANY) ;
  72         t2.datos_finales.add ("robot.sensor_2", ANY) ;
  73         t2.datos_finales.add ("robot.sensor_3", ANY) ;
  74
  75         CTeoria t3("Avanzar_Z_1", "AvanzarZ", 1, 1) ;
  76         t3.datos_iniciales.add ("robot.sensor_1", ANY) ;
  77         t3.datos_iniciales.add ("robot.sensor_2", ANY) ;
  78         t3.datos_iniciales.add ("robot.sensor_3", 0) ;
  79         t3.datos_iniciales.add ("robot.avanzo_en_x", ANY) ;
  80         t3.datos_iniciales.add ("robot.avanzo_en_y", ANY) ;
  81         t3.datos_iniciales.add ("robot.avanzo_en_z", ANY) ;
  82         t3.datos_finales.add ("robot.avanzo_en_x", 0) ;
  83         t3.datos_finales.add ("robot.avanzo_en_y", 0) ;
  84         t3.datos_finales.add ("robot.avanzo_en_z", 1) ;
  85         t3.datos_finales.add ("robot.sensor_1", ANY) ;
  86         t3.datos_finales.add ("robot.sensor_2", ANY) ;
  87         t3.datos_finales.add ("robot.sensor_3", ANY) ;
  88 */
  89
  90         // Inicializo las teorias
  91         CTeoria t1("Avanzar_X_1", "AvanzarX", 1, 1) ;
  92         t1.datos_iniciales.add ("robot.sensor_1", ANY) ;
  93         t1.datos_iniciales.add ("robot.sensor_2", ANY) ;
  94         t1.datos_iniciales.add ("robot.sensor_3", ANY) ;
  95         t1.datos_iniciales.add ("robot.sensor_4", ANY) ;
  96         t1.datos_iniciales.add ("robot.avanzo_en_x", ANY) ;
  97         t1.datos_finales.add ("robot.avanzo_en_x", ANY) ;
  98         t1.datos_finales.add ("robot.sensor_1", ANY) ;
  99         t1.datos_finales.add ("robot.sensor_2", ANY) ;
 100         t1.datos_finales.add ("robot.sensor_3", ANY) ;
 101         t1.datos_finales.add ("robot.sensor_4", ANY) ;
 102
 103         // Inicializo las teorias
 104         CTeoria t2("Avanzar_X_2", "AvanzarX", 1, 1) ;
 105         t2.datos_iniciales.add ("robot.sensor_1", ANY) ;
 106         t2.datos_iniciales.add ("robot.sensor_2", ANY) ;
 107         t2.datos_iniciales.add ("robot.sensor_3", ANY) ;
 108         t2.datos_iniciales.add ("robot.sensor_4", ANY) ;
 109         t2.datos_iniciales.add ("robot.avanzo_en_x", ANY) ;
 110         t2.datos_finales.add ("robot.avanzo_en_x", ANY) ;
 111         t2.datos_finales.add ("robot.sensor_1", ANY) ;
 112         t2.datos_finales.add ("robot.sensor_2", ANY) ;
 113         t2.datos_finales.add ("robot.sensor_3", ANY) ;
 114         t2.datos_finales.add ("robot.sensor_4", ANY) ;
 115
 116         a.teorias.add(t1.nombre.c_str(), t1) ;
 117         a.teorias.add(t2.nombre.c_str(), t2) ;
 118         // OLD a.teorias.add(t3.nombre.c_str(), t3) ;
 119 #ifdef DEBUG
 120         std::cout << "Agrega teoria " << t1.nombre << ":\n";
 121         std::cout << "  .datos_iniciales:\n" << t1.datos_iniciales << "\n";
 122         std::cout << "  .datos_finales:\n" << t1.datos_finales << "\n";
 123         std::cout << "Agrega teoria " << t2.nombre << ":\n";
 124         std::cout << "  .datos_iniciales:\n" << t2.datos_iniciales << "\n";
 125         std::cout << "  .datos_finales:\n" << t2.datos_finales << "\n";
 126 #endif // DEBUG
 127
 128
 129         // Inicializo el SA
 130         delete a.p_entorno;
 131         a.p_entorno = e;
 132
 133
 134         // Obtengo un plan
 135         CIndiceMagico<t_dato> datos_finales;
 136
 137         a.m_datos_finales.add ("robot.avanzo_en_x", 1) ;
 138         a.plan();
 139         for (unsigned i=0; i<a.m_plan.count(); i++)
 140                 std::cout << a.m_plan[i].nombre.c_str() << std::endl ;
 141
 142         if (a.has_next_theory())
 143         {
 144                 CTeoria* t = a.get_next_theory();
 145                 if (a.validate_theory(t))
 146                         std::cout << "Valida\n";
 147         }
 148
 149 //      datos_finales.add ("robot.avanzo_en_x", 1) ;
 150
 151 //      a.planificar (a.p_entorno->datos, datos_finales, plan, p) ;
 152
 153 //      for (unsigned i=0; i<plan.count(); i++)
 154 //              std::cout << plan[i].nombre.c_str() << std::endl ;
 155
 156
 157         // Ejecuto el plan
 158 //      a.ejecutar(plan) ;
 159
 160
 161         return 0 ;
 162 }
 163
 164