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

[z.facultad/75.68/celdas.git] / trunk / src / breve / Celdas.tz

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