trunk/src/main.cpp

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