1 @use PhysicalControl.
\r
8 @define CELDAS_MAX_VELOCITY 30.
\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
19 floorShape (object).
\r
20 cloudTexture (object).
\r
24 self enable-lighting.
\r
25 #self enable-smooth-drawing.
\r
27 floorShape = new Shape.
\r
28 floorShape init-with-cube size (200, .2, 200).
\r
30 floor = new Stationary.
\r
31 floor register with-shape floorShape at-location (0, 0, 0).
\r
32 #floor catch-shadows.
\r
34 self point-camera at (0, 0, 0) from (3, 3, 24).
\r
36 #self enable-shadows.
\r
37 #self enable-reflections.
\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
45 MultiBody : CeldasLightVehicle (aka CeldasLightVehicles) {
\r
46 % This object is used in conjunction with OBJECT(CeldasControl) to
\r
47 % create simple vehicles.
\r
51 wheelShape (object).
\r
52 sensorShape (object).
\r
59 bodyShape = new Shape.
\r
60 bodyShape init-with-cube size (4.0, .75, 3.0).
\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
66 sensorShape = new Shape.
\r
67 sensorShape init-with-polygon-cone radius .2 sides 5 height .5.
\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
76 self set-root to bodyLink.
\r
78 self move to (0, 0.9, 0).
\r
79 self set-texture-scale to 1.5.
\r
84 - to get-wheel-width:
\r
87 - to get-wheel-radius:
\r
90 + section "Adding Wheels and Sensors to a Vehicle"
\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
96 wheel, joint (object).
\r
98 wheel = new CeldasWheel.
\r
99 wheel set-shape to wheelShape.
\r
101 joint = new RevoluteJoint.
\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
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
114 self add-dependency on joint.
\r
115 self add-dependency on wheel.
\r
117 push wheel onto wheels.
\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
125 sensor, joint (object).
\r
127 sensor = new CeldasSensor.
\r
128 sensor set-direction to direction.
\r
130 sensor set-shape to sensorShape.
\r
132 joint = new RevoluteJoint.
\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
138 joint set-double-spring with-strength 300 with-max 0.01 with-min -0.01.
\r
140 self add-dependency on joint.
\r
141 self add-dependency on sensor.
\r
143 sensor set-color to (0, 0, 0).
\r
145 #push sensor onto sensors.
\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
162 lSensor, rSensor, fSensor, bSensor (object).
\r
163 lWheel,rWheel (object).
\r
168 - to get-wheel-width:
\r
171 - to get-wheel-radius:
\r
174 + to set-global-velocity to velocity (float):
\r
175 rWheel set-velocity to velocity.
\r
176 lWheel set-velocity to velocity.
\r
178 + to get-global-velocity:
\r
179 return ((rWheel get-velocity) + (lWheel get-velocity)) / 2.
\r
181 + to turn-right with-velocity velocity (float):
\r
182 lWheel set-velocity to velocity.
\r
183 rWheel set-velocity to -velocity.
\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
190 + to get-sensor-value:
\r
191 return (fSensor get-sensor-value).
\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
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
214 #+ to post-iterate:
\r
215 valuef,valueb,valuer,valuel (float).
\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
223 #value = sensor get-data.
\r
224 #value = self get-sensor-value.
\r
225 #valueb = sensor2 get-sensor-value.
\r
228 self set-global-velocity to (15).
\r
229 else if (valuef <=7) && (valuef > 0):
\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
236 #print "sensor valuef: $valuef valueb: $valueb".
\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
241 #fl = (flWheel get-velocity).
\r
242 #fr = (frWheel get-velocity).
\r
243 #print " sensorf: $value sensorb $valueb, fr: $fr, fl: $fl".
\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
252 % There are no special behaviors associated with the walls--they're
\r
253 % basically just plain OBJECT(Stationary) objects.
\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
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
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
284 - to set-joint to j (object):
\r
289 + section "Configuring the Wheel's Velocity"
\r
291 + to set-velocity to n (float):
\r
292 % Sets the velocity of this wheel.
\r
294 if n > CELDAS_MAX_VELOCITY: n = CELDAS_MAX_VELOCITY.
\r
297 joint set-joint-velocity to velocity.
\r
300 % Gets the velocity of this wheel.
\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
313 direction (vector).
\r
314 positiveDirection(vector).
\r
315 sensorAngle (float).
\r
322 direction = (1,0,1).
\r
323 positiveDirection= (1,0,1).
\r
326 draw = new Drawing.
\r
329 + section "Configuring the Sensor Values"
\r
330 + to set-id at n (int):
\r
333 + to set-body at robotBody(object):
\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
344 + to set-direction to n (vector):
\r
346 positiveDirection::x=|n::x|.
\r
347 positiveDirection::y=|n::y|.
\r
348 positiveDirection::z=|n::z|.
\r
350 + section "Getting the Sensor Values"
\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
366 strength, angle (float).
\r
367 toObstacle, transDir (vector).
\r
369 source,destiny (vector).
\r
372 posObstacle,posSensor (vector).
\r
376 transDir = (self get-rotation) * direction.
\r
379 foreach i in (all CeldasObstacles): {
\r
381 posObstacle=i get-location.
\r
386 if(dot(direction,(1,1,1))<0):
\r
387 des = ((dot((self get-location),positiveDirection))>(dot(posObstacle,positiveDirection))).
\r
389 des = ((dot((self get-location),positiveDirection))<(dot(posObstacle,positiveDirection))).
\r
394 #posObstacle::z = (self get-location)::z -posObstacle::z.
\r
396 #print " posObstacle: $posObstacle".
\r
398 angle = angle(toObstacle, transDir).
\r
399 posSensor=self get-location.
\r
400 source = self get-location.
\r
401 #obsLoc = posObstacle.
\r
403 #v = (self get-rotation) * (direction).
\r
404 v = (body get-location) - (self get-location ).
\r
406 #aux=(self get-rotation).
\r
407 #print "rot: $aux".
\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
416 print "v:$direction x,y,z: $x,$y,$z".
\r
420 draw set-color to (1, 0, 0).
\r
421 draw draw-line from source to obsLoc.
\r