Agrega un modo de salida intentando que sea más fácil generar tablas con la evolución...

[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 7c1afdc5d79b018d6edb469e9e8104d3b3dd3fb6..ae29a51d0e9a3474b9a14715087abfd817a88c99 100644 (file)
--- a/trunk/src/breve/Celdas.tz
+++ b/trunk/src/breve/Celdas.tz
@@ -6,9 +6,9 @@
 @use Drawing.\r
 @use SistemaAutonomo.\r
 \r
 @use Drawing.\r
 @use SistemaAutonomo.\r
 \r

-@define CELDAS_MAX_VELOCITY 30.\r
-@define CELDAS_TURNO 100.\r
-@define CELDAS_SENSOR_THRESHOLD 9.\r
+@define CELDAS_MAX_VELOCITY 5.\r
+@define CELDAS_TURNO 30.\r
+@define CELDAS_SENSOR_THRESHOLD 10.\r

 \r
 PhysicalControl : CeldasControl {\r
        % This class is used for building simple vehicle \r
 \r
 PhysicalControl : CeldasControl {\r
        % This class is used for building simple vehicle \r


@@ -171,7 +171,9 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
                teoria (object).\r
                entorno (hash).\r
                datos-finales (hash).\r
                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
         \r
        - to get-density:\r
                return 20.0.\r


@@ -182,6 +184,17 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
        - to get-wheel-radius:\r
                return 0.8.\r
 \r
        - to get-wheel-radius:\r
                return 0.8.\r
 \r

+       - to near position thePosition (vector) with-error error (float):\r
+               vectorAux(vector).\r
+               vectorAux = (self get-location) - thePosition.\r
+\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

         + to set-global-velocity to velocity (float):\r
                rfWheel set-velocity to velocity.\r
                lfWheel set-velocity to velocity.\r
         + to set-global-velocity to velocity (float):\r
                rfWheel set-velocity to velocity.\r
                lfWheel set-velocity to velocity.\r


@@ -192,30 +205,35 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
                return ((rfWheel get-velocity) + (lfWheel get-velocity)) / 2.\r
 \r
        + to turn-right:                \r
                return ((rfWheel get-velocity) + (lfWheel get-velocity)) / 2.\r
 \r
        + to turn-right:                \r

-               tright++.\r
-\r
+               #tright++.\r
+               #self rotate around-axis (0,1,0) by (-1.5709/CELDAS_TURNO)*tright. \r
+               #if (tright == CELDAS_TURNO): tright=0.\r
+               if (tright == 0): self set-global-velocity to 0.\r

                self rotate around-axis (0,1,0) by (-1.5709/CELDAS_TURNO)*tright. \r
                self rotate around-axis (0,1,0) by (-1.5709/CELDAS_TURNO)*tright. \r

-                       \r

                if (tright == CELDAS_TURNO): tright=0.\r
                if (tright == CELDAS_TURNO): tright=0.\r

+               else tright++.\r

 \r
 \r
        + to turn-left:\r
 \r
 \r
        + to turn-left:\r

-               tleft++.\r
-\r
-               self rotate around-axis (0,1,0) by (1.5709/CELDAS_TURNO)*tleft. \r
-                       \r
+               #tleft++.\r
+               #self rotate around-axis (0,1,0) by (1.5709/CELDAS_TURNO)*tleft. \r
+               #if (tleft == CELDAS_TURNO): tleft=0.\r
+               if (tleft == 0): self set-global-velocity to 0.\r
+               self rotate around-axis (0,1,0) by (1.5709/CELDAS_TURNO)*tleft.\r

                if (tleft == CELDAS_TURNO): tleft=0.\r
                if (tleft == CELDAS_TURNO): tleft=0.\r

+               else tleft++.\r

 \r
 \r
         + to get-sensor-value:\r
                return (fSensor get-sensor-value).\r
 \r
 \r
 \r
         + to get-sensor-value:\r
                return (fSensor get-sensor-value).\r
 \r

+\r
+\r

        +to update-entorno:\r
                entorno{"sensor_f"} = (fSensor get-sensor-value).\r
                entorno{"sensor_b"} = (bSensor get-sensor-value).\r
                entorno{"sensor_r"} = (rSensor get-sensor-value).\r
                entorno{"sensor_l"} = (lSensor get-sensor-value).\r
        +to update-entorno:\r
                entorno{"sensor_f"} = (fSensor get-sensor-value).\r
                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
                sa update-entorno with entorno.            \r
 \r
         +to init:\r


@@ -253,10 +271,14 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
                girando_izq=0.            \r
                girando_der=0.            \r
 \r
                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
                # Configuracion de sistema autonomo\r
                sa = new SistemaAutonomo.\r

+               sa init with-max-pasos 4 with-max-teorias 15.\r

                iterate = 0.\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
 \r
                teorias = 4 new Teorias.\r
                teorias{0} init named "Avanzar" with-action "adelante".\r


@@ -271,7 +293,7 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
                teorias{0} set-dato-final name "sensor_l" value ANY.\r
                teorias{0} set-dato-final name "movido" value 1.\r
 \r
                teorias{0} set-dato-final name "sensor_l" value ANY.\r
                teorias{0} set-dato-final name "movido" value 1.\r
 \r

-               teorias{1} init named "Retroceder" with-action "atras".\r
+               teorias{1} init named "Retroceder" with-action "atras" executed 2.\r

                teorias{1} set-dato-inicial name "sensor_f" value 1.\r
                teorias{1} set-dato-inicial name "sensor_b" value ANY.\r
                teorias{1} set-dato-inicial name "sensor_r" value ANY.\r
                teorias{1} set-dato-inicial name "sensor_f" value 1.\r
                teorias{1} set-dato-inicial name "sensor_b" value ANY.\r
                teorias{1} set-dato-inicial name "sensor_r" value ANY.\r


@@ -295,7 +317,7 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
                teorias{2} set-dato-final name "sensor_l" value 1.\r
                teorias{2} set-dato-final name "movido" value 0.\r
 \r
                teorias{2} set-dato-final name "sensor_l" value 1.\r
                teorias{2} set-dato-final name "movido" value 0.\r
 \r

-               teorias{3} init named "Rotar a izquierda" with-action "izquierda".\r
+               teorias{3} init named "Rotar a izquierda" with-action "izquierda" executed 2.\r

                teorias{3} set-dato-inicial name "sensor_f" value 1.\r
                teorias{3} set-dato-inicial name "sensor_b" value ANY.\r
                teorias{3} set-dato-inicial name "sensor_r" value ANY.\r
                teorias{3} set-dato-inicial name "sensor_f" value 1.\r
                teorias{3} set-dato-inicial name "sensor_b" value ANY.\r
                teorias{3} set-dato-inicial name "sensor_r" value ANY.\r


@@ -316,30 +338,39 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
                sa update-datos-finales with datos-finales.\r
 \r
         +to iterate:\r
                sa update-datos-finales with datos-finales.\r
 \r
         +to iterate:\r

+\r
+               # Actualiza entorno\r

                self update-entorno.\r
 \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
                {\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
                {\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
                        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
                        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
                                print "El planificador no encuentra PLAN!!!".\r
                                return.\r
                        }\r
                }\r
 \r

+               # Ejecución de teoría\r

                if (iterate == 0):\r
                {\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
                        if (sa has-next-theory):\r
                        {\r
                                teoria = sa get-next-theory.\r


@@ -372,29 +403,41 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
                                        girando_der = 1.\r
                                }\r
                        }\r
                                        girando_der = 1.\r
                                }\r
                        }\r

+                       else\r
+                       {\r
+                               plan-finished = 1.\r
+                       }\r

                }\r
 \r
                }\r
 \r

+               # Validación de teoría\r

                if (iterate == CELDAS_TURNO):\r
                {\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 2.0):\r
+                       {\r
+                               sa set-entorno value 0 with-name "movido".\r

                        }\r
                        else\r
                        {\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
-\r
-               iterate++.\r
-               if (iterate == CELDAS_TURNO + 1):\r

                        iterate = 0.\r
                        iterate = 0.\r

+               }\r
+               else\r
+               {\r
+                       iterate++.\r
+               }\r

 \r
                # Movimiento del robot\r
                if (avanzando):\r
 \r
                # Movimiento del robot\r
                if (avanzando):\r

-                       self set-global-velocity to (15).\r
+                       self set-global-velocity to (CELDAS_MAX_VELOCITY).\r

                if (retrocediendo):\r
                if (retrocediendo):\r

-                       self set-global-velocity to (-15).\r
+                       self set-global-velocity to (-CELDAS_MAX_VELOCITY).\r

                if (girando_izq):\r
                        self turn-left.\r
                if (girando_der):\r
                if (girando_izq):\r
                        self turn-left.\r
                if (girando_der):\r