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                 lfWheel,rfWheel,lbWheel,rbWheel (object).\r
 165                 tleft,tright (int).         \r
 166                 iterate(int).\r
 167                 teorias (list).\r
 168                 sa (object).\r
 169                 teoria (object).\r
 170                 entorno (hash).\r
 171                 datos-finales (hash).\r
 172                 avanzando,retrocediendo,girando(int).       \r
 173         \r
 174         - to get-density:\r
 175                 return 20.0.\r
 176 \r
 177         - to get-wheel-width:\r
 178                 return 0.4.\r
 179 \r
 180         - to get-wheel-radius:\r
 181                 return 0.8.\r
 182 \r
 183         + to set-global-velocity to velocity (float):\r
 184                 rfWheel set-velocity to velocity.\r
 185                 lfWheel set-velocity to velocity.\r
 186                 rbWheel set-velocity to velocity.\r
 187                 lbWheel set-velocity to velocity.\r
 188 \r
 189         + to get-global-velocity:\r
 190                 return ((rfWheel get-velocity) + (lfWheel 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 update-entorno:\r
 212                 entorno{"sensor_f"} = (fSensor get-data).\r
 213                 entorno{"sensor_b"} = (bSensor get-data).\r
 214                 entorno{"sensor_r"} = (rSensor get-data).\r
 215                 entorno{"sensor_l"} = (lSensor get-data).\r
 216                 entorno{"movido"} = 0. # TODO\r
 217                 sa update-entorno entorno entorno.            \r
 218 \r
 219         +to init:\r
 220                 sa = new SistemaAutonomo.\r
 221                 iterate=0.\r
 222 \r
 223                 teorias = 2 new Teorias.\r
 224                 teorias{0} init named "Avanzar" with-action "avanzar".\r
 225                 teorias{0} set-dato-inicial name "sensor_f" value 0.\r
 226                 teorias{0} set-dato-inicial name "sensor_b" value ANY.\r
 227                 teorias{0} set-dato-inicial name "sensor_r" value ANY.\r
 228                 teorias{0} set-dato-inicial name "sensor_l" value ANY.\r
 229                 teorias{0} set-dato-inicial name "movido" value ANY.\r
 230                 teorias{0} set-dato-final name "sensor_f" value ANY.\r
 231                 teorias{0} set-dato-final name "sensor_b" value ANY.\r
 232                 teorias{0} set-dato-final name "sensor_r" value ANY.\r
 233                 teorias{0} set-dato-final name "sensor_l" value ANY.\r
 234                 teorias{0} set-dato-final name "movido" value 1.\r
 235 \r
 236                 teorias{1} init named "Rotar a derecha" with-action "derecha".\r
 237                 teorias{1} set-dato-inicial name "sensor_f" value 1.\r
 238                 teorias{1} set-dato-inicial name "sensor_b" value ANY.\r
 239                 teorias{1} set-dato-inicial name "sensor_r" value ANY.\r
 240                 teorias{1} set-dato-inicial name "sensor_l" value ANY.\r
 241                 teorias{1} set-dato-inicial name "movido" value ANY.\r
 242                 teorias{1} set-dato-final name "sensor_f" value 0.\r
 243                 teorias{1} set-dato-final name "sensor_b" value ANY.\r
 244                 teorias{1} set-dato-final name "sensor_r" value ANY.\r
 245                 teorias{1} set-dato-final name "sensor_l" value ANY.\r
 246                 teorias{1} set-dato-final name "movido" value 0.\r
 247 \r
 248                 sa add teoria teorias{0}.\r
 249                 sa add teoria teorias{1}.\r
 250 \r
 251                 fSensor = (self add-sensor at (2.0, .4, 0)).            \r
 252                 fSensor set-direction to (1,0,0).\r
 253                 #fSensor set-direction to (0,0,1).\r
 254                 fSensor set-id at 1.\r
 255                 fSensor set-body at self.\r
 256                 bSensor = (self add-sensor at (-2.0, .4, 0)).\r
 257                 bSensor set-direction to (-1,0,0).\r
 258                 #bSensor set-direction to (0,0,1).\r
 259                 bSensor set-id at 2.\r
 260                 bSensor set-body at self.\r
 261                 lSensor = (self add-sensor at (0, .4, 1.5)).\r
 262                 lSensor set-direction to (0,0,1).\r
 263                 #lSensor set-direction to (1,0,0).\r
 264                 lSensor set-id at 3.\r
 265                 lSensor set-body at self.\r
 266 \r
 267 \r
 268                 rSensor = (self add-sensor at (0, .4, -1.5)).\r
 269                 rSensor set-direction to (0,0,-1).\r
 270                 #rSensor set-direction to (-1,0,0).\r
 271                 rSensor set-id at 4.\r
 272                 rSensor set-body at self.\r
 273 \r
 274 \r
 275                 lfWheel = (self add-wheel at (2, 0, -1.5)).\r
 276                 lbWheel = (self add-wheel at (-2, 0, -1.5)).\r
 277                 rfWheel = (self add-wheel at (2, 0, 1.5)).\r
 278                 rbWheel = (self add-wheel at (-2, 0, 1.5)).\r
 279 \r
 280                 tleft=tright=0. #Debe ser inicializado en 0 esta asi para probar!!!!!!!!!!!!!!!!!!!!!!!!\r
 281                 avanzando=1.\r
 282                 retrocediendo=0.\r
 283                 girando=0.            \r
 284 \r
 285         +to iterate:\r
 286                 valuef,valueb,valuer,valuel (float).\r
 287                 fl, fr(float).\r
 288 \r
 289                 valuef=fSensor get-data.\r
 290                 valueb=bSensor get-data.\r
 291                 valuel=lSensor get-data.\r
 292                 valuer=rSensor get-data.\r
 293 \r
 294 \r
 295                 datos-finales{"movido"} = 1.\r
 296                 print "senforl: $valuel".\r
 297 \r
 298 \r
 299                 sa update-datos-finales datos-finales datos-finales.\r
 300                 sa plan.\r
 301 \r
 302                 if (iterate==0): {\r
 303                         print "iteracion 0".\r
 304                                 if (sa has-next-theory):\r
 305                         {\r
 306 \r
 307                                 print "hay teoria".\r
 308                                         teoria = sa get-next-theory.\r
 309                                         if ((teoria get-accion) == "avanza"): {\r
 310                                                 #entorno{"sensor"} = 1.\r
 311                                                 #entorno{"movido"} = 1.\r
 312                                                 self set-global-velocity.\r
 313 \r
 314                                         }\r
 315                                 if ((teoria get-accion) == "retrocede"): {\r
 316                                         self set-global-velocity to -5.\r
 317 \r
 318                                 }\r
 319                         }\r
 320                 }\r
 321 \r
 322                 if ((iterate==100) && (teoria)):\r
 323                 {\r
 324 \r
 325                         self update-entorno.\r
 326                         if (sa validate theory teoria): {\r
 327                                 print "valida".\r
 328                         }\r
 329                         else {\r
 330                                 print "Teoria no valida, salimos".\r
 331                         }\r
 332 \r
 333                 }\r
 334 \r
 335                 iterate++.\r
 336                         if(iterate==101):\r
 337                                 iterate=0.\r
 338 }\r
 339 \r
 340 Stationary : CeldasObstacle (aka CeldasObstacles) {\r
 341         % A CeldasObstacle is used in conjunction with OBJECT(CeldasControl)\r
 342         % and OBJECT(CeldasVehicle).  It is what the OBJECT(CeldasSensor)\r
 343         % objects on the CeldasVehicle detect.\r
 344         % <p>\r
 345         % There are no special behaviors associated with the walls--they're \r
 346         % basically just plain OBJECT(Stationary) objects.\r
 347    \r
 348         +variables:\r
 349             large (float).\r
 350             direction (vector). \r
 351 \r
 352 \r
 353         + 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
 354                 self init-with-shape shape (new Shape init-with-cube size theSize) color theColor at-location theLocation with-rotation theRotation.\r
 355                 large=20.\r
 356 \r
 357         + 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
 358                 self register with-shape theShape at-location theLocation with-rotation theRotation.\r
 359                 self set-color to theColor.\r
 360                 \r
 361         + to get-large:\r
 362             return large.\r
 363 \r
 364         + to set-direction at theDirection (vector):\r
 365             direction=theDirection.\r
 366 \r
 367         + to get-direction:\r
 368             return direction.\r
 369 }\r
 370 \r
 371 Link : CeldasWheel (aka CeldasWheels) {\r
 372         % A CeldasWheel is used in conjunction with OBJECT(CeldasVehicle)\r
 373         % to build Celdas vehicles.  This class is typically not instantiated\r
 374         % manually, since OBJECT(CeldasVehicle) creates one for you when you\r
 375         % add a wheel to the vehicle.\r
 376 \r
 377         + variables:\r
 378                 joint (object).\r
 379                 velocity (float).\r
 380 \r
 381         + to init:\r
 382                 velocity = 0.\r
 383 \r
 384         - to set-joint to j (object):\r
 385                 % Used internally.\r
 386 \r
 387                 joint = j.\r
 388 \r
 389         + section "Configuring the Wheel's Velocity"\r
 390 \r
 391         + to set-velocity to n (float):\r
 392                 % Sets the velocity of this wheel.\r
 393 \r
 394                 if n > CELDAS_MAX_VELOCITY: n = CELDAS_MAX_VELOCITY.\r
 395                 velocity = n.\r
 396 \r
 397                 joint set-joint-velocity to velocity.\r
 398 \r
 399         + to get-velocity:\r
 400                 % Gets the velocity of this wheel.\r
 401                 \r
 402                 return velocity.\r
 403 \r
 404 }\r
 405 \r
 406 Link : CeldasSensor (aka CeldasSensors) {\r
 407         % A CeldasSensor is used in conjunction with OBJECT(CeldasVehicle)\r
 408         % to build Celdas vehicles.  This class is typically not instantiated\r
 409         % manually, since OBJECT(CeldasVehicle) creates one for you when you\r
 410         % add a sensor to the vehicle.\r
 411 \r
 412         + variables:\r
 413                 direction (vector).\r
 414                 positiveDirection(vector).\r
 415                 sensorAngle (float).\r
 416                 value (float).\r
 417                 draw (object).\r
 418                 body(object).\r
 419                 id(int).\r
 420 \r
 421         + to init :\r
 422                 direction = (1,0,1).\r
 423                 positiveDirection= (1,0,1).\r
 424                 sensorAngle = 1.6.\r
 425                 value = 0.0.\r
 426                 draw = new Drawing.\r
 427                                 \r
 428 \r
 429   + section "Configuring the Sensor Values"\r
 430         + to set-id at n (int):\r
 431             id=n.\r
 432 \r
 433         + to set-body at robotBody(object):\r
 434                 body=robotBody.\r
 435                 \r
 436         + to set-sensor-angle to n (float):\r
 437                 % Sets the angle in which this sensor can detect obstacles.  The default\r
 438                 % value of 1.6 means that the sensor can see most of everything in\r
 439                 % front of it.  Setting the value to be any higher leads to general\r
 440                 % wackiness, so I don't suggest it.\r
 441 \r
 442                 sensorAngle = n.\r
 443 \r
 444         + to set-direction to n (vector):\r
 445                 direction = n.\r
 446                 positiveDirection::x=|n::x|.\r
 447                 positiveDirection::y=|n::y|.\r
 448                 positiveDirection::z=|n::z|.\r
 449 \r
 450   + section "Getting the Sensor Values"\r
 451 \r
 452         + to get-sensor-value:\r
 453                 % Gets the sensor value. This should be used from post-iterate,\r
 454                 % if not, the sensor reading correspond to the previous\r
 455                 % iteration.\r
 456         \r
 457         #+ to iterate:\r
 458         \r
 459         + to get-data:\r
 460                 i (object).\r
 461                 min,dist (float).\r
 462                 v,obs(vector).\r
 463                 aux(float).\r
 464                 j (int).\r
 465                 des2,des3(int).\r
 466                 wallBegin,wallEnd,wallCenter (float).\r
 467                 \r
 468                 toObstacle(vector).\r
 469                 largeWall (float).\r
 470                 \r
 471                 obsLoc (vector).                \r
 472                 location (vector).\r
 473                 posObstacle,destiny,yo(vector).\r
 474                                              \r
 475                 draw clear.\r
 476                 value = 0.0.\r
 477                 j=0.\r
 478                 min=0.\r
 479                 foreach i in (all CeldasObstacles): \r
 480                         {\r
 481                          posObstacle=i get-location.\r
 482                          v = (body get-location) - (self get-location ).\r
 483                          obsLoc::y=posObstacle::y.\r
 484                          \r
 485                          if (dot((i get-direction),(1,0,0))):\r
 486                           {\r
 487                            obsLoc::x=((self get-location)::x + ((posObstacle::z - (self get-location)::z)*v::x/v::z)).\r
 488                            obsLoc::z=posObstacle::z.\r
 489                           }                             \r
 490                           else\r
 491                           {\r
 492                            obsLoc::z=((self get-location)::z + ((posObstacle::x - (self get-location)::x)*v::z/v::x)).\r
 493                            obsLoc::x=posObstacle::x.\r
 494                           } \r
 495                                                                 \r
 496                         #!\r
 497                         if(dot((i get-direction),direction)==0):\r
 498                                 des1=1.\r
 499                         else\r
 500                                 des1=0.\r
 501                         !#\r
 502 \r
 503                         des2=0.\r
 504                         if(dot(direction,(1,1,1))<0):\r
 505                         {                        \r
 506                             if((dot((self get-location),positiveDirection))>(dot(obsLoc,positiveDirection))):\r
 507                                         des2=1.      \r
 508                         }\r
 509                         else\r
 510                         {\r
 511                             if((dot((self get-location),positiveDirection))<(dot(obsLoc,positiveDirection))):\r
 512                                         des2=1.         \r
 513                         }                       \r
 514 \r
 515 \r
 516                         #Compruebo que el robot este frente a la pared\r
 517                         wallCenter=dot((i get-location),(i get-direction)).\r
 518                         wallBegin=wallCenter- (i get-large)/2.\r
 519                         wallEnd=wallCenter + (i get-large)/2.           \r
 520 \r
 521                         \r
 522                         yo=self get-location.\r
 523                         destiny=i get-direction.\r
 524 \r
 525                                                                                                                 \r
 526 \r
 527                         if (dot((self get-location),(i get-direction)) > wallBegin) && (dot((self get-location),(i get-direction)) < wallEnd):\r
 528                                 des3=1.\r
 529                         else\r
 530                         {\r
 531                                  des3=0.\r
 532                                  \r
 533                         }                               \r
 534                        \r
 535                         if ((des2) && (des3)):\r
 536                          {                                    \r
 537                                 draw clear.\r
 538 \r
 539                                 dist=|obsLoc - (self get-location)|.\r
 540                                 if( (j==0) || (min>dist) ):\r
 541                                  {\r
 542                                         min=dist.\r
 543                                         obs=obsLoc.\r
 544                                         j++.\r
 545                                         #print "sensor: $id obstaculo: $posObstacle direP: $destiny direS: $direction yo: $yo ".        \r
 546                                  }\r
 547 \r
 548                          }                                              \r
 549 \r
 550                         \r
 551                 } #end for\r
 552 \r
 553                 if(j!=0):\r
 554                         {\r
 555                           #Dibujo el laser\r
 556                           draw set-color to (1, 0, 0).\r
 557                           draw draw-line from (self get-location) to (obs).\r
 558                           return min.\r
 559                         }\r
 560                 \r
 561 \r
 562                 value = -1.\r
 563                 return value.\r
 564 \r
 565 \r
 566 }\r
 567                 \r