]> git.llucax.com Git - z.facultad/75.68/celdas.git/blob - trunk/src/breve/Celdas.tz
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
1 @use PhysicalControl.\r
2 @use Shape.\r
3 @use Stationary.\r
4 @use Link.\r
5 @use MultiBody.\r
6 @use Drawing.\r
7 @use SistemaAutonomo.\r
8 \r
9 @define CELDAS_MAX_VELOCITY 30.\r
10 @define CELDAS_TURNO 100.\r
11 @define CELDAS_SENSOR_THRESHOLD 10.\r
12 \r
13 PhysicalControl : CeldasControl {\r
14         % This class is used for building simple vehicle \r
15         % simulations.  To create a vehicle simulation, \r
16         % subclass CeldasControl and use the init method to \r
17         % create OBJECT(CeldasObstacle) and \r
18         % OBJECT(CeldasVehicle) objects.\r
19 \r
20         + variables:\r
21                 floor (object).\r
22                 floorShape (object).\r
23                 cloudTexture (object).\r
24 \r
25 \r
26         + to init:\r
27                 self enable-lighting.\r
28                 #self enable-smooth-drawing.\r
29 \r
30                 floorShape = new Shape.\r
31                 floorShape init-with-cube size (200, .2, 200).\r
32 \r
33                 floor = new Stationary.\r
34                 floor register with-shape floorShape at-location (0, 0, 0).\r
35                 #floor catch-shadows.\r
36 \r
37                 self point-camera at (0, 0, 0) from (3, 3, 24).\r
38 \r
39                 #self enable-shadows.\r
40                 #self enable-reflections.\r
41 \r
42                 cloudTexture = (new Image load from "images/clouds.png"). \r
43                 self set-background-color to (.4, .6, .9).\r
44                 self set-background-texture-image to cloudTexture.\r
45 \r
46 }\r
47 \r
48 MultiBody : CeldasLightVehicle (aka CeldasLightVehicles) {\r
49         % This object is used in conjunction with OBJECT(CeldasControl) to\r
50         % create simple vehicles.\r
51 \r
52         + variables:\r
53                 bodyShape (object).\r
54                 wheelShape (object).\r
55                 sensorShape (object).\r
56                 bodyLink (object).\r
57 \r
58                 wheels (list).\r
59 \r
60         + to init:\r
61                 bodyShape = new Shape.\r
62                 bodyShape init-with-cube size (4.0, .75, 3.0).  \r
63 \r
64                 wheelShape = new Shape.\r
65                 wheelShape init-with-polygon-disk radius ( self get-wheel-radius ) sides 20 height ( self get-wheel-width ).\r
66                 # 40\r
67 \r
68                 sensorShape = new Shape.\r
69                 sensorShape init-with-polygon-cone radius .2 sides 5 height .5.\r
70                 # 10\r
71 \r
72                 bodyShape set-density to ( self get-density ).\r
73                 bodyLink = new Link.\r
74                 bodyLink set-shape to bodyShape.        \r
75                 bodyLink set-mu to -1.0.\r
76                 bodyLink set-eT to .8.\r
77 \r
78                 self set-root to bodyLink.\r
79 \r
80                 self move to (0, 0.9, 0).\r
81                 self set-texture-scale to 1.5.\r
82 \r
83         - to get-density:\r
84                 return 1.0.\r
85 \r
86         - to get-wheel-width:\r
87                 return 0.1.\r
88 \r
89         - to get-wheel-radius:\r
90                 return 0.6.\r
91 \r
92         + section "Adding Wheels and Sensors to a Vehicle"\r
93 \r
94         + to add-wheel at location (vector):\r
95                 % Adds a wheel at location on the vehicle.  This method returns\r
96                 % the wheel which is created, a OBJECT(CeldasWheel).\r
97 \r
98                 wheel, joint (object).\r
99 \r
100                 wheel = new CeldasWheel.\r
101                 wheel set-shape to wheelShape.\r
102 \r
103                 joint = new RevoluteJoint.\r
104 \r
105                 joint set-relative-rotation around-axis (1, 0, 0) by 1.5708.\r
106                 joint link parent bodyLink to-child wheel with-normal (0, 0, 1)\r
107                                         with-parent-point location with-child-point (0, 0, 0).\r
108 \r
109                 wheel set-eT to .8.\r
110                 wheel set-texture to 0.\r
111                 wheel set-joint to joint.\r
112                 joint set-strength-limit to (joint get-strength-hard-limit) / 2.\r
113                 wheel set-color to (.6, .6, .6).\r
114                 wheel set-mu to 100000.\r
115 \r
116                 self add-dependency on joint.\r
117                 self add-dependency on wheel.\r
118 \r
119                 push wheel onto wheels.\r
120 \r
121                 return wheel.\r
122 \r
123         + to add-sensor at location (vector) with-direction direction = (0,1,0)(vector) :\r
124                 % Adds a sensor at location on the vehicle.  This method returns\r
125                 % the sensor which is created, a OBJECT(CeldasSensor).\r
126 \r
127                 sensor, joint (object).\r
128 \r
129                 sensor = new CeldasSensor.\r
130                 sensor set-direction to direction.\r
131                 \r
132                 sensor set-shape to sensorShape.\r
133 \r
134                 joint = new RevoluteJoint.\r
135 \r
136                 joint set-relative-rotation around-axis (0, 0, 1) by -1.57.\r
137                 joint link parent bodyLink to-child sensor with-normal (1, 0, 0)\r
138                                         with-parent-point location with-child-point (0, 0, 0).\r
139 \r
140                 joint set-double-spring with-strength 300 with-max 0.01 with-min -0.01.\r
141 \r
142                 self add-dependency on joint.\r
143                 self add-dependency on sensor.\r
144 \r
145                 sensor set-color to (0, 0, 0).\r
146 \r
147                 #push sensor onto sensors.\r
148 \r
149                 return sensor.\r
150 \r
151         + to destroy:\r
152                 free sensorShape.\r
153                 free wheelShape.\r
154                 free bodyShape.\r
155 \r
156                 super destroy.\r
157 }\r
158 \r
159 CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {\r
160         % A heavy duty version of OBJECT(CeldasLightVehicle), this\r
161         % vehicle is heavier and harder to control, but more stable\r
162         % at higher speeds.\r
163         +variables:\r
164                 lSensor, rSensor, fSensor, bSensor (object).\r
165                 lfWheel,rfWheel,lbWheel,rbWheel (object).\r
166                 tleft,tright (int).         \r
167                 avanzando,retrocediendo,girando_izq,girando_der(int).       \r
168                 iterate(int).\r
169                 teorias (list).\r
170                 sa (object).\r
171                 teoria (object).\r
172                 entorno (hash).\r
173                 datos-finales (hash).\r
174                 plan-finished (int).\r
175                 posicion-inicial (vector).\r
176                 posicion-final (vector).\r
177         \r
178         - to get-density:\r
179                 return 20.0.\r
180 \r
181         - to get-wheel-width:\r
182                 return 0.4.\r
183 \r
184         - to get-wheel-radius:\r
185                 return 0.8.\r
186 \r
187         - to near position thePosition (vector) with-error error (float):\r
188                 vectorAux(vector).\r
189                 vectorAux = (self get-location) - thePosition.\r
190 \r
191                 #print "-----> (pos, other_pos, diff, error): ", (self get-location), thePosition, vectorAux, error.\r
192 \r
193                 if ((|vectorAux::x| < error) && (|vectorAux::z| < error)):\r
194                         return 1.\r
195 \r
196                 return 0.\r
197 \r
198         + to set-global-velocity to velocity (float):\r
199                 rfWheel set-velocity to velocity.\r
200                 lfWheel set-velocity to velocity.\r
201                 rbWheel set-velocity to velocity.\r
202                 lbWheel set-velocity to velocity.\r
203 \r
204         + to get-global-velocity:\r
205                 return ((rfWheel get-velocity) + (lfWheel get-velocity)) / 2.\r
206 \r
207         + to turn-right:                \r
208                 tright++.\r
209 \r
210                 self rotate around-axis (0,1,0) by (-1.5709/CELDAS_TURNO)*tright. \r
211                         \r
212                 if (tright == CELDAS_TURNO): tright=0.\r
213 \r
214 \r
215         + to turn-left:\r
216                 tleft++.\r
217 \r
218                 self rotate around-axis (0,1,0) by (1.5709/CELDAS_TURNO)*tleft. \r
219                         \r
220                 if (tleft == CELDAS_TURNO): tleft=0.\r
221 \r
222 \r
223         + to get-sensor-value:\r
224                 return (fSensor get-sensor-value).\r
225 \r
226 \r
227 \r
228         +to update-entorno:\r
229                 entorno{"sensor_f"} = (fSensor get-sensor-value).\r
230                 entorno{"sensor_b"} = (bSensor get-sensor-value).\r
231                 entorno{"sensor_r"} = (rSensor get-sensor-value).\r
232                 entorno{"sensor_l"} = (lSensor get-sensor-value).\r
233                 sa update-entorno with entorno.            \r
234 \r
235         +to init:\r
236                 # Configuracion de robot\r
237                 fSensor = (self add-sensor at (2.0, .4, 0)).            \r
238                 fSensor set-direction to (1,0,0).\r
239                 #fSensor set-direction to (0,0,1).\r
240                 fSensor set-id at 1.\r
241                 fSensor set-body at self.\r
242                 bSensor = (self add-sensor at (-2.0, .4, 0)).\r
243                 bSensor set-direction to (-1,0,0).\r
244                 #bSensor set-direction to (0,0,1).\r
245                 bSensor set-id at 2.\r
246                 bSensor set-body at self.\r
247                 lSensor = (self add-sensor at (0, .4, 1.5)).\r
248                 lSensor set-direction to (0,0,1).\r
249                 #lSensor set-direction to (1,0,0).\r
250                 lSensor set-id at 3.\r
251                 lSensor set-body at self.\r
252 \r
253                 rSensor = (self add-sensor at (0, .4, -1.5)).\r
254                 rSensor set-direction to (0,0,-1).\r
255                 #rSensor set-direction to (-1,0,0).\r
256                 rSensor set-id at 4.\r
257                 rSensor set-body at self.\r
258 \r
259                 lfWheel = (self add-wheel at (2, 0, -1.5)).\r
260                 lbWheel = (self add-wheel at (-2, 0, -1.5)).\r
261                 rfWheel = (self add-wheel at (2, 0, 1.5)).\r
262                 rbWheel = (self add-wheel at (-2, 0, 1.5)).\r
263 \r
264                 tleft=tright=0.\r
265                 avanzando=0.\r
266                 retrocediendo=0.\r
267                 girando_izq=0.            \r
268                 girando_der=0.            \r
269 \r
270                 posicion-inicial = (self get-location).\r
271                 posicion-final = (0, 0, 0).\r
272 \r
273                 # Configuracion de sistema autonomo\r
274                 sa = new SistemaAutonomo.\r
275                 sa init with-max-pasos 4 with-max-teorias 15.\r
276                 iterate = 0.\r
277                 plan-finished = 1. # así planificamos apenas empezamos\r
278 \r
279                 teorias = 4 new Teorias.\r
280                 teorias{0} init named "Avanzar" with-action "adelante".\r
281                 teorias{0} set-dato-inicial name "sensor_f" value 0.\r
282                 teorias{0} set-dato-inicial name "sensor_b" value ANY.\r
283                 teorias{0} set-dato-inicial name "sensor_r" value ANY.\r
284                 teorias{0} set-dato-inicial name "sensor_l" value ANY.\r
285                 teorias{0} set-dato-inicial name "movido" value ANY.\r
286                 teorias{0} set-dato-final name "sensor_f" value ANY.\r
287                 teorias{0} set-dato-final name "sensor_b" value ANY.\r
288                 teorias{0} set-dato-final name "sensor_r" value ANY.\r
289                 teorias{0} set-dato-final name "sensor_l" value ANY.\r
290                 teorias{0} set-dato-final name "movido" value 1.\r
291 \r
292                 teorias{1} init named "Retroceder" with-action "atras".\r
293                 teorias{1} set-dato-inicial name "sensor_f" value 1.\r
294                 teorias{1} set-dato-inicial name "sensor_b" value ANY.\r
295                 teorias{1} set-dato-inicial name "sensor_r" value ANY.\r
296                 teorias{1} set-dato-inicial name "sensor_l" value ANY.\r
297                 teorias{1} set-dato-inicial name "movido" value ANY.\r
298                 teorias{1} set-dato-final name "sensor_f" value 0.\r
299                 teorias{1} set-dato-final name "sensor_b" value ANY.\r
300                 teorias{1} set-dato-final name "sensor_r" value ANY.\r
301                 teorias{1} set-dato-final name "sensor_l" value ANY.\r
302                 teorias{1} set-dato-final name "movido" value 1.\r
303 \r
304                 teorias{2} init named "Rotar a derecha" with-action "derecha".\r
305                 teorias{2} set-dato-inicial name "sensor_f" value 1.\r
306                 teorias{2} set-dato-inicial name "sensor_b" value ANY.\r
307                 teorias{2} set-dato-inicial name "sensor_r" value ANY.\r
308                 teorias{2} set-dato-inicial name "sensor_l" value ANY.\r
309                 teorias{2} set-dato-inicial name "movido" value ANY.\r
310                 teorias{2} set-dato-final name "sensor_f" value 0.\r
311                 teorias{2} set-dato-final name "sensor_b" value ANY.\r
312                 teorias{2} set-dato-final name "sensor_r" value ANY.\r
313                 teorias{2} set-dato-final name "sensor_l" value 1.\r
314                 teorias{2} set-dato-final name "movido" value 0.\r
315 \r
316                 teorias{3} init named "Rotar a izquierda" with-action "izquierda".\r
317                 teorias{3} set-dato-inicial name "sensor_f" value 1.\r
318                 teorias{3} set-dato-inicial name "sensor_b" value ANY.\r
319                 teorias{3} set-dato-inicial name "sensor_r" value ANY.\r
320                 teorias{3} set-dato-inicial name "sensor_l" value ANY.\r
321                 teorias{3} set-dato-inicial name "movido" value ANY.\r
322                 teorias{3} set-dato-final name "sensor_f" value 0.\r
323                 teorias{3} set-dato-final name "sensor_b" value ANY.\r
324                 teorias{3} set-dato-final name "sensor_r" value 1.\r
325                 teorias{3} set-dato-final name "sensor_l" value ANY.\r
326                 teorias{3} set-dato-final name "movido" value 0.\r
327 \r
328                 sa add teoria teorias{0}.\r
329                 sa add teoria teorias{1}.\r
330                 sa add teoria teorias{2}.\r
331                 sa add teoria teorias{3}.\r
332 \r
333                 datos-finales{"movido"} = 1.\r
334                 sa update-datos-finales with datos-finales.\r
335 \r
336         +to iterate:\r
337 \r
338                 # Actualiza entorno\r
339                 self update-entorno.\r
340 \r
341                 # Chequeo de objetivo\r
342                 if (self near position posicion-final with-error 5.0):\r
343                 {\r
344                         print "Llegamos al FINAL!!!".\r
345                         self set-global-velocity to 0.\r
346                         return.\r
347                 }\r
348 \r
349                 # Planificación\r
350                 if (plan-finished):\r
351                 {\r
352                         # Actualiza entorno indicando que no se movió para que\r
353                         # el planificador actue\r
354                         sa set-entorno value 0 with-name "movido".\r
355                         sa plan. # Si no tenemos plan, lo hacemos\r
356                         plan-finished = 0.\r
357                         iterate = 0.\r
358                         if (! (sa has-next-theory)):\r
359                         {\r
360                                 plan-finished = 1.\r
361                                 print "El planificador no encuentra PLAN!!!".\r
362                                 return.\r
363                         }\r
364                 }\r
365 \r
366                 # Ejecución de teoría\r
367                 if (iterate == 0):\r
368                 {\r
369                         posicion-inicial = (self get-location).\r
370                         if (sa has-next-theory):\r
371                         {\r
372                                 teoria = sa get-next-theory.\r
373                                 if ((teoria get-accion) == "adelante"):\r
374                                 {\r
375                                         avanzando = 1.\r
376                                         retrocediendo = 0.\r
377                                         girando_izq = 0.\r
378                                         girando_der = 0.\r
379                                 }\r
380                                 if ((teoria get-accion) == "atras"):\r
381                                 {\r
382                                         avanzando = 0.\r
383                                         retrocediendo = 1.\r
384                                         girando_izq = 0.\r
385                                         girando_der = 0.\r
386                                 }\r
387                                 if ((teoria get-accion) == "izquierda"):\r
388                                 {\r
389                                         avanzando = 0.\r
390                                         retrocediendo = 0.\r
391                                         girando_izq = 1.\r
392                                         girando_der = 0.\r
393                                 }\r
394                                 if ((teoria get-accion) == "derecha"):\r
395                                 {\r
396                                         avanzando = 0.\r
397                                         retrocediendo = 0.\r
398                                         girando_izq = 0.\r
399                                         girando_der = 1.\r
400                                 }\r
401                         }\r
402                 }\r
403 \r
404                 # Validación de teoría\r
405                 if (iterate == CELDAS_TURNO):\r
406                 {\r
407                         # Actualiza entorno segun si se movio o no\r
408                         if (self near position posicion-inicial with-error 1.0):\r
409                         {\r
410                                 sa set-entorno value 0 with-name "movido".\r
411                         }\r
412                         else\r
413                         {\r
414                                 sa set-entorno value 1 with-name "movido".\r
415                         }\r
416                         print iterate.\r
417                         if (!(sa validate theory teoria)):\r
418                         {\r
419                                 plan-finished = 1.\r
420                         }\r
421                         iterate = 0.\r
422                 }\r
423                 else\r
424                 {\r
425                         iterate++.\r
426                 }\r
427 \r
428                 # Movimiento del robot\r
429                 if (avanzando):\r
430                         self set-global-velocity to (15).\r
431                 if (retrocediendo):\r
432                         self set-global-velocity to (-15).\r
433                 if (girando_izq):\r
434                         self turn-left.\r
435                 if (girando_der):\r
436                         self turn-right.\r
437 \r
438 }\r
439 \r
440 Stationary : CeldasObstacle (aka CeldasObstacles) {\r
441         % A CeldasObstacle is used in conjunction with OBJECT(CeldasControl)\r
442         % and OBJECT(CeldasVehicle).  It is what the OBJECT(CeldasSensor)\r
443         % objects on the CeldasVehicle detect.\r
444         % <p>\r
445         % There are no special behaviors associated with the walls--they're \r
446         % basically just plain OBJECT(Stationary) objects.\r
447    \r
448         +variables:\r
449             large (float).\r
450             direction (vector). \r
451 \r
452 \r
453         + to init with-size theSize = (10, 3, .1) (vector) with-color theColor = (1, 0, 0) (vector) at-location theLocation = (0, 0, 0) (vector) with-rotation theRotation = [ ( 0, 0, 1 ), ( 0, 1, 0 ), ( 1, 0, 0 ) ] (matrix):                    \r
454                 self init-with-shape shape (new Shape init-with-cube size theSize) color theColor at-location theLocation with-rotation theRotation.\r
455                 large=20.\r
456 \r
457         + to init-with-shape shape theShape (object) color theColor = (1, 0, 0) (vector) at-location theLocation = (0, 0, 0) (vector) with-rotation theRotation = [ ( 1, 0, 0 ), ( 0, 1, 0 ), ( 0, 0, 1 ) ] (matrix):\r
458                 self register with-shape theShape at-location theLocation with-rotation theRotation.\r
459                 self set-color to theColor.\r
460                 \r
461         + to get-large:\r
462             return large.\r
463 \r
464         + to set-direction at theDirection (vector):\r
465             direction=theDirection.\r
466 \r
467         + to get-direction:\r
468             return direction.\r
469 }\r
470 \r
471 Link : CeldasWheel (aka CeldasWheels) {\r
472         % A CeldasWheel is used in conjunction with OBJECT(CeldasVehicle)\r
473         % to build Celdas vehicles.  This class is typically not instantiated\r
474         % manually, since OBJECT(CeldasVehicle) creates one for you when you\r
475         % add a wheel to the vehicle.\r
476 \r
477         + variables:\r
478                 joint (object).\r
479                 velocity (float).\r
480 \r
481         + to init:\r
482                 velocity = 0.\r
483 \r
484         - to set-joint to j (object):\r
485                 % Used internally.\r
486 \r
487                 joint = j.\r
488 \r
489         + section "Configuring the Wheel's Velocity"\r
490 \r
491         + to set-velocity to n (float):\r
492                 % Sets the velocity of this wheel.\r
493 \r
494                 if n > CELDAS_MAX_VELOCITY: n = CELDAS_MAX_VELOCITY.\r
495                 velocity = n.\r
496 \r
497                 joint set-joint-velocity to velocity.\r
498 \r
499         + to get-velocity:\r
500                 % Gets the velocity of this wheel.\r
501                 \r
502                 return velocity.\r
503 \r
504 }\r
505 \r
506 Link : CeldasSensor (aka CeldasSensors) {\r
507         % A CeldasSensor is used in conjunction with OBJECT(CeldasVehicle)\r
508         % to build Celdas vehicles.  This class is typically not instantiated\r
509         % manually, since OBJECT(CeldasVehicle) creates one for you when you\r
510         % add a sensor to the vehicle.\r
511 \r
512         + variables:\r
513                 direction (vector).\r
514                 positiveDirection(vector).\r
515                 sensorAngle (float).\r
516                 value (float).\r
517                 draw (object).\r
518                 body(object).\r
519                 id(int).\r
520 \r
521         + to init :\r
522                 direction = (1,0,1).\r
523                 positiveDirection= (1,0,1).\r
524                 sensorAngle = 1.6.\r
525                 value = 0.0.\r
526                 draw = new Drawing.\r
527                                 \r
528 \r
529   + section "Configuring the Sensor Values"\r
530         + to set-id at n (int):\r
531             id=n.\r
532 \r
533         + to set-body at robotBody(object):\r
534                 body=robotBody.\r
535                 \r
536         + to set-sensor-angle to n (float):\r
537                 % Sets the angle in which this sensor can detect obstacles.  The default\r
538                 % value of 1.6 means that the sensor can see most of everything in\r
539                 % front of it.  Setting the value to be any higher leads to general\r
540                 % wackiness, so I don't suggest it.\r
541 \r
542                 sensorAngle = n.\r
543 \r
544         + to set-direction to n (vector):\r
545                 direction = n.\r
546                 positiveDirection::x=|n::x|.\r
547                 positiveDirection::y=|n::y|.\r
548                 positiveDirection::z=|n::z|.\r
549 \r
550   + section "Getting the Sensor Values"\r
551 \r
552         + to get-sensor-value:\r
553                 % Gets the sensor value. This should be used from post-iterate,\r
554                 % if not, the sensor reading correspond to the previous\r
555                 % iteration.\r
556                 val (float).\r
557 \r
558                 val = self get-data.\r
559                 if (val > CELDAS_SENSOR_THRESHOLD): return 0.\r
560                 else           return 1.\r
561         \r
562         #+ to iterate:\r
563         \r
564         + to get-data:\r
565                 i (object).\r
566                 min,dist (float).\r
567                 v,obs(vector).\r
568                 j (int).\r
569                 des2,des3(int).\r
570                 wallBegin,wallEnd,wallCenter (float).\r
571                 obsLoc (vector).                \r
572                 posObstacle,destiny,yo(vector).\r
573                                              \r
574                 draw clear.\r
575                 value = 0.0.\r
576                 j=0.\r
577                 min=0.\r
578                 foreach i in (all CeldasObstacles): \r
579                         {\r
580                          posObstacle=i get-location.\r
581                          v = (body get-location) - (self get-location ).\r
582                          obsLoc::y=posObstacle::y.\r
583                          \r
584                          if (dot((i get-direction),(1,0,0))):\r
585                           {\r
586                            obsLoc::x=((self get-location)::x + ((posObstacle::z - (self get-location)::z)*v::x/v::z)).\r
587                            obsLoc::z=posObstacle::z.\r
588                           }                             \r
589                           else\r
590                           {\r
591                            obsLoc::z=((self get-location)::z + ((posObstacle::x - (self get-location)::x)*v::z/v::x)).\r
592                            obsLoc::x=posObstacle::x.\r
593                           } \r
594                                                                 \r
595                         #!\r
596                         if(dot((i get-direction),direction)==0):\r
597                                 des1=1.\r
598                         else\r
599                                 des1=0.\r
600                         !#\r
601 \r
602                         des2=0.\r
603                         if(dot(direction,(1,1,1))<0):\r
604                         {                        \r
605                             if((dot((self get-location),positiveDirection))>(dot(obsLoc,positiveDirection))):\r
606                                         des2=1.      \r
607                         }\r
608                         else\r
609                         {\r
610                             if((dot((self get-location),positiveDirection))<(dot(obsLoc,positiveDirection))):\r
611                                         des2=1.         \r
612                         }                       \r
613 \r
614 \r
615                         #Compruebo que el robot este frente a la pared\r
616                         wallCenter=dot((i get-location),(i get-direction)).\r
617                         wallBegin=wallCenter- (i get-large)/2.\r
618                         wallEnd=wallCenter + (i get-large)/2.           \r
619 \r
620                         \r
621                         yo=self get-location.\r
622                         destiny=i get-direction.\r
623 \r
624                                                                                                                 \r
625 \r
626                         if (dot((self get-location),(i get-direction)) > wallBegin) && (dot((self get-location),(i get-direction)) < wallEnd):\r
627                                 des3=1.\r
628                         else\r
629                         {\r
630                                  des3=0.\r
631                                  \r
632                         }                               \r
633                        \r
634                         if ((des2) && (des3)):\r
635                          {                                    \r
636                                 draw clear.\r
637 \r
638                                 dist=|obsLoc - (self get-location)|.\r
639                                 if( (j==0) || (min>dist) ):\r
640                                  {\r
641                                         min=dist.\r
642                                         obs=obsLoc.\r
643                                         j++.\r
644                                         #print "sensor: $id obstaculo: $posObstacle direP: $destiny direS: $direction yo: $yo ".        \r
645                                  }\r
646 \r
647                          }                                              \r
648 \r
649                         \r
650                 } #end for\r
651 \r
652                 if(j!=0):\r
653                         {\r
654                           #Dibujo el laser\r
655                           draw set-color to (1, 0, 0).\r
656                           draw draw-line from (self get-location) to (obs).\r
657                           return min.\r
658                         }\r
659                 \r
660 \r
661                 value = -1.\r
662                 return value.\r
663 \r
664 \r
665 }\r
666                 \r