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