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 5.\r
  10 @define CELDAS_TURNO 30.\r
  11 @define CELDAS_SENSOR_THRESHOLD 6.\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                 self rotate around-axis (0,1,0) by (-1.5709/CELDAS_TURNO)*tright. \r
 210 #!\r
 211                 if (tright == CELDAS_TURNO):\r
 212                 {\r
 213                   fSensor set-direction to (-1,0,0).\r
 214                   bSensor set-direction to (1,0,0).\r
 215                   lSensor set-direction to (0,0,-1).\r
 216                   rSensor set-direction to (0,0,1).\r
 217                 }\r
 218 !#\r
 219 \r
 220 \r
 221         + to turn-left:\r
 222                 tleft++.\r
 223                 self rotate around-axis (0,1,0) by (1.5709/CELDAS_TURNO)*tleft. \r
 224 #!\r
 225                 if (tleft == CELDAS_TURNO):\r
 226                 {\r
 227                   fSensor set-direction to (-1,0,0).\r
 228                   bSensor set-direction to (1,0,0).\r
 229                   lSensor set-direction to (0,0,-1).\r
 230                   rSensor set-direction to (0,0,1).\r
 231                 }\r
 232 !#\r
 233 \r
 234         + to get-sensor-value:\r
 235                 return (fSensor get-sensor-value).\r
 236 \r
 237 \r
 238 \r
 239         +to update-entorno:\r
 240                 entorno{"sensor_f"} = (fSensor get-sensor-value).\r
 241                 entorno{"sensor_b"} = (bSensor get-sensor-value).\r
 242                 entorno{"sensor_r"} = (rSensor get-sensor-value).\r
 243                 entorno{"sensor_l"} = (lSensor get-sensor-value).\r
 244                 sa update-entorno with entorno.            \r
 245 \r
 246         +to init:\r
 247                 # Configuracion de robot\r
 248                 fSensor = (self add-sensor at (2.0, .4, 0)).            \r
 249                 fSensor set-direction to (1,0,0).\r
 250                 #fSensor set-direction to (0,0,1).\r
 251                 fSensor set-id at 1.\r
 252                 fSensor set-body at self.\r
 253                 bSensor = (self add-sensor at (-2.0, .4, 0)).\r
 254                 bSensor set-direction to (-1,0,0).\r
 255                 #bSensor set-direction to (0,0,1).\r
 256                 bSensor set-id at 2.\r
 257                 bSensor set-body at self.\r
 258                 lSensor = (self add-sensor at (0, .4, 1.5)).\r
 259                 lSensor set-direction to (0,0,1).\r
 260                 #lSensor set-direction to (1,0,0).\r
 261                 lSensor set-id at 3.\r
 262                 lSensor set-body at self.\r
 263 \r
 264                 rSensor = (self add-sensor at (0, .4, -1.5)).\r
 265                 rSensor set-direction to (0,0,-1).\r
 266                 #rSensor set-direction to (-1,0,0).\r
 267                 rSensor set-id at 4.\r
 268                 rSensor set-body at self.\r
 269 \r
 270                 lfWheel = (self add-wheel at (2, 0, -1.5)).\r
 271                 lbWheel = (self add-wheel at (-2, 0, -1.5)).\r
 272                 rfWheel = (self add-wheel at (2, 0, 1.5)).\r
 273                 rbWheel = (self add-wheel at (-2, 0, 1.5)).\r
 274 \r
 275                 tleft=tright=0.\r
 276                 avanzando=0.\r
 277                 retrocediendo=0.\r
 278                 girando_izq=0.            \r
 279                 girando_der=0.            \r
 280 \r
 281                 posicion-inicial = (self get-location).\r
 282                 posicion-final = (0, 0, 0).\r
 283 \r
 284                 # Configuracion de sistema autonomo\r
 285                 sa = new SistemaAutonomo.\r
 286                 sa init with-max-pasos 4 with-max-teorias 15.\r
 287                 iterate = 0.\r
 288                 plan-finished = 1. # así planificamos apenas empezamos\r
 289 \r
 290                 teorias = 4 new Teorias.\r
 291                 teorias{0} init named "Avanzar" with-action "adelante".\r
 292                 teorias{0} set-dato-inicial name "sensor_f" value 0.\r
 293                 teorias{0} set-dato-inicial name "sensor_b" value ANY.\r
 294                 teorias{0} set-dato-inicial name "sensor_r" value ANY.\r
 295                 teorias{0} set-dato-inicial name "sensor_l" value ANY.\r
 296                 teorias{0} set-dato-inicial name "movido" value ANY.\r
 297                 teorias{0} set-dato-final name "sensor_f" value ANY.\r
 298                 teorias{0} set-dato-final name "sensor_b" value ANY.\r
 299                 teorias{0} set-dato-final name "sensor_r" value ANY.\r
 300                 teorias{0} set-dato-final name "sensor_l" value ANY.\r
 301                 teorias{0} set-dato-final name "movido" value 1.\r
 302 \r
 303                 teorias{1} init named "Retroceder" with-action "atras".# executed 2.\r
 304                 teorias{1} set-dato-inicial name "sensor_f" value 1.\r
 305                 teorias{1} set-dato-inicial name "sensor_b" value ANY.\r
 306                 teorias{1} set-dato-inicial name "sensor_r" value ANY.\r
 307                 teorias{1} set-dato-inicial name "sensor_l" value ANY.\r
 308                 teorias{1} set-dato-inicial name "movido" value ANY.\r
 309                 teorias{1} set-dato-final name "sensor_f" value 0.\r
 310                 teorias{1} set-dato-final name "sensor_b" value ANY.\r
 311                 teorias{1} set-dato-final name "sensor_r" value ANY.\r
 312                 teorias{1} set-dato-final name "sensor_l" value ANY.\r
 313                 teorias{1} set-dato-final name "movido" value 1.\r
 314 \r
 315                 teorias{2} init named "Rotar a derecha" with-action "derecha".\r
 316                 teorias{2} set-dato-inicial name "sensor_f" value 1.\r
 317                 teorias{2} set-dato-inicial name "sensor_b" value ANY.\r
 318                 teorias{2} set-dato-inicial name "sensor_r" value ANY.\r
 319                 teorias{2} set-dato-inicial name "sensor_l" value ANY.\r
 320                 teorias{2} set-dato-inicial name "movido" value ANY.\r
 321                 teorias{2} set-dato-final name "sensor_f" value 0.\r
 322                 teorias{2} set-dato-final name "sensor_b" value ANY.\r
 323                 teorias{2} set-dato-final name "sensor_r" value ANY.\r
 324                 teorias{2} set-dato-final name "sensor_l" value 1.\r
 325                 teorias{2} set-dato-final name "movido" value 0.\r
 326 \r
 327                 teorias{3} init named "Rotar a izquierda" with-action "izquierda". # executed 2.\r
 328                 teorias{3} set-dato-inicial name "sensor_f" value 1.\r
 329                 teorias{3} set-dato-inicial name "sensor_b" value ANY.\r
 330                 teorias{3} set-dato-inicial name "sensor_r" value ANY.\r
 331                 teorias{3} set-dato-inicial name "sensor_l" value ANY.\r
 332                 teorias{3} set-dato-inicial name "movido" value ANY.\r
 333                 teorias{3} set-dato-final name "sensor_f" value 0.\r
 334                 teorias{3} set-dato-final name "sensor_b" value ANY.\r
 335                 teorias{3} set-dato-final name "sensor_r" value 1.\r
 336                 teorias{3} set-dato-final name "sensor_l" value ANY.\r
 337                 teorias{3} set-dato-final name "movido" value 0.\r
 338 \r
 339                 sa add teoria teorias{0}.\r
 340                 sa add teoria teorias{1}.\r
 341                 sa add teoria teorias{2}.\r
 342                 sa add teoria teorias{3}.\r
 343 \r
 344                 datos-finales{"movido"} = 1.\r
 345                 sa update-datos-finales with datos-finales.\r
 346 \r
 347         +to iterate:\r
 348 \r
 349                 # Movimiento del robot\r
 350                 if (avanzando):\r
 351                 {\r
 352                         self set-global-velocity to (CELDAS_MAX_VELOCITY).\r
 353                 }\r
 354                 if (retrocediendo):\r
 355                 {\r
 356                         self set-global-velocity to (-CELDAS_MAX_VELOCITY).\r
 357                 }\r
 358                 if (girando_izq):\r
 359                 {\r
 360                         self set-global-velocity to 0.\r
 361                         self turn-left.\r
 362                         #if (iterate): self rotate around-axis (0,1,0) by (1.570796/CELDAS_TURNO)*iterate.\r
 363                         print "izq: ", (1.570796/CELDAS_TURNO)*iterate.\r
 364                 }\r
 365                 if (girando_der):\r
 366                 {\r
 367                         self set-global-velocity to 0.\r
 368                         self turn-right.\r
 369                         #if (iterate): self rotate around-axis (0,1,0) by (-1.570796/CELDAS_TURNO)*iterate. \r
 370                         print "der: ", (-1.570796/CELDAS_TURNO)*iterate.\r
 371                 }\r
 372                 #print "vel: ", (bodyLink get-velocity).\r
 373 \r
 374                 # Actualiza entorno\r
 375                 self update-entorno.\r
 376 \r
 377                 # Chequeo de objetivo\r
 378                 if (self near position posicion-final with-error 5.0):\r
 379                 {\r
 380                         print "Llegamos al FINAL!!!".\r
 381                         self set-global-velocity to 0.\r
 382                         return.\r
 383                 }\r
 384 \r
 385                 # Planificación\r
 386                 if (plan-finished):\r
 387                 {\r
 388                         # Actualiza entorno indicando que no se movió para que\r
 389                         # el planificador actue\r
 390                         sa set-entorno value 0 with-name "movido".\r
 391                         sa plan. # Si no tenemos plan, lo hacemos\r
 392                         plan-finished = 0.\r
 393                         iterate = 0.\r
 394                         if (! (sa has-next-theory)):\r
 395                         {\r
 396                                 plan-finished = 1.\r
 397                                 print "El planificador no encuentra PLAN!!!".\r
 398                                 return.\r
 399                         }\r
 400                 }\r
 401 \r
 402                 # Ejecución de teoría\r
 403                 if (iterate == 0):\r
 404                 {\r
 405                         posicion-inicial = (self get-location).\r
 406                         if (sa has-next-theory):\r
 407                         {\r
 408                                 teoria = sa get-next-theory.\r
 409                                 if ((teoria get-accion) == "adelante"):\r
 410                                 {\r
 411                                         avanzando = 1.\r
 412                                         retrocediendo = 0.\r
 413                                         girando_izq = 0.\r
 414                                         girando_der = 0.\r
 415                                 }\r
 416                                 if ((teoria get-accion) == "atras"):\r
 417                                 {\r
 418                                         avanzando = 0.\r
 419                                         retrocediendo = 1.\r
 420                                         girando_izq = 0.\r
 421                                         girando_der = 0.\r
 422                                 }\r
 423                                 if ((teoria get-accion) == "izquierda"):\r
 424                                 {\r
 425                                         avanzando = 0.\r
 426                                         retrocediendo = 0.\r
 427                                         girando_izq = 1.\r
 428                                         girando_der = 0.\r
 429                                 }\r
 430                                 if ((teoria get-accion) == "derecha"):\r
 431                                 {\r
 432                                         avanzando = 0.\r
 433                                         retrocediendo = 0.\r
 434                                         girando_izq = 0.\r
 435                                         girando_der = 1.\r
 436                                 }\r
 437                         }\r
 438                         else\r
 439                         {\r
 440                                 plan-finished = 1.\r
 441                         }\r
 442                 }\r
 443 \r
 444                 # Validación de teoría\r
 445                 if (iterate == CELDAS_TURNO):\r
 446                 {\r
 447                         # Actualiza entorno segun si se movio o no\r
 448                         if (self near position posicion-inicial with-error 1.0):\r
 449                         {\r
 450                                 sa set-entorno value 0 with-name "movido".\r
 451                         }\r
 452                         else\r
 453                         {\r
 454                                 sa set-entorno value 1 with-name "movido".\r
 455                         }\r
 456                         print iterate.\r
 457                         if (!(sa validate theory teoria)):\r
 458                         {\r
 459                                 plan-finished = 1.\r
 460                         }\r
 461                         iterate = 0.\r
 462                 }\r
 463                 else\r
 464                 {\r
 465                         iterate++.\r
 466                 }\r
 467 \r
 468 }\r
 469 \r
 470 Stationary : CeldasObstacle (aka CeldasObstacles) {\r
 471         % A CeldasObstacle is used in conjunction with OBJECT(CeldasControl)\r
 472         % and OBJECT(CeldasVehicle).  It is what the OBJECT(CeldasSensor)\r
 473         % objects on the CeldasVehicle detect.\r
 474         % <p>\r
 475         % There are no special behaviors associated with the walls--they're \r
 476         % basically just plain OBJECT(Stationary) objects.\r
 477    \r
 478         +variables:\r
 479             large (float).\r
 480             direction (vector). \r
 481 \r
 482 \r
 483         + 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
 484                 self init-with-shape shape (new Shape init-with-cube size theSize) color theColor at-location theLocation with-rotation theRotation.\r
 485                 large=20.\r
 486 \r
 487         + 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
 488                 self register with-shape theShape at-location theLocation with-rotation theRotation.\r
 489                 self set-color to theColor.\r
 490                 \r
 491         + to get-large:\r
 492             return large.\r
 493 \r
 494         + to set-direction at theDirection (vector):\r
 495             direction=theDirection.\r
 496 \r
 497         + to get-direction:\r
 498             return direction.\r
 499 }\r
 500 \r
 501 Link : CeldasWheel (aka CeldasWheels) {\r
 502         % A CeldasWheel is used in conjunction with OBJECT(CeldasVehicle)\r
 503         % to build Celdas vehicles.  This class is typically not instantiated\r
 504         % manually, since OBJECT(CeldasVehicle) creates one for you when you\r
 505         % add a wheel to the vehicle.\r
 506 \r
 507         + variables:\r
 508                 joint (object).\r
 509                 velocity (float).\r
 510 \r
 511         + to init:\r
 512                 velocity = 0.\r
 513 \r
 514         - to set-joint to j (object):\r
 515                 % Used internally.\r
 516 \r
 517                 joint = j.\r
 518 \r
 519         + section "Configuring the Wheel's Velocity"\r
 520 \r
 521         + to set-velocity to n (float):\r
 522                 % Sets the velocity of this wheel.\r
 523 \r
 524                 if n > CELDAS_MAX_VELOCITY: n = CELDAS_MAX_VELOCITY.\r
 525                 velocity = n.\r
 526 \r
 527                 joint set-joint-velocity to velocity.\r
 528 \r
 529         + to get-velocity:\r
 530                 % Gets the velocity of this wheel.\r
 531                 \r
 532                 return velocity.\r
 533 \r
 534 }\r
 535 \r
 536 Link : CeldasSensor (aka CeldasSensors) {\r
 537         % A CeldasSensor is used in conjunction with OBJECT(CeldasVehicle)\r
 538         % to build Celdas vehicles.  This class is typically not instantiated\r
 539         % manually, since OBJECT(CeldasVehicle) creates one for you when you\r
 540         % add a sensor to the vehicle.\r
 541 \r
 542         + variables:\r
 543                 direction (vector).\r
 544                 positiveDirection(vector).\r
 545                 sensorAngle (float).\r
 546                 value (float).\r
 547                 draw (object).\r
 548                 body(object).\r
 549                 id(int).\r
 550 \r
 551         + to init :\r
 552                 direction = (1,0,1).\r
 553                 positiveDirection= (1,0,1).\r
 554                 sensorAngle = 1.6.\r
 555                 value = 0.0.\r
 556                 draw = new Drawing.\r
 557                                 \r
 558 \r
 559   + section "Configuring the Sensor Values"\r
 560         + to set-id at n (int):\r
 561             id=n.\r
 562 \r
 563         + to set-body at robotBody(object):\r
 564                 body=robotBody.\r
 565                 \r
 566         + to set-sensor-angle to n (float):\r
 567                 % Sets the angle in which this sensor can detect obstacles.  The default\r
 568                 % value of 1.6 means that the sensor can see most of everything in\r
 569                 % front of it.  Setting the value to be any higher leads to general\r
 570                 % wackiness, so I don't suggest it.\r
 571 \r
 572                 sensorAngle = n.\r
 573 \r
 574         + to set-direction to n (vector):\r
 575                 direction = n.\r
 576                 positiveDirection::x=|n::x|.\r
 577                 positiveDirection::y=|n::y|.\r
 578                 positiveDirection::z=|n::z|.\r
 579 \r
 580   + section "Getting the Sensor Values"\r
 581 \r
 582         + to get-sensor-value:\r
 583                 % Gets the sensor value. This should be used from post-iterate,\r
 584                 % if not, the sensor reading correspond to the previous\r
 585                 % iteration.\r
 586                 val (float).\r
 587 \r
 588                 val = self get-data.\r
 589                 if (val > CELDAS_SENSOR_THRESHOLD): return 0.\r
 590                 else           return 1.\r
 591         \r
 592         #+ to iterate:\r
 593         \r
 594         + to get-data:\r
 595                 i (object).\r
 596                 min,dist (float).\r
 597                 v,obs(vector).\r
 598                 j (int).\r
 599                 des2,des3(int).\r
 600                 wallBegin,wallEnd,wallCenter (float).\r
 601                 obsLoc (vector).                \r
 602                 posObstacle,destiny,yo(vector).\r
 603                                              \r
 604                 draw clear.\r
 605                 value = 0.0.\r
 606                 j=0.\r
 607                 min=0.\r
 608                 foreach i in (all CeldasObstacles): \r
 609                         {\r
 610                          posObstacle=i get-location.\r
 611                          v = (body get-location) - (self get-location ).\r
 612                          obsLoc::y=posObstacle::y.\r
 613                          \r
 614                          if (dot((i get-direction),(1,0,0))):\r
 615                           {\r
 616                            obsLoc::x=((self get-location)::x + ((posObstacle::z - (self get-location)::z)*v::x/v::z)).\r
 617                            obsLoc::z=posObstacle::z.\r
 618                           }                             \r
 619                           else\r
 620                           {\r
 621                            obsLoc::z=((self get-location)::z + ((posObstacle::x - (self get-location)::x)*v::z/v::x)).\r
 622                            obsLoc::x=posObstacle::x.\r
 623                           } \r
 624                                                                 \r
 625                         #!\r
 626                         if(dot((i get-direction),direction)==0):\r
 627                                 des1=1.\r
 628                         else\r
 629                                 des1=0.\r
 630                         !#\r
 631 \r
 632                         des2=0.\r
 633                         if(dot(direction,(1,1,1))<0):\r
 634                         {                        \r
 635                             if(dot((body get-location),positiveDirection) > dot((self get-location),positiveDirection)):\r
 636                                 {\r
 637                                         if((dot((self get-location),positiveDirection))>(dot(obsLoc,positiveDirection))):\r
 638                                                 des2=1.\r
 639                                 }\r
 640                             else\r
 641                             if((dot((self get-location),positiveDirection))<(dot(obsLoc,positiveDirection))):\r
 642                                         des2=1.         \r
 643                         }\r
 644                         else\r
 645                         {\r
 646                             if(dot((body get-location),positiveDirection) < dot((self get-location),positiveDirection)):\r
 647                                 {\r
 648                                 if((dot((self get-location),positiveDirection))<(dot(obsLoc,positiveDirection))):\r
 649                                         des2=1.         \r
 650                                 }\r
 651                            else\r
 652                                 if((dot((self get-location),positiveDirection))>(dot(obsLoc,positiveDirection))):\r
 653                                                 des2=1.\r
 654                         \r
 655                         }                       \r
 656 \r
 657 \r
 658                         #Compruebo que el robot este frente a la pared\r
 659                         wallCenter=dot((i get-location),(i get-direction)).\r
 660                         wallBegin=wallCenter- (i get-large)/2.\r
 661                         wallEnd=wallCenter + (i get-large)/2.           \r
 662 \r
 663                         \r
 664                         yo=self get-location.\r
 665                         destiny=i get-direction.\r
 666 \r
 667                                                                                                                 \r
 668 \r
 669                         if (dot((self get-location),(i get-direction)) > wallBegin) && (dot((self get-location),(i get-direction)) < wallEnd):\r
 670                                 des3=1.\r
 671                         else\r
 672                         {\r
 673                                  des3=0.\r
 674                                  \r
 675                         }                               \r
 676                        \r
 677                         if ((des2) && (des3)):\r
 678                          {                                    \r
 679                                 draw clear.\r
 680 \r
 681                                 dist=|obsLoc - (self get-location)|.\r
 682                                 if( (j==0) || (min>dist) ):\r
 683                                  {\r
 684                                         min=dist.\r
 685                                         obs=obsLoc.\r
 686                                         j++.\r
 687                                         #print "sensor: $id obstaculo: $posObstacle direP: $destiny direS: $direction yo: $yo ".        \r
 688                                  }\r
 689 \r
 690                          }                                              \r
 691 \r
 692                         \r
 693                 } #end for\r
 694 \r
 695                 if(j!=0):\r
 696                         {\r
 697                           #Dibujo el laser\r
 698                           draw set-color to (1, 0, 0).\r
 699                           draw draw-line from (self get-location) to (obs).\r
 700                           return min.\r
 701                         }\r
 702                 \r
 703 \r
 704                 value = -1.\r
 705                 return value.\r
 706 \r
 707 \r
 708 }\r
 709                 \r