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