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