trunk/src/breve/robot/Celdas-2-5.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         \r
 165         - to get-density:\r
 166                 return 20.0.\r
 167 \r
 168         - to get-wheel-width:\r
 169                 return 0.4.\r
 170 \r
 171         - to get-wheel-radius:\r
 172                 return 0.8.\r
 173 \r
 174         + to set-global-velocity to velocity (float):\r
 175                 rWheel set-velocity to velocity.\r
 176                 lWheel set-velocity to velocity.\r
 177 \r
 178         + to get-global-velocity:\r
 179                 return ((rWheel get-velocity) + (lWheel get-velocity)) / 2.\r
 180 \r
 181         + to turn-right with-velocity velocity (float):         \r
 182                 lWheel set-velocity to velocity.\r
 183                 rWheel set-velocity to -velocity.\r
 184 \r
 185         + to turn-left with-velocity velocity (float):\r
 186 #                vehicle rotate around-axis (0,1,0) by 1. \r
 187                 lWheel set-velocity to -velocity.       \r
 188                 rWheel set-velocity to velocity.\r
 189 \r
 190         + to get-sensor-value:\r
 191                 return (fSensor get-sensor-value).\r
 192 \r
 193         +to init:\r
 194             fSensor = (self add-sensor at (2.0, .4, 0)).            \r
 195             fSensor set-direction to (1,0,0).\r
 196             fSensor set-id at 1.\r
 197             fSensor set-body at self.\r
 198             bSensor = (self add-sensor at (-2.0, .4, 0)).\r
 199             bSensor set-direction to (-1,0,0).\r
 200             bSensor set-id at 2.\r
 201             bSensor set-body at self.\r
 202             lSensor = (self add-sensor at (0, .4, 1.5)).\r
 203             lSensor set-direction to (0,0,1).\r
 204             lSensor set-id at 3.\r
 205             lSensor set-body at self.\r
 206 \r
 207 \r
 208             rSensor = (self add-sensor at (0, .4, -1.5)).\r
 209             rSensor set-direction to (0,0,-1).\r
 210             rSensor set-id at 4.\r
 211             rSensor set-body at self.\r
 212 \r
 213             lWheel  = (self add-wheel at (0, 0, -1.5)).\r
 214             rWheel  = (self add-wheel at (0, 0, 1.5)).\r
 215                         \r
 216         +to iterate:            \r
 217         #+ to post-iterate:\r
 218                 valuef,valueb,valuer,valuel (float).\r
 219                 fl, fr(float).\r
 220                 \r
 221                 valuef=fSensor get-data.\r
 222                 valueb=bSensor get-data.\r
 223                 valuel=lSensor get-data.\r
 224                 valuer=rSensor get-data.\r
 225 \r
 226                 self turn-left with-velocity(20).\r
 227                 self set-global-velocity to (15).\r
 228                 if valuef >7:           \r
 229                     self set-global-velocity to (15).\r
 230                 else if (valuef <=7) && (valuef > 0):\r
 231                 {   \r
 232                     self set-global-velocity to (0).\r
 233                     #self turn-left with-velocity(2).\r
 234                     #self turn-right with-velocity(2).\r
 235                     #self set-global-velocity to (0).\r
 236                 }\r
 237                 #print "sensor valuef: $valuef  valueb: $valueb".\r
 238                 \r
 239                 #else if value < 0.1: self turn-left with-velocity CELDAS_MAX_TURN_VELOCITY.\r
 240                 #else if value > 10: self set-global-velocity to ((self get-global-velocity) - 1).\r
 241 \r
 242                 #fl = (flWheel get-velocity).\r
 243                 #fr = (frWheel get-velocity).\r
 244                 #print " sensorf: $value  sensorb $valueb, fr: $fr, fl: $fl".            \r
 245             \r
 246 }\r
 247 \r
 248 Stationary : CeldasObstacle (aka CeldasObstacles) {\r
 249         % A CeldasObstacle is used in conjunction with OBJECT(CeldasControl)\r
 250         % and OBJECT(CeldasVehicle).  It is what the OBJECT(CeldasSensor)\r
 251         % objects on the CeldasVehicle detect.\r
 252         % <p>\r
 253         % There are no special behaviors associated with the walls--they're \r
 254         % basically just plain OBJECT(Stationary) objects.\r
 255    \r
 256         +variables:\r
 257             large (float).\r
 258             direction (vector). \r
 259 \r
 260 \r
 261         + 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
 262                 self init-with-shape shape (new Shape init-with-cube size theSize) color theColor at-location theLocation with-rotation theRotation.\r
 263                 large=20.\r
 264 \r
 265         + 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
 266                 self register with-shape theShape at-location theLocation with-rotation theRotation.\r
 267                 self set-color to theColor.\r
 268                 \r
 269         + to get-large:\r
 270             return large.\r
 271 \r
 272         + to set-direction at theDirection (vector):\r
 273             direction=theDirection.\r
 274 \r
 275         + to get-direction:\r
 276             return direction.\r
 277 }\r
 278 \r
 279 Link : CeldasWheel (aka CeldasWheels) {\r
 280         % A CeldasWheel is used in conjunction with OBJECT(CeldasVehicle)\r
 281         % to build Celdas vehicles.  This class is typically not instantiated\r
 282         % manually, since OBJECT(CeldasVehicle) creates one for you when you\r
 283         % add a wheel to the vehicle.\r
 284 \r
 285         + variables:\r
 286                 joint (object).\r
 287                 velocity (float).\r
 288 \r
 289         + to init:\r
 290                 velocity = 0.\r
 291 \r
 292         - to set-joint to j (object):\r
 293                 % Used internally.\r
 294 \r
 295                 joint = j.\r
 296 \r
 297         + section "Configuring the Wheel's Velocity"\r
 298 \r
 299         + to set-velocity to n (float):\r
 300                 % Sets the velocity of this wheel.\r
 301 \r
 302                 if n > CELDAS_MAX_VELOCITY: n = CELDAS_MAX_VELOCITY.\r
 303                 velocity = n.\r
 304 \r
 305                 joint set-joint-velocity to velocity.\r
 306 \r
 307         + to get-velocity:\r
 308                 % Gets the velocity of this wheel.\r
 309                 \r
 310                 return velocity.\r
 311 \r
 312 }\r
 313 \r
 314 Link : CeldasSensor (aka CeldasSensors) {\r
 315         % A CeldasSensor is used in conjunction with OBJECT(CeldasVehicle)\r
 316         % to build Celdas vehicles.  This class is typically not instantiated\r
 317         % manually, since OBJECT(CeldasVehicle) creates one for you when you\r
 318         % add a sensor to the vehicle.\r
 319 \r
 320         + variables:\r
 321                 direction (vector).\r
 322                 positiveDirection(vector).\r
 323                 sensorAngle (float).\r
 324                 value (float).\r
 325                 draw (object).\r
 326                 body(object).\r
 327                 id(int).\r
 328 \r
 329         + to init :\r
 330                 direction = (1,0,1).\r
 331                 positiveDirection= (1,0,1).\r
 332                 sensorAngle = 1.6.\r
 333                 value = 0.0.\r
 334                 draw = new Drawing.\r
 335                                 \r
 336 \r
 337   + section "Configuring the Sensor Values"\r
 338         + to set-id at n (int):\r
 339             id=n.\r
 340 \r
 341         + to set-body at robotBody(object):\r
 342                 body=robotBody.\r
 343                 \r
 344         + to set-sensor-angle to n (float):\r
 345                 % Sets the angle in which this sensor can detect obstacles.  The default\r
 346                 % value of 1.6 means that the sensor can see most of everything in\r
 347                 % front of it.  Setting the value to be any higher leads to general\r
 348                 % wackiness, so I don't suggest it.\r
 349 \r
 350                 sensorAngle = n.\r
 351 \r
 352         + to set-direction to n (vector):\r
 353                 direction = n.\r
 354                 positiveDirection::x=|n::x|.\r
 355                 positiveDirection::y=|n::y|.\r
 356                 positiveDirection::z=|n::z|.\r
 357 \r
 358   + section "Getting the Sensor Values"\r
 359 \r
 360         + to get-sensor-value:\r
 361                 % Gets the sensor value. This should be used from post-iterate,\r
 362                 % if not, the sensor reading correspond to the previous\r
 363                 % iteration.\r
 364         \r
 365         #+ to iterate:\r
 366         \r
 367         + to get-data:\r
 368                 i (object).\r
 369                 x,y,z (float).\r
 370                 min,dist (float).\r
 371                 v,obs(vector).\r
 372                 aux(float).\r
 373                 j (int).\r
 374                 des1,des2,des3(int).\r
 375                 wallBegin,wallEnd (float).\r
 376                 \r
 377                 aux1,aux2,aux3,aux4 (float).\r
 378                 yo,toObstacle, transDir (vector).\r
 379                 largeWall (float).\r
 380                 source,destiny (vector).\r
 381                 obsLoc (vector).                \r
 382                 location (vector).\r
 383                 posObstacle,posSensor (vector).\r
 384                                              \r
 385                 draw clear.\r
 386                 value = 0.0.\r
 387                 j=0.\r
 388                 min=0.\r
 389                 foreach i in (all CeldasObstacles): \r
 390                         {\r
 391                          posObstacle=i get-location.\r
 392                          \r
 393                                                                 \r
 394                         \r
 395                         if(dot((i get-direction),direction)==0):\r
 396                                 des1=1.\r
 397                         else\r
 398                                 des1=0.\r
 399 \r
 400                         des1=1.\r
 401                         des2=0.\r
 402                         if(dot(direction,(1,1,1))<0):\r
 403                         {                        \r
 404                             if((dot((self get-location),positiveDirection))>(dot(posObstacle,positiveDirection))):\r
 405                                         des2=1.      \r
 406                         }\r
 407                         else\r
 408                         {\r
 409                             if((dot((self get-location),positiveDirection))<(dot(posObstacle,positiveDirection))):\r
 410                                         des2=1.         \r
 411                         }                       \r
 412 \r
 413 \r
 414                         #Compruebo que el robot este frente a la pared\r
 415                         wallBegin=dot((i get-location),(i get-direction) )- (i get-large)/2.\r
 416                         wallEnd=dot((i get-location),(i get-direction) )+ (i get-large)/2.              \r
 417 \r
 418 \r
 419                         #print "begin: $wallBegin end: $wallEnd".\r
 420                         yo=self get-location.\r
 421                         destiny=i get-direction.\r
 422 \r
 423                         v = (body get-location) - (self get-location ).\r
 424                         obsLoc::y=y=posObstacle::y.                                                             \r
 425                                 \r
 426                         if (dot((i get-direction),(1,0,0))):\r
 427                         {\r
 428                          obsLoc::x=x=((self get-location)::x + ((posObstacle::z - (self get-location)::z)*v::x/v::z)).\r
 429                          obsLoc::z=z=posObstacle::z.\r
 430                         }                               \r
 431                         else\r
 432                         {\r
 433                          obsLoc::z=z=((self get-location)::z + ((posObstacle::x - (self get-location)::x)*v::z/v::x)).\r
 434                          obsLoc::x=x=posObstacle::x.\r
 435                         }                                                       \r
 436 \r
 437 \r
 438                         if (dot((obsLoc),(i get-direction)) > wallBegin) && (dot((obsLoc),(i get-direction)) < wallEnd):\r
 439                                 des3=1.\r
 440                         else\r
 441                         {\r
 442                                  des3=0.\r
 443                                  \r
 444                         }                               \r
 445 \r
 446                         aux1=dot((self get-location),(i get-direction)).\r
 447 \r
 448                         #print "sensor: $id obstaculo: $posObstacle direP: $destiny direS: $direction yo: $yo ".\r
 449                         #print "dist: $aux1 begin: $wallBegin end: $wallEnd".\r
 450 \r
 451                        \r
 452                         print "sensor: $id , des1: $des1, des2: $des2, des3: $des3".\r
 453                         if ((des1) && (des2) && (des3)):\r
 454                          {                                    \r
 455                                 draw clear.                             \r
 456                                 #print " posObstacle: $posObstacle".                                                    \r
 457                                                         \r
 458                                 dist=|obsLoc - (self get-location)|.\r
 459                                 if( (j==0) || (min>dist) ):\r
 460                                  {\r
 461                                         min=dist.\r
 462                                         obs=obsLoc.\r
 463                                         j++.    \r
 464                                  }\r
 465 \r
 466                          }                                              \r
 467 \r
 468                         \r
 469                 } #end for\r
 470 \r
 471                 if(j!=0):\r
 472                         {\r
 473                           #Dibujo el laser\r
 474                           draw set-color to (1, 0, 0).\r
 475                           draw draw-line from (self get-location) to (obs).\r
 476                           return dist.\r
 477                         }\r
 478                 \r
 479 \r
 480                 value = -1.\r
 481                 return value.\r
 482 \r
 483 \r
 484 }\r
 485