Se avanza más con la integración (aun sin probar).

diff --git a/trunk/src/breve/robot/Celdas-2-7.tz b/trunk/src/breve/robot/Celdas-2-7.tz
index 46d4bfa6dc49e95f21ebe52b37243cebce835c7f..30e36db57552e29286c98c018be417ddb0277e8d 100644 (file)
--- a/trunk/src/breve/robot/Celdas-2-7.tz
+++ b/trunk/src/breve/robot/Celdas-2-7.tz
@@ -7,6 +7,7 @@
 @use SistemaAutonomo.\r
 \r
 @define CELDAS_MAX_VELOCITY 30.\r
+@define CELDAS_TURNO 100.\r
 \r
 PhysicalControl : CeldasControl {\r
        % This class is used for building simple vehicle \r
@@ -163,13 +164,14 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
                lSensor, rSensor, fSensor, bSensor (object).\r
                lfWheel,rfWheel,lbWheel,rbWheel (object).\r
                tleft,tright (int).         \r
+               avanzando,retrocediendo,girando_izq,girando_der(int).       \r
                iterate(int).\r
                teorias (list).\r
                sa (object).\r
                teoria (object).\r
                entorno (hash).\r
                datos-finales (hash).\r
-               avanzando,retrocediendo,girando(int).       \r
+               plan_finished (int).\r
         \r
        - to get-density:\r
                return 20.0.\r
@@ -192,17 +194,17 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
        + to turn-right:                \r
                tright++.\r
 \r
-               self rotate around-axis (0,1,0) by (-1.5709/10)*tright. \r
+               self rotate around-axis (0,1,0) by (-1.5709/CELDAS_TURNO)*tright. \r
                        \r
-               if(tright==10): tright=0.\r
+               if (tright == CELDAS_TURNO): tright=0.\r
 \r
 \r
        + to turn-left:\r
                tleft++.\r
 \r
-               self rotate around-axis (0,1,0) by (1.5709/10)*tleft. \r
+               self rotate around-axis (0,1,0) by (1.5709/CELDAS_TURNO)*tleft. \r
                        \r
-               if(tleft==10): tleft=0.\r
+               if (tleft == CELDAS_TURNO): tleft=0.\r
 \r
 \r
         + to get-sensor-value:\r
@@ -214,40 +216,10 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
                entorno{"sensor_r"} = (rSensor get-data).\r
                entorno{"sensor_l"} = (lSensor get-data).\r
                entorno{"movido"} = 0. # TODO\r
-               sa update-entorno entorno entorno.            \r
+               sa update-entorno with entorno.            \r
 \r
         +to init:\r
-               sa = new SistemaAutonomo.\r
-               iterate=0.\r
-\r
-               teorias = 2 new Teorias.\r
-               teorias{0} init named "Avanzar" with-action "avanzar".\r
-               teorias{0} set-dato-inicial name "sensor_f" value 0.\r
-               teorias{0} set-dato-inicial name "sensor_b" value ANY.\r
-               teorias{0} set-dato-inicial name "sensor_r" value ANY.\r
-               teorias{0} set-dato-inicial name "sensor_l" value ANY.\r
-               teorias{0} set-dato-inicial name "movido" value ANY.\r
-               teorias{0} set-dato-final name "sensor_f" value ANY.\r
-               teorias{0} set-dato-final name "sensor_b" value ANY.\r
-               teorias{0} set-dato-final name "sensor_r" value ANY.\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 "Rotar a derecha" with-action "derecha".\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_l" value ANY.\r
-               teorias{1} set-dato-inicial name "movido" value ANY.\r
-               teorias{1} set-dato-final name "sensor_f" value 0.\r
-               teorias{1} set-dato-final name "sensor_b" value ANY.\r
-               teorias{1} set-dato-final name "sensor_r" value ANY.\r
-               teorias{1} set-dato-final name "sensor_l" value ANY.\r
-               teorias{1} set-dato-final name "movido" value 0.\r
-\r
-               sa add teoria teorias{0}.\r
-               sa add teoria teorias{1}.\r
-\r
+               # Configuracion de robot\r
                fSensor = (self add-sensor at (2.0, .4, 0)).            \r
                fSensor set-direction to (1,0,0).\r
                #fSensor set-direction to (0,0,1).\r
@@ -264,77 +236,174 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
                lSensor set-id at 3.\r
                lSensor set-body at self.\r
 \r
-\r
                rSensor = (self add-sensor at (0, .4, -1.5)).\r
                rSensor set-direction to (0,0,-1).\r
                #rSensor set-direction to (-1,0,0).\r
                rSensor set-id at 4.\r
                rSensor set-body at self.\r
 \r
-\r
                lfWheel = (self add-wheel at (2, 0, -1.5)).\r
                lbWheel = (self add-wheel at (-2, 0, -1.5)).\r
                rfWheel = (self add-wheel at (2, 0, 1.5)).\r
                rbWheel = (self add-wheel at (-2, 0, 1.5)).\r
 \r
-               tleft=tright=0. #Debe ser inicializado en 0 esta asi para probar!!!!!!!!!!!!!!!!!!!!!!!!\r
-               avanzando=1.\r
+               tleft=tright=0.\r
+               avanzando=0.\r
                retrocediendo=0.\r
-               girando=0.            \r
+               girando_izq=0.            \r
+               girando_der=0.            \r
 \r
-        +to iterate:\r
-               valuef,valueb,valuer,valuel (float).\r
-               fl, fr(float).\r
+               # Configuracion de sistema autonomo\r
+               sa = new SistemaAutonomo.\r
+               iterate = 0.\r
+               plan_finished = 1. # así planificamos apenas empezamos\r
 \r
-               valuef=fSensor get-data.\r
-               valueb=bSensor get-data.\r
-               valuel=lSensor get-data.\r
-               valuer=rSensor get-data.\r
+               teorias = 4 new Teorias.\r
+               teorias{0} init named "Avanzar" with-action "adelante".\r
+               teorias{0} set-dato-inicial name "sensor_f" value 0.\r
+               teorias{0} set-dato-inicial name "sensor_b" value ANY.\r
+               teorias{0} set-dato-inicial name "sensor_r" value ANY.\r
+               teorias{0} set-dato-inicial name "sensor_l" value ANY.\r
+               teorias{0} set-dato-inicial name "movido" value ANY.\r
+               teorias{0} set-dato-final name "sensor_f" value ANY.\r
+               teorias{0} set-dato-final name "sensor_b" value ANY.\r
+               teorias{0} set-dato-final name "sensor_r" value ANY.\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} 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_l" value ANY.\r
+               teorias{1} set-dato-inicial name "movido" value ANY.\r
+               teorias{1} set-dato-final name "sensor_f" value 0.\r
+               teorias{1} set-dato-final name "sensor_b" value ANY.\r
+               teorias{1} set-dato-final name "sensor_r" value ANY.\r
+               teorias{1} set-dato-final name "sensor_l" value ANY.\r
+               teorias{1} set-dato-final name "movido" value 1.\r
+\r
+               teorias{2} init named "Rotar a derecha" with-action "derecha".\r
+               teorias{2} set-dato-inicial name "sensor_f" value 1.\r
+               teorias{2} set-dato-inicial name "sensor_b" value ANY.\r
+               teorias{2} set-dato-inicial name "sensor_r" value ANY.\r
+               teorias{2} set-dato-inicial name "sensor_l" value ANY.\r
+               teorias{2} set-dato-inicial name "movido" value ANY.\r
+               teorias{2} set-dato-final name "sensor_f" value 0.\r
+               teorias{2} set-dato-final name "sensor_b" value ANY.\r
+               teorias{2} set-dato-final name "sensor_r" value ANY.\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} 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_l" value ANY.\r
+               teorias{3} set-dato-inicial name "movido" value ANY.\r
+               teorias{3} set-dato-final name "sensor_f" value 0.\r
+               teorias{3} set-dato-final name "sensor_b" value ANY.\r
+               teorias{3} set-dato-final name "sensor_r" value 1.\r
+               teorias{3} set-dato-final name "sensor_l" value ANY.\r
+               teorias{3} set-dato-final name "movido" value 0.\r
+\r
+               sa add teoria teorias{0}.\r
+               sa add teoria teorias{1}.\r
+               sa add teoria teorias{2}.\r
+               sa add teoria teorias{3}.\r
 \r
                datos-finales{"movido"} = 1.\r
-               print "senforl: $valuel".\r
+               sa update-datos-finales with datos-finales.\r
 \r
+        +to iterate:\r
+               fl, fr(float).\r
 \r
-               sa update-datos-finales datos-finales datos-finales.\r
-               sa plan.\r
+               if (0): # TODO posicion_final == posicion_actual\r
+               {\r
+                       print "Llegamos al FINAL!!!".\r
+                       return.\r
+               }\r
 \r
-               if (iterate==0): {\r
-                       print "iteracion 0".\r
-                               if (sa has-next-theory):\r
+               if (plan_finished):\r
+               {\r
+                       self update-entorno.\r
+                       sa plan. # Si no tenemos plan, lo hacemos\r
+                       plan_finished = 0.\r
+                       # TODO posicion_inicial = posicion_actual\r
+                       if (! sa-has-next-theory):\r
                        {\r
+                               print "El planificador no encuentra PLAN!!!".\r
+                               return.\r
+                       }\r
+               }\r
 \r
+               if (iterate == 0):\r
+               {\r
+                       print "iteracion 0".\r
+                       if (sa has-next-theory):\r
+                       {\r
                                print "hay teoria".\r
-                                       teoria = sa get-next-theory.\r
-                                       if ((teoria get-accion) == "avanza"): {\r
-                                               #entorno{"sensor"} = 1.\r
-                                               #entorno{"movido"} = 1.\r
-                                               self set-global-velocity.\r
-\r
-                                       }\r
-                               if ((teoria get-accion) == "retrocede"): {\r
-                                       self set-global-velocity to -5.\r
-\r
+                               teoria = sa get-next-theory.\r
+                               if ((teoria get-accion) == "adelante"):\r
+                               {\r
+                                       avanzando = 1.\r
+                                       retrocediendo = 0.\r
+                                       girando_izq = 0.\r
+                                       girando_der = 0.\r
+                               }\r
+                               if ((teoria get-accion) == "atras"):\r
+                               {\r
+                                       avanzando = 0.\r
+                                       retrocediendo = 1.\r
+                                       girando_izq = 0.\r
+                                       girando_der = 0.\r
+                               }\r
+                               if ((teoria get-accion) == "izquierda"):\r
+                               {\r
+                                       avanzando = 0.\r
+                                       retrocediendo = 0.\r
+                                       girando_izq = 1.\r
+                                       girando_der = 0.\r
+                               }\r
+                               if ((teoria get-accion) == "derecha"):\r
+                               {\r
+                                       avanzando = 0.\r
+                                       retrocediendo = 0.\r
+                                       girando_izq = 0.\r
+                                       girando_der = 1.\r
                                }\r
                        }\r
                }\r
 \r
-               if ((iterate==100) && (teoria)):\r
+               if (iterate == CELDAS_TURNO):\r
                {\r
-\r
                        self update-entorno.\r
-                       if (sa validate theory teoria): {\r
+                       # TODO if (posicion_actual == posicion_inicial): movido = false. else movido = true.\r
+                       if (sa validate theory teoria):\r
+                       {\r
                                print "valida".\r
                        }\r
-                       else {\r
-                               print "Teoria no valida, salimos".\r
+                       else\r
+                       {\r
+                               print "Teoria no valida".\r
+                               plan_finished = 1.\r
                        }\r
-\r
                }\r
 \r
                iterate++.\r
-                       if(iterate==101):\r
-                               iterate=0.\r
+               if (iterate == CELDAS_TURNO + 1):\r
+                       iterate = 0.\r
+\r
+               # Movimiento del robot\r
+               if (avanzando):\r
+                       self set-global-velocity to (15).\r
+               if (retrocediendo):\r
+                       self set-global-velocity to (-15).\r
+               if (girando_izq):\r
+                       self turn-left.\r
+               if (girando_der):\r
+                       self turn-right.\r
+\r
 }\r
 \r
 Stationary : CeldasObstacle (aka CeldasObstacles) {\r

diff --git a/trunk/src/breve/robot/laberintov4.tz b/trunk/src/breve/robot/laberintov4.tz
index 157623ce794a28f95ca9ef22c81655f030c9c103..488ae4231700dcccc07167fc99c188aa27f4f6b2 100644 (file)
--- a/trunk/src/breve/robot/laberintov4.tz
+++ b/trunk/src/breve/robot/laberintov4.tz
@@ -1,6 +1,6 @@
 @use Control.\r
 @use Stationary.\r
-@use Celdas-2-6.\r
+@use Celdas-2-7.\r
 \r
 @define altoPared  5.\r
 @define posYPared 25.\r