Construyo wrappers con la nueva interfaz que habíamos hablado para el sistema autónom...

[z.facultad/75.68/celdas.git] / trunk / src / breve / robot / Celdas-2-5.tz

diff --git a/trunk/src/breve/robot/Celdas-2-5.tz b/trunk/src/breve/robot/Celdas-2-5.tz
index 8534014aa11f533779c0c808aa3d1044a8ca7da3..ee56a6e23d8c810ef2fba1fc962519c86e231f0e 100644 (file)
--- a/trunk/src/breve/robot/Celdas-2-5.tz
+++ b/trunk/src/breve/robot/Celdas-2-5.tz
@@ -199,7 +199,6 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
             bSensor set-direction to (-1,0,0).\r
             bSensor set-id at 2.\r
            bSensor set-body at self.\r
             bSensor set-direction to (-1,0,0).\r
             bSensor set-id at 2.\r
            bSensor set-body at self.\r

-            #bSensor set-sensor-angle to (-1.6).\r

             lSensor = (self add-sensor at (0, .4, 1.5)).\r
             lSensor set-direction to (0,0,1).\r
             lSensor set-id at 3.\r
             lSensor = (self add-sensor at (0, .4, 1.5)).\r
             lSensor set-direction to (0,0,1).\r
             lSensor set-id at 3.\r


@@ -223,12 +222,10 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
                valueb=bSensor get-data.\r
                 valuel=lSensor get-data.\r
                 valuer=rSensor get-data.\r
                valueb=bSensor get-data.\r
                 valuel=lSensor get-data.\r
                 valuer=rSensor get-data.\r

-                \r
-               #value = sensor get-data.\r
-                #value = self get-sensor-value.\r
-                #valueb = sensor2 get-sensor-value.\r

 \r
 \r

-               if valuef >7: \r
+               self turn-left with-velocity(20).\r
+                self set-global-velocity to (15).\r
+               if valuef >7:           \r

                     self set-global-velocity to (15).\r
                 else if (valuef <=7) && (valuef > 0):\r
                 {   \r
                     self set-global-velocity to (15).\r
                 else if (valuef <=7) && (valuef > 0):\r
                 {   \r


@@ -370,9 +367,10 @@ Link : CeldasSensor (aka CeldasSensors) {
        + to get-data:\r
                i (object).\r
                 x,y,z (float).\r
        + to get-data:\r
                i (object).\r
                 x,y,z (float).\r

-               a,b (float).\r
-                v(vector).\r
+               min,dist (float).\r
+                v,obs(vector).\r

                aux(float).\r
                aux(float).\r

+               j (int).\r

                des1,des2,des3(int).\r
                wallBegin,wallEnd (float).\r
                 \r
                des1,des2,des3(int).\r
                wallBegin,wallEnd (float).\r
                 \r


@@ -383,22 +381,22 @@ Link : CeldasSensor (aka CeldasSensors) {
                 obsLoc (vector).                \r
                 location (vector).\r
                 posObstacle,posSensor (vector).\r
                 obsLoc (vector).                \r
                 location (vector).\r
                 posObstacle,posSensor (vector).\r

-                angulo(double).\r
-                \r
-                               \r
-               transDir = (self get-rotation) * direction.\r
+                                             \r

                draw clear.\r
                value = 0.0.\r
                draw clear.\r
                value = 0.0.\r

-               foreach i in (all CeldasObstacles): {                                   \r
-                                                \r
-                        posObstacle=i get-location.\r
-                       #print "$i".\r
-                       \r
-                       \r
+               j=0.\r
+               min=0.\r
+               foreach i in (all CeldasObstacles): \r
+                       {\r
+                        posObstacle=i get-location.\r
+                        \r
+                                                               \r
+                       #!\r

                        if(dot((i get-direction),direction)==0):\r
                                des1=1.\r
                        else\r
                                des1=0.\r
                        if(dot((i get-direction),direction)==0):\r
                                des1=1.\r
                        else\r
                                des1=0.\r

+                       !#\r

 \r
                        des2=0.\r
                        if(dot(direction,(1,1,1))<0):\r
 \r
                        des2=0.\r
                        if(dot(direction,(1,1,1))<0):\r


@@ -412,6 +410,7 @@ Link : CeldasSensor (aka CeldasSensors) {
                                        des2=1.         \r
                        }                       \r
 \r
                                        des2=1.         \r
                        }                       \r
 \r

+\r

                        #Compruebo que el robot este frente a la pared\r
                        wallBegin=dot((i get-location),(i get-direction) )- (i get-large)/2.\r
                        wallEnd=dot((i get-location),(i get-direction) )+ (i get-large)/2.              \r
                        #Compruebo que el robot este frente a la pared\r
                        wallBegin=dot((i get-location),(i get-direction) )- (i get-large)/2.\r
                        wallEnd=dot((i get-location),(i get-direction) )+ (i get-large)/2.              \r


@@ -421,10 +420,22 @@ Link : CeldasSensor (aka CeldasSensors) {
                        yo=self get-location.\r
                        destiny=i get-direction.\r
 \r
                        yo=self get-location.\r
                        destiny=i get-direction.\r
 \r

-                       \r
+                       v = (body get-location) - (self get-location ).\r
+                       obsLoc::y=y=posObstacle::y.                                                             \r
+                               \r
+                       if (dot((i get-direction),(1,0,0))):\r
+                       {\r
+                        obsLoc::x=x=((self get-location)::x + ((posObstacle::z - (self get-location)::z)*v::x/v::z)).\r
+                         obsLoc::z=z=posObstacle::z.\r
+                       }                               \r
+                       else\r
+                       {\r
+                        obsLoc::z=z=((self get-location)::z + ((posObstacle::x - (self get-location)::x)*v::z/v::x)).\r
+                         obsLoc::x=x=posObstacle::x.\r
+                       }                                                       \r

 \r
 \r
 \r
 \r

-                       if (dot((self get-location),(i get-direction)) > wallBegin) && (dot((self get-location),(i get-direction)) < wallEnd):\r
+                       if (dot((obsLoc),(i get-direction)) > wallBegin) && (dot((obsLoc),(i get-direction)) < wallEnd):\r

                                des3=1.\r
                        else\r
                        {\r
                                des3=1.\r
                        else\r
                        {\r


@@ -434,41 +445,41 @@ Link : CeldasSensor (aka CeldasSensors) {
 \r
                        aux1=dot((self get-location),(i get-direction)).\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 "dist: $aux1 begin: $wallBegin end: $wallEnd".\r
-\r
-                       v = (body get-location) - (self get-location ).\r
+                       #print "sensor: $id obstaculo: $posObstacle direP: $destiny direS: $direction yo: $yo ".\r
+                       #print "dist: $aux1 begin: $wallBegin end: $wallEnd".\r

 \r
                        \r
 \r
                        \r

-\r

                        print "sensor: $id , des1: $des1, des2: $des2, des3: $des3".\r
                        print "sensor: $id , des1: $des1, des2: $des2, des3: $des3".\r

-                       if ((des1) && (des2) && (des3)):\r
-                        {                                    \r
+                       if ((des2) && (des3)):\r
+                         {                                    \r

                                draw clear.                             \r
                                draw clear.                             \r

-                               #print " posObstacle: $posObstacle".                        \r
-                               \r
-                               obsLoc::y=y=posObstacle::y.                                                             \r
-                               \r
-                               if (dot((i get-direction),(1,0,0))):\r
-                               {\r
-                                obsLoc::x=x=((self get-location)::x + ((posObstacle::z - (self get-location)::z)*v::x/v::z)).\r
-                                obsLoc::z=z=posObstacle::z.\r
-                               }                               \r
-                               else\r
-                               {\r
-                                obsLoc::z=z=((self get-location)::z + ((posObstacle::x - (self get-location)::x)*v::z/v::x)).\r
-                                obsLoc::x=x=posObstacle::x.\r
-                               }                               \r
-\r
-                               #Dibujo el laser\r
-       \r
-                               draw set-color to (1, 0, 0).\r
-                               draw draw-line from (self get-location) to obsLoc.\r
-                       }\r
+                               #print " posObstacle: $posObstacle".                                                    \r
+                                                       \r
+                               dist=|obsLoc - (self get-location)|.\r
+                               if( (j==0) || (min>dist) ):\r
+                                {\r
+                                       min=dist.\r
+                                       obs=obsLoc.\r
+                                       j++.    \r
+                                }\r
+\r
+                        }                                              \r
+\r

                         \r
                         \r

-               }\r
+               } #end for\r
+\r
+               if(j!=0):\r
+                       {\r
+                         #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
+                       }\r
+               \r
+\r

                 value = -1.\r
                 return value.\r
 \r
 \r
 }\r
                 value = -1.\r
                 return value.\r
 \r
 \r
 }\r

+               
\ No newline at end of file