Varios cambios para terminar de integrar. Ya está casi andando perfecto, sólo hay...

diff --git a/trunk/src/breve/Celdas.tz b/trunk/src/breve/Celdas.tz
index ed8532c9c249389560274cf0ad8bc6a3611a3bd0..6ca2d6c064a89763fda5461dd10c696b5e58f292 100644 (file)
--- a/trunk/src/breve/Celdas.tz
+++ b/trunk/src/breve/Celdas.tz
@@ -7,9 +7,8 @@
 @use SistemaAutonomo.\r
 \r
 @define CELDAS_MAX_VELOCITY 30.\r
-@define CELDAS_TURNO 80.\r
-@define CELDAS_SENSOR_THRESHOLD 9.\r
-@define CELDAS_VECTOR_ERROR 0.5.\r
+@define CELDAS_TURNO 100.\r
+@define CELDAS_SENSOR_THRESHOLD 10.\r
 \r
 PhysicalControl : CeldasControl {\r
        % This class is used for building simple vehicle \r
@@ -172,7 +171,9 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
                teoria (object).\r
                entorno (hash).\r
                datos-finales (hash).\r
-               plan_finished (int).\r
+               plan-finished (int).\r
+               posicion-inicial (vector).\r
+               posicion-final (vector).\r
         \r
        - to get-density:\r
                return 20.0.\r
@@ -183,15 +184,17 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
        - to get-wheel-radius:\r
                return 0.8.\r
 \r
-       - to compare-vectors compare-vector vector1(vector) with-vector vector2(vector):\r
+       - to near position thePosition (vector) with-error error (float):\r
                vectorAux(vector).\r
-               vectorAux=vector1-vector2.\r
+               vectorAux = (self get-location) - thePosition.\r
 \r
-               if ((|vectorAux::x|<CELDAS_VECTOR_ERROR) && (|vectorAux::y|<CELDAS_VECTOR_ERROR) && (|vectorAux::z|<CELDAS_VECTOR_ERROR))\r
+               #print "-----> (pos, other_pos, diff, error): ", (self get-location), thePosition, vectorAux, error.\r
+\r
+               if ((|vectorAux::x| < error) && (|vectorAux::z| < error)):\r
                        return 1.\r
 \r
                return 0.\r
-               \r
+\r
         + to set-global-velocity to velocity (float):\r
                rfWheel set-velocity to velocity.\r
                lfWheel set-velocity to velocity.\r
@@ -227,7 +230,6 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
                entorno{"sensor_b"} = (bSensor get-sensor-value).\r
                entorno{"sensor_r"} = (rSensor get-sensor-value).\r
                entorno{"sensor_l"} = (lSensor get-sensor-value).\r
-               entorno{"movido"} = 0. # TODO\r
                sa update-entorno with entorno.            \r
 \r
         +to init:\r
@@ -265,11 +267,14 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
                girando_izq=0.            \r
                girando_der=0.            \r
 \r
+               posicion-inicial = (self get-location).\r
+               posicion-final = (0, 0, 0).\r
+\r
                # Configuracion de sistema autonomo\r
                sa = new SistemaAutonomo.\r
                sa init with-max-pasos 4 with-max-teorias 15.\r
                iterate = 0.\r
-               plan_finished = 1. # así planificamos apenas empezamos\r
+               plan-finished = 1. # así planificamos apenas empezamos\r
 \r
                teorias = 4 new Teorias.\r
                teorias{0} init named "Avanzar" with-action "adelante".\r
@@ -329,30 +334,39 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
                sa update-datos-finales with datos-finales.\r
 \r
         +to iterate:\r
+\r
+               # Actualiza entorno\r
                self update-entorno.\r
 \r
-               if (0): # TODO posicion_final == posicion_actual\r
+               # Chequeo de objetivo\r
+               if (self near position posicion-final with-error 5.0):\r
                {\r
                        print "Llegamos al FINAL!!!".\r
                        self set-global-velocity to 0.\r
                        return.\r
                }\r
 \r
-               if (plan_finished):\r
+               # Planificación\r
+               if (plan-finished):\r
                {\r
+                       # Actualiza entorno indicando que no se movió para que\r
+                       # el planificador actue\r
+                       sa set-entorno value 0 with-name "movido".\r
                        sa plan. # Si no tenemos plan, lo hacemos\r
-                       plan_finished = 0.\r
-                       # TODO posicion_inicial = posicion_actual\r
+                       plan-finished = 0.\r
+                       iterate = 0.\r
                        if (! (sa has-next-theory)):\r
                        {\r
-                               plan_finished = 1.\r
+                               plan-finished = 1.\r
                                print "El planificador no encuentra PLAN!!!".\r
                                return.\r
                        }\r
                }\r
 \r
+               # Ejecución de teoría\r
                if (iterate == 0):\r
                {\r
+                       posicion-inicial = (self get-location).\r
                        if (sa has-next-theory):\r
                        {\r
                                teoria = sa get-next-theory.\r
@@ -387,21 +401,29 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
                        }\r
                }\r
 \r
+               # Validación de teoría\r
                if (iterate == CELDAS_TURNO):\r
                {\r
-                       # TODO if (posicion_actual == posicion_inicial): movido = false. else movido = true.\r
-                       if (sa validate theory teoria):\r
-                       {\r
+                       # Actualiza entorno segun si se movio o no\r
+                       if (self near position posicion-inicial with-error 1.0):\r
+                       {\r
+                               sa set-entorno value 0 with-name "movido".\r
                        }\r
                        else\r
                        {\r
-                               plan_finished = 1.\r
+                               sa set-entorno value 1 with-name "movido".\r
+                       }\r
+                       print iterate.\r
+                       if (!(sa validate theory teoria)):\r
+                       {\r
+                               plan-finished = 1.\r
                        }\r
-               }\r
-\r
-               iterate++.\r
-               if (iterate == CELDAS_TURNO + 1):\r
                        iterate = 0.\r
+               }\r
+               else\r
+               {\r
+                       iterate++.\r
+               }\r
 \r
                # Movimiento del robot\r
                if (avanzando):\r

diff --git a/trunk/src/sistemaautonomo.cpp b/trunk/src/sistemaautonomo.cpp
index 54b13cc2c2a4c8f2a6bb5715b7bf3f7a30c55dc2..2c85e518eb2bc21b82100a8f4a4f319a4ee03c30 100644 (file)
--- a/trunk/src/sistemaautonomo.cpp
+++ b/trunk/src/sistemaautonomo.cpp
@@ -69,6 +69,9 @@ CTeoria* CSistemaAutonomo::get_next_theory()
        }
        else
        {
+#ifdef DEBUG
+               std::cout << "SA: ejecuta teoria: " << **(curr_theory) << "\n";
+#endif // DEBUG
                return *(curr_theory++);
        }
 }
@@ -82,6 +85,10 @@ bool CSistemaAutonomo::validate_theory(CTeoria* t)
        // Aumento k (cantidad de veces que se probó la teoría
        ++t->k;
 
+#ifdef DEBUG
+       std::cout << "SA: Entorno de verificacion:\n" << p_entorno->datos;
+#endif // DEBUG
+
        // Verifico
        result = verificar_condicion(t->datos_finales) ;
 
@@ -107,6 +114,9 @@ bool CSistemaAutonomo::validate_theory(CTeoria* t)
                // Aplico heuristicas de correccion
                this->heuristca_retraccion(*t) ;
        }
+#ifdef DEBUG
+       else std::cout << "SA: Verifica!\n";
+#endif // DEBUG
 
        return result;
 }