trunk/src/breve/robot/Celdas-2-3.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             #bSensor set-sensor-angle to (-1.6).\r
 203             lSensor = (self add-sensor at (1.0, .4, 1.4)).\r
 204             lSensor set-direction to (0,0,1).\r
 205             bSensor set-id at 3.\r
 206             bSensor set-body at self.\r
 207 \r
 208             #rSensor = (self add-sensor at (1.0, .4, -1.4)).\r
 209             #rSensor set-direction to (0,0,-1).\r
 210             lWheel  = (self add-wheel at (0, 0, -1.5)).\r
 211             rWheel  = (self add-wheel at (0, 0, 1.5)).\r
 212                         \r
 213         +to iterate:            \r
 214         #+ to post-iterate:\r
 215                 valuef,valueb,valuer,valuel (float).\r
 216                 fl, fr(float).\r
 217                 \r
 218                 valuef=fSensor get-data.\r
 219                 valueb=bSensor get-data.\r
 220                 #valuel=lSensor get-data.\r
 221                 #valuer=rSensor get-data.\r
 222                 \r
 223                 #value = sensor get-data.\r
 224                 #value = self get-sensor-value.\r
 225                 #valueb = sensor2 get-sensor-value.\r
 226 \r
 227                 if valuef >7: \r
 228                     self set-global-velocity to (15).\r
 229                 else if (valuef <=7) && (valuef > 0):\r
 230                 {   \r
 231                     self set-global-velocity to (0).\r
 232                     #self turn-left with-velocity(2).\r
 233                     #self turn-right with-velocity(2).\r
 234                     #self set-global-velocity to (0).\r
 235                 }\r
 236                 #print "sensor valuef: $valuef  valueb: $valueb".\r
 237                 \r
 238                 #else if value < 0.1: self turn-left with-velocity CELDAS_MAX_TURN_VELOCITY.\r
 239                 #else if value > 10: self set-global-velocity to ((self get-global-velocity) - 1).\r
 240 \r
 241                 #fl = (flWheel get-velocity).\r
 242                 #fr = (frWheel get-velocity).\r
 243                 #print " sensorf: $value  sensorb $valueb, fr: $fr, fl: $fl".            \r
 244             \r
 245 }\r
 246 \r
 247 Stationary : CeldasObstacle (aka CeldasObstacles) {\r
 248         % A CeldasObstacle is used in conjunction with OBJECT(CeldasControl)\r
 249         % and OBJECT(CeldasVehicle).  It is what the OBJECT(CeldasSensor)\r
 250         % objects on the CeldasVehicle detect.\r
 251         % <p>\r
 252         % There are no special behaviors associated with the walls--they're \r
 253         % basically just plain OBJECT(Stationary) objects.\r
 254    \r
 255         +variables:\r
 256             large (float).\r
 257 \r
 258 \r
 259         + 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
 260                 self init-with-shape shape (new Shape init-with-cube size theSize) color theColor at-location theLocation with-rotation theRotation.\r
 261                 large=10.\r
 262 \r
 263         + 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
 264                 self register with-shape theShape at-location theLocation with-rotation theRotation.\r
 265                 self set-color to theColor.\r
 266                 \r
 267         + to get-large:\r
 268             return large.\r
 269 }\r
 270 \r
 271 Link : CeldasWheel (aka CeldasWheels) {\r
 272         % A CeldasWheel is used in conjunction with OBJECT(CeldasVehicle)\r
 273         % to build Celdas vehicles.  This class is typically not instantiated\r
 274         % manually, since OBJECT(CeldasVehicle) creates one for you when you\r
 275         % add a wheel to the vehicle.\r
 276 \r
 277         + variables:\r
 278                 joint (object).\r
 279                 velocity (float).\r
 280 \r
 281         + to init:\r
 282                 velocity = 0.\r
 283 \r
 284         - to set-joint to j (object):\r
 285                 % Used internally.\r
 286 \r
 287                 joint = j.\r
 288 \r
 289         + section "Configuring the Wheel's Velocity"\r
 290 \r
 291         + to set-velocity to n (float):\r
 292                 % Sets the velocity of this wheel.\r
 293 \r
 294                 if n > CELDAS_MAX_VELOCITY: n = CELDAS_MAX_VELOCITY.\r
 295                 velocity = n.\r
 296 \r
 297                 joint set-joint-velocity to velocity.\r
 298 \r
 299         + to get-velocity:\r
 300                 % Gets the velocity of this wheel.\r
 301                 \r
 302                 return velocity.\r
 303 \r
 304 }\r
 305 \r
 306 Link : CeldasSensor (aka CeldasSensors) {\r
 307         % A CeldasSensor is used in conjunction with OBJECT(CeldasVehicle)\r
 308         % to build Celdas vehicles.  This class is typically not instantiated\r
 309         % manually, since OBJECT(CeldasVehicle) creates one for you when you\r
 310         % add a sensor to the vehicle.\r
 311 \r
 312         + variables:\r
 313                 direction (vector).\r
 314                 positiveDirection(vector).\r
 315                 sensorAngle (float).\r
 316                 value (float).\r
 317                 draw (object).\r
 318                 body(object).\r
 319                 id(int).\r
 320 \r
 321         + to init :\r
 322                 direction = (1,0,1).\r
 323                 positiveDirection= (1,0,1).\r
 324                 sensorAngle = 1.6.\r
 325                 value = 0.0.\r
 326                 draw = new Drawing.\r
 327                                 \r
 328 \r
 329   + section "Configuring the Sensor Values"\r
 330         + to set-id at n (int):\r
 331             id=n.\r
 332 \r
 333         + to set-body at robotBody(object):\r
 334                 body=robotBody.\r
 335                 \r
 336         + to set-sensor-angle to n (float):\r
 337                 % Sets the angle in which this sensor can detect obstacles.  The default\r
 338                 % value of 1.6 means that the sensor can see most of everything in\r
 339                 % front of it.  Setting the value to be any higher leads to general\r
 340                 % wackiness, so I don't suggest it.\r
 341 \r
 342                 sensorAngle = n.\r
 343 \r
 344         + to set-direction to n (vector):\r
 345                 direction = n.\r
 346                 positiveDirection::x=|n::x|.\r
 347                 positiveDirection::y=|n::y|.\r
 348                 positiveDirection::z=|n::z|.\r
 349 \r
 350   + section "Getting the Sensor Values"\r
 351 \r
 352         + to get-sensor-value:\r
 353                 % Gets the sensor value. This should be used from post-iterate,\r
 354                 % if not, the sensor reading correspond to the previous\r
 355                 % iteration.\r
 356         \r
 357         #+ to iterate:\r
 358         \r
 359         + to get-data:\r
 360                 i (object).\r
 361                 x,y,z (float).\r
 362                 v(vector).\r
 363                 aux(float).\r
 364 \r
 365                 \r
 366                 strength, angle (float).\r
 367                 toObstacle, transDir (vector).\r
 368                 largeWall (float).\r
 369                 source,destiny (vector).\r
 370                 obsLoc (vector).                \r
 371                 location (vector).\r
 372                 posObstacle,posSensor (vector).\r
 373                 angulo(double).\r
 374                 des(int).\r
 375                                \r
 376                 transDir = (self get-rotation) * direction.\r
 377            \r
 378                 value = 0.0.\r
 379                 foreach i in (all CeldasObstacles): {                                   \r
 380                                                 \r
 381                         posObstacle=i get-location.\r
 382                         print "$i".\r
 383                         \r
 384                         \r
 385                         \r
 386                         if(dot(direction,(1,1,1))<0):                        \r
 387                             des = ((dot((self get-location),positiveDirection))>(dot(posObstacle,positiveDirection))).        \r
 388                         else\r
 389                             des = ((dot((self get-location),positiveDirection))<(dot(posObstacle,positiveDirection))).        \r
 390                         \r
 391 \r
 392                         if (des):\r
 393                         {\r
 394                                 #posObstacle::z = (self get-location)::z -posObstacle::z. \r
 395                                                                        \r
 396                                 #print " posObstacle: $posObstacle".                        \r
 397                                                                                     \r
 398                                 angle = angle(toObstacle, transDir).                                               \r
 399                                 posSensor=self get-location.\r
 400                                 source = self get-location.\r
 401                                 #obsLoc = posObstacle.                        \r
 402                                                 \r
 403                                 #v =  (self get-rotation) * (direction).\r
 404                                 v = (body get-location) - (self get-location ).\r
 405                                 #v = (direction).\r
 406                                 #aux=(self get-rotation).\r
 407                                 #print "rot: $aux".\r
 408                                 aux=v::z/v::x.\r
 409                                 print "m: $aux".\r
 410         \r
 411                                 obsLoc::z=z=((self get-location)::z + ((posObstacle::x - (self get-location)::x)*v::z/v::x)).\r
 412                                 obsLoc::y=y=posObstacle::y.\r
 413                                 obsLoc::x=x=posObstacle::x.\r
 414                         \r
 415                                 destiny = obsLoc.                        \r
 416                                 print "v:$direction x,y,z: $x,$y,$z".                                                            \r
 417                                 #print "v: $v".                                                                                    \r
 418                                 draw clear. \r
 419                         \r
 420                                 draw set-color to (1, 0, 0).\r
 421                                 draw draw-line from source to obsLoc.\r
 422                         }\r
 423                         \r
 424                 }\r
 425                 value = -1.\r
 426                 return value.\r
 427 \r
 428 \r
 429 }\r