Merge de Celdas-2-6 con Celdas-2-7. No está probado y seguramente no anda nada, pero...

diff --git a/trunk/src/breve/robot/Celdas-2-7.tz b/trunk/src/breve/robot/Celdas-2-7.tz
index 661c068b080c13ba25cfa02d85d1983d2c30aefb..46d4bfa6dc49e95f21ebe52b37243cebce835c7f 100644 (file)
--- a/trunk/src/breve/robot/Celdas-2-7.tz
+++ b/trunk/src/breve/robot/Celdas-2-7.tz
@@ -160,18 +160,16 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
        % vehicle is heavier and harder to control, but more stable\r
        % at higher speeds.\r
         +variables:\r
-            lSensor, rSensor, fSensor, bSensor (object).\r
-            lWheel,rWheel (object).\r
-               tleft,tright (int).      \r
+               lSensor, rSensor, fSensor, bSensor (object).\r
+               lfWheel,rfWheel,lbWheel,rbWheel (object).\r
+               tleft,tright (int).         \r
                iterate(int).\r
-\r
                teorias (list).\r
                sa (object).\r
                teoria (object).\r
                entorno (hash).\r
                datos-finales (hash).\r
-   \r
-        \r
+               avanzando,retrocediendo,girando(int).       \r
         \r
        - to get-density:\r
                return 20.0.\r
@@ -182,12 +180,14 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
        - to get-wheel-radius:\r
                return 0.8.\r
 \r
-        + to set-global-velocity to velocity=5 (float):\r
-               rWheel set-velocity to velocity.\r
-               lWheel 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
+               rbWheel set-velocity to velocity.\r
+               lbWheel set-velocity to velocity.\r
 \r
        + to get-global-velocity:\r
-               return ((rWheel get-velocity) + (lWheel get-velocity)) / 2.\r
+               return ((rfWheel get-velocity) + (lfWheel get-velocity)) / 2.\r
 \r
        + to turn-right:                \r
                tright++.\r
@@ -208,118 +208,121 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
         + to get-sensor-value:\r
                return (fSensor get-sensor-value).\r
 \r
+       +to update-entorno:\r
+               entorno{"sensor_f"} = (fSensor get-data).\r
+               entorno{"sensor_b"} = (bSensor get-data).\r
+               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
+\r
         +to init:\r
                sa = new SistemaAutonomo.\r
                iterate=0.\r
 \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
-            fSensor set-id at 1.\r
-               fSensor set-body at self.\r
-            bSensor = (self add-sensor at (-2.0, .4, 0)).\r
-            bSensor set-direction to (-1,0,0).\r
-            #bSensor set-direction to (0,0,1).\r
-            bSensor set-id at 2.\r
-               bSensor set-body at self.\r
-            lSensor = (self add-sensor at (0, .4, 1.5)).\r
-            lSensor set-direction to (0,0,1).\r
-            #lSensor set-direction to (1,0,0).\r
-            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
-            lWheel = (self add-wheel at (0, 0, -1.5)).\r
-            rWheel = (self add-wheel at (0, 0, 1.5)).\r
-\r
-                tleft=tright=0.        #Debe ser inicializado en 0 esta asi para probar!!!!!!!!!!!!!!!!!!!!!!!!\r
-\r
-               entorno{"sensor_f"} = 0.\r
-               entorno{"sensor_b"} = 0.\r
-               entorno{"sensor_r"} = 0.\r
-               entorno{"sensor_l"} = 0.\r
-\r
-               entorno{"movido"} = 0.\r
-               sa update-entorno entorno entorno.            \r
-       \r
-\r
                teorias = 2 new Teorias.\r
-               teorias{0} init named "avanzar" with-action "avanza".\r
-               (teorias{0}) set-dato-inicial name "sensor_f" value ANY.\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 ANY.\r
-\r
-               teorias{1} init named "avanzar2" with-action "avanza".\r
-               (teorias{1}) set-dato-inicial name "sensor_f" value ANY.\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 ANY.\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 ANY.\r
-\r
-\r
-               sa add teoria (teorias{0}).\r
-               sa add teoria (teorias{1}).\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
-               datos-finales{"movido"} = 1.\r
-\r
-               sa update-datos-finales datos-finales datos-finales.\r
-               sa plan.\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
+               fSensor set-id at 1.\r
+               fSensor set-body at self.\r
+               bSensor = (self add-sensor at (-2.0, .4, 0)).\r
+               bSensor set-direction to (-1,0,0).\r
+               #bSensor set-direction to (0,0,1).\r
+               bSensor set-id at 2.\r
+               bSensor set-body at self.\r
+               lSensor = (self add-sensor at (0, .4, 1.5)).\r
+               lSensor set-direction to (0,0,1).\r
+               #lSensor set-direction to (1,0,0).\r
+               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
+               retrocediendo=0.\r
+               girando=0.            \r
 \r
-                        \r
         +to iterate:\r
-        #+ to post-iterate:\r
                valuef,valueb,valuer,valuel (float).\r
                fl, fr(float).\r
 \r
+               valuef=fSensor get-data.\r
+               valueb=bSensor get-data.\r
+               valuel=lSensor get-data.\r
+               valuer=rSensor get-data.\r
 \r
 \r
-           if(iterate==0): {\r
-               print "iteracion 0".\r
-               if (sa has-next-theory):\r
-               {\r
+               datos-finales{"movido"} = 1.\r
+               print "senforl: $valuel".\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
+               sa update-datos-finales datos-finales datos-finales.\r
+               sa plan.\r
+\r
+               if (iterate==0): {\r
+                       print "iteracion 0".\r
+                               if (sa has-next-theory):\r
+                       {\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
+                               }\r
                        }\r
                }\r
-           }\r
 \r
-               if (iterate==100) && (teoria):\r
+               if ((iterate==100) && (teoria)):\r
                {\r
 \r
-                       entorno{"sensor_f"} = (fSensor get-data).\r
-                       entorno{"sensor_b"} = (bSensor get-data).\r
-                       entorno{"sensor_r"} = (rSensor get-data).\r
-                       entorno{"sensor_l"} = (lSensor get-data).\r
-\r
-                       sa update-entorno entorno entorno.\r
+                       self update-entorno.\r
                        if (sa validate theory teoria): {\r
                                print "valida".\r
                        }\r
@@ -328,11 +331,10 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
                        }\r
 \r
                }\r
-       \r
-               iterate++.\r
-               if(iterate==101):\r
-                       iterate=0.\r
 \r
+               iterate++.\r
+                       if(iterate==101):\r
+                               iterate=0.\r
 }\r
 \r
 Stationary : CeldasObstacle (aka CeldasObstacles) {\r
@@ -529,24 +531,10 @@ Link : CeldasSensor (aka CeldasSensors) {
                                 des3=0.\r
                                 \r
                        }                               \r
-\r
-                       #aux1=dot((self get-location),(i get-direction)).\r
-\r
-                       #print "sensor: $id obstaculo: $posObstacle direP: $destiny direS: $direction yo: $yo ".\r
-                       #print "begin: $wallBegin end: $wallEnd ".\r
-                       #print "begin: $wallBegin end: $wallEnd".\r
-\r
                        \r
-\r
-                       #print "sensor: $id , des1: $des1, des2: $des2, des3: $des3".\r
                        if ((des2) && (des3)):\r
                          {                                    \r
-                               draw clear.                             \r
-                               #print " posObstacle: $posObstacle".                                                    \r
-                               \r
-                       print "sensor: $id obstaculo: $posObstacle direP: $destiny direS: $direction yo: $yo ".\r
-                       print "begin: $wallBegin end: $wallEnd ".\r
-                                       \r
+                               draw clear.\r
 \r
                                dist=|obsLoc - (self get-location)|.\r
                                if( (j==0) || (min>dist) ):\r
@@ -567,7 +555,7 @@ Link : CeldasSensor (aka CeldasSensors) {
                          #Dibujo el laser\r
                                  draw set-color to (1, 0, 0).\r
                          draw draw-line from (self get-location) to (obs).\r
-                         return dist.\r
+                         return min.\r
                        }\r
                \r
 \r
@@ -576,4 +564,4 @@ Link : CeldasSensor (aka CeldasSensors) {
 \r
 \r
 }\r
-               
\ No newline at end of file
+               \r