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         datos.add("robot.avanzo_en_y", 0) ;
  11         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 }
  17
  18
  19 void CMiEntorno::actualizar()
  20 {
  21         datos.find("robot.sensor_1") = sensor_1 ;
  22         datos.find("robot.sensor_2") = sensor_2 ;
  23         datos.find("robot.sensor_3") = sensor_3 ;
  24
  25         datos.find("robot.avanzo_en_x") = avanzo_en_x?1:0 ;
  26         datos.find("robot.avanzo_en_y") = avanzo_en_y?1:0 ;
  27         datos.find("robot.avanzo_en_z") = avanzo_en_z?1:0 ;
  28
  29
  30 }
  31
  32
  33
  34 //
  35 int main(int argc, char** argv)
  36 {
  37         //
  38         CMiEntorno                      e ;
  39         CSistemaAutonomo                a ;
  40
  41         e.inicializar() ;
  42
  43         // Inicializo las teorias
  44         CTeoria t1("Avanzar_X_1", "AvanzarX", 1, 1) ;
  45         t1.datos_iniciales.add ("robot.sensor_1", 0) ;
  46         t1.datos_iniciales.add ("robot.sensor_2", ANY) ;
  47         t1.datos_iniciales.add ("robot.sensor_3", ANY) ;
  48         t1.datos_iniciales.add ("robot.avanzo_en_x", ANY) ;
  49         t1.datos_iniciales.add ("robot.avanzo_en_y", ANY) ;
  50         t1.datos_iniciales.add ("robot.avanzo_en_z", ANY) ;
  51         t1.datos_finales.add ("robot.avanzo_en_x", 1) ;
  52         t1.datos_finales.add ("robot.avanzo_en_y", ANY) ;
  53         t1.datos_finales.add ("robot.avanzo_en_z", ANY) ;
  54         t1.datos_finales.add ("robot.sensor_1", ANY) ;
  55         t1.datos_finales.add ("robot.sensor_2", ANY) ;
  56         t1.datos_finales.add ("robot.sensor_3", ANY) ;
  57
  58         CTeoria t2("Avanzar_Y_1", "AvanzarY", 1, 1) ;
  59         t2.datos_iniciales.add ("robot.sensor_1", ANY) ;
  60         t2.datos_iniciales.add ("robot.sensor_2", 0) ;
  61         t2.datos_iniciales.add ("robot.sensor_3", ANY) ;
  62         t2.datos_iniciales.add ("robot.avanzo_en_x", ANY) ;
  63         t2.datos_iniciales.add ("robot.avanzo_en_y", ANY) ;
  64         t2.datos_iniciales.add ("robot.avanzo_en_z", ANY) ;
  65         t2.datos_finales.add ("robot.avanzo_en_x", 0) ;
  66         t2.datos_finales.add ("robot.avanzo_en_y", 1) ;
  67         t2.datos_finales.add ("robot.avanzo_en_z", 0) ;
  68         t2.datos_finales.add ("robot.sensor_1", ANY) ;
  69         t2.datos_finales.add ("robot.sensor_2", ANY) ;
  70         t2.datos_finales.add ("robot.sensor_3", ANY) ;
  71
  72         CTeoria t3("Avanzar_Z_1", "AvanzarZ", 1, 1) ;
  73         t3.datos_iniciales.add ("robot.sensor_1", ANY) ;
  74         t3.datos_iniciales.add ("robot.sensor_2", ANY) ;
  75         t3.datos_iniciales.add ("robot.sensor_3", 0) ;
  76         t3.datos_iniciales.add ("robot.avanzo_en_x", ANY) ;
  77         t3.datos_iniciales.add ("robot.avanzo_en_y", ANY) ;
  78         t3.datos_iniciales.add ("robot.avanzo_en_z", ANY) ;
  79         t3.datos_finales.add ("robot.avanzo_en_x", 0) ;
  80         t3.datos_finales.add ("robot.avanzo_en_y", 0) ;
  81         t3.datos_finales.add ("robot.avanzo_en_z", 1) ;
  82         t3.datos_finales.add ("robot.sensor_1", ANY) ;
  83         t3.datos_finales.add ("robot.sensor_2", ANY) ;
  84         t3.datos_finales.add ("robot.sensor_3", ANY) ;
  85
  86         a.teorias.add(t1.nombre.c_str(), t1) ;
  87         a.teorias.add(t2.nombre.c_str(), t2) ;
  88         a.teorias.add(t3.nombre.c_str(), t3) ;
  89
  90
  91         // Inicializo el SA
  92         a.p_entorno = (CEntorno*)(&e) ;
  93
  94
  95         // Obtengo un plan
  96         double p = 1 ;
  97         CIndiceMagico<CTeoria> plan ;
  98         CIndiceMagico<t_dato> datos_finales ;
  99
 100         datos_finales.add ("robot.avanzo_en_x", 1) ;
 101
 102         a.planificar (a.p_entorno->datos, datos_finales, plan, p) ;
 103
 104         for (unsigned i=0; i<plan.count(); i++)
 105                 std::cout << plan[i].nombre.c_str() << std::endl ;
 106
 107
 108         // Ejecuto el plan
 109 //      a.ejecutar(plan) ;
 110
 111
 112         return 0 ;
 113 }
 114
 115