1 @use PhysicalControl.
\r
7 @use SistemaAutonomo.
\r
9 @define CELDAS_MAX_VELOCITY 30.
\r
10 @define CELDAS_TURNO 100.
\r
11 @define CELDAS_SENSOR_THRESHOLD 10.
\r
13 PhysicalControl : CeldasControl {
\r
14 % This class is used for building simple vehicle
\r
15 % simulations. To create a vehicle simulation,
\r
16 % subclass CeldasControl and use the init method to
\r
17 % create OBJECT(CeldasObstacle) and
\r
18 % OBJECT(CeldasVehicle) objects.
\r
22 floorShape (object).
\r
23 cloudTexture (object).
\r
27 self enable-lighting.
\r
28 #self enable-smooth-drawing.
\r
30 floorShape = new Shape.
\r
31 floorShape init-with-cube size (200, .2, 200).
\r
33 floor = new Stationary.
\r
34 floor register with-shape floorShape at-location (0, 0, 0).
\r
35 #floor catch-shadows.
\r
37 self point-camera at (0, 0, 0) from (3, 3, 24).
\r
39 #self enable-shadows.
\r
40 #self enable-reflections.
\r
42 cloudTexture = (new Image load from "images/clouds.png").
\r
43 self set-background-color to (.4, .6, .9).
\r
44 self set-background-texture-image to cloudTexture.
\r
48 MultiBody : CeldasLightVehicle (aka CeldasLightVehicles) {
\r
49 % This object is used in conjunction with OBJECT(CeldasControl) to
\r
50 % create simple vehicles.
\r
54 wheelShape (object).
\r
55 sensorShape (object).
\r
61 bodyShape = new Shape.
\r
62 bodyShape init-with-cube size (4.0, .75, 3.0).
\r
64 wheelShape = new Shape.
\r
65 wheelShape init-with-polygon-disk radius ( self get-wheel-radius ) sides 20 height ( self get-wheel-width ).
\r
68 sensorShape = new Shape.
\r
69 sensorShape init-with-polygon-cone radius .2 sides 5 height .5.
\r
72 bodyShape set-density to ( self get-density ).
\r
73 bodyLink = new Link.
\r
74 bodyLink set-shape to bodyShape.
\r
75 bodyLink set-mu to -1.0.
\r
76 bodyLink set-eT to .8.
\r
78 self set-root to bodyLink.
\r
80 self move to (0, 0.9, 0).
\r
81 self set-texture-scale to 1.5.
\r
86 - to get-wheel-width:
\r
89 - to get-wheel-radius:
\r
92 + section "Adding Wheels and Sensors to a Vehicle"
\r
94 + to add-wheel at location (vector):
\r
95 % Adds a wheel at location on the vehicle. This method returns
\r
96 % the wheel which is created, a OBJECT(CeldasWheel).
\r
98 wheel, joint (object).
\r
100 wheel = new CeldasWheel.
\r
101 wheel set-shape to wheelShape.
\r
103 joint = new RevoluteJoint.
\r
105 joint set-relative-rotation around-axis (1, 0, 0) by 1.5708.
\r
106 joint link parent bodyLink to-child wheel with-normal (0, 0, 1)
\r
107 with-parent-point location with-child-point (0, 0, 0).
\r
109 wheel set-eT to .8.
\r
110 wheel set-texture to 0.
\r
111 wheel set-joint to joint.
\r
112 joint set-strength-limit to (joint get-strength-hard-limit) / 2.
\r
113 wheel set-color to (.6, .6, .6).
\r
114 wheel set-mu to 100000.
\r
116 self add-dependency on joint.
\r
117 self add-dependency on wheel.
\r
119 push wheel onto wheels.
\r
123 + to add-sensor at location (vector) with-direction direction = (0,1,0)(vector) :
\r
124 % Adds a sensor at location on the vehicle. This method returns
\r
125 % the sensor which is created, a OBJECT(CeldasSensor).
\r
127 sensor, joint (object).
\r
129 sensor = new CeldasSensor.
\r
130 sensor set-direction to direction.
\r
132 sensor set-shape to sensorShape.
\r
134 joint = new RevoluteJoint.
\r
136 joint set-relative-rotation around-axis (0, 0, 1) by -1.57.
\r
137 joint link parent bodyLink to-child sensor with-normal (1, 0, 0)
\r
138 with-parent-point location with-child-point (0, 0, 0).
\r
140 joint set-double-spring with-strength 300 with-max 0.01 with-min -0.01.
\r
142 self add-dependency on joint.
\r
143 self add-dependency on sensor.
\r
145 sensor set-color to (0, 0, 0).
\r
147 #push sensor onto sensors.
\r
159 CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
\r
160 % A heavy duty version of OBJECT(CeldasLightVehicle), this
\r
161 % vehicle is heavier and harder to control, but more stable
\r
162 % at higher speeds.
\r
164 lSensor, rSensor, fSensor, bSensor (object).
\r
165 lfWheel,rfWheel,lbWheel,rbWheel (object).
\r
166 tleft,tright (int).
\r
167 avanzando,retrocediendo,girando_izq,girando_der(int).
\r
173 datos-finales (hash).
\r
174 plan-finished (int).
\r
175 posicion-inicial (vector).
\r
176 posicion-final (vector).
\r
181 - to get-wheel-width:
\r
184 - to get-wheel-radius:
\r
187 - to near position thePosition (vector) with-error error (float):
\r
189 vectorAux = (self get-location) - thePosition.
\r
191 #print "-----> (pos, other_pos, diff, error): ", (self get-location), thePosition, vectorAux, error.
\r
193 if ((|vectorAux::x| < error) && (|vectorAux::z| < error)):
\r
198 + to set-global-velocity to velocity (float):
\r
199 rfWheel set-velocity to velocity.
\r
200 lfWheel set-velocity to velocity.
\r
201 rbWheel set-velocity to velocity.
\r
202 lbWheel set-velocity to velocity.
\r
204 + to get-global-velocity:
\r
205 return ((rfWheel get-velocity) + (lfWheel get-velocity)) / 2.
\r
210 self rotate around-axis (0,1,0) by (-1.5709/CELDAS_TURNO)*tright.
\r
212 if (tright == CELDAS_TURNO): tright=0.
\r
218 self rotate around-axis (0,1,0) by (1.5709/CELDAS_TURNO)*tleft.
\r
220 if (tleft == CELDAS_TURNO): tleft=0.
\r
223 + to get-sensor-value:
\r
224 return (fSensor get-sensor-value).
\r
228 +to update-entorno:
\r
229 entorno{"sensor_f"} = (fSensor get-sensor-value).
\r
230 entorno{"sensor_b"} = (bSensor get-sensor-value).
\r
231 entorno{"sensor_r"} = (rSensor get-sensor-value).
\r
232 entorno{"sensor_l"} = (lSensor get-sensor-value).
\r
233 sa update-entorno with entorno.
\r
236 # Configuracion de robot
\r
237 fSensor = (self add-sensor at (2.0, .4, 0)).
\r
238 fSensor set-direction to (1,0,0).
\r
239 #fSensor set-direction to (0,0,1).
\r
240 fSensor set-id at 1.
\r
241 fSensor set-body at self.
\r
242 bSensor = (self add-sensor at (-2.0, .4, 0)).
\r
243 bSensor set-direction to (-1,0,0).
\r
244 #bSensor set-direction to (0,0,1).
\r
245 bSensor set-id at 2.
\r
246 bSensor set-body at self.
\r
247 lSensor = (self add-sensor at (0, .4, 1.5)).
\r
248 lSensor set-direction to (0,0,1).
\r
249 #lSensor set-direction to (1,0,0).
\r
250 lSensor set-id at 3.
\r
251 lSensor set-body at self.
\r
253 rSensor = (self add-sensor at (0, .4, -1.5)).
\r
254 rSensor set-direction to (0,0,-1).
\r
255 #rSensor set-direction to (-1,0,0).
\r
256 rSensor set-id at 4.
\r
257 rSensor set-body at self.
\r
259 lfWheel = (self add-wheel at (2, 0, -1.5)).
\r
260 lbWheel = (self add-wheel at (-2, 0, -1.5)).
\r
261 rfWheel = (self add-wheel at (2, 0, 1.5)).
\r
262 rbWheel = (self add-wheel at (-2, 0, 1.5)).
\r
270 posicion-inicial = (self get-location).
\r
271 posicion-final = (0, 0, 0).
\r
273 # Configuracion de sistema autonomo
\r
274 sa = new SistemaAutonomo.
\r
275 sa init with-max-pasos 4 with-max-teorias 15.
\r
277 plan-finished = 1. # así planificamos apenas empezamos
\r
279 teorias = 4 new Teorias.
\r
280 teorias{0} init named "Avanzar" with-action "adelante".
\r
281 teorias{0} set-dato-inicial name "sensor_f" value 0.
\r
282 teorias{0} set-dato-inicial name "sensor_b" value ANY.
\r
283 teorias{0} set-dato-inicial name "sensor_r" value ANY.
\r
284 teorias{0} set-dato-inicial name "sensor_l" value ANY.
\r
285 teorias{0} set-dato-inicial name "movido" value ANY.
\r
286 teorias{0} set-dato-final name "sensor_f" value ANY.
\r
287 teorias{0} set-dato-final name "sensor_b" value ANY.
\r
288 teorias{0} set-dato-final name "sensor_r" value ANY.
\r
289 teorias{0} set-dato-final name "sensor_l" value ANY.
\r
290 teorias{0} set-dato-final name "movido" value 1.
\r
292 teorias{1} init named "Retroceder" with-action "atras".
\r
293 teorias{1} set-dato-inicial name "sensor_f" value 1.
\r
294 teorias{1} set-dato-inicial name "sensor_b" value ANY.
\r
295 teorias{1} set-dato-inicial name "sensor_r" value ANY.
\r
296 teorias{1} set-dato-inicial name "sensor_l" value ANY.
\r
297 teorias{1} set-dato-inicial name "movido" value ANY.
\r
298 teorias{1} set-dato-final name "sensor_f" value 0.
\r
299 teorias{1} set-dato-final name "sensor_b" value ANY.
\r
300 teorias{1} set-dato-final name "sensor_r" value ANY.
\r
301 teorias{1} set-dato-final name "sensor_l" value ANY.
\r
302 teorias{1} set-dato-final name "movido" value 1.
\r
304 teorias{2} init named "Rotar a derecha" with-action "derecha".
\r
305 teorias{2} set-dato-inicial name "sensor_f" value 1.
\r
306 teorias{2} set-dato-inicial name "sensor_b" value ANY.
\r
307 teorias{2} set-dato-inicial name "sensor_r" value ANY.
\r
308 teorias{2} set-dato-inicial name "sensor_l" value ANY.
\r
309 teorias{2} set-dato-inicial name "movido" value ANY.
\r
310 teorias{2} set-dato-final name "sensor_f" value 0.
\r
311 teorias{2} set-dato-final name "sensor_b" value ANY.
\r
312 teorias{2} set-dato-final name "sensor_r" value ANY.
\r
313 teorias{2} set-dato-final name "sensor_l" value 1.
\r
314 teorias{2} set-dato-final name "movido" value 0.
\r
316 teorias{3} init named "Rotar a izquierda" with-action "izquierda".
\r
317 teorias{3} set-dato-inicial name "sensor_f" value 1.
\r
318 teorias{3} set-dato-inicial name "sensor_b" value ANY.
\r
319 teorias{3} set-dato-inicial name "sensor_r" value ANY.
\r
320 teorias{3} set-dato-inicial name "sensor_l" value ANY.
\r
321 teorias{3} set-dato-inicial name "movido" value ANY.
\r
322 teorias{3} set-dato-final name "sensor_f" value 0.
\r
323 teorias{3} set-dato-final name "sensor_b" value ANY.
\r
324 teorias{3} set-dato-final name "sensor_r" value 1.
\r
325 teorias{3} set-dato-final name "sensor_l" value ANY.
\r
326 teorias{3} set-dato-final name "movido" value 0.
\r
328 sa add teoria teorias{0}.
\r
329 sa add teoria teorias{1}.
\r
330 sa add teoria teorias{2}.
\r
331 sa add teoria teorias{3}.
\r
333 datos-finales{"movido"} = 1.
\r
334 sa update-datos-finales with datos-finales.
\r
338 # Actualiza entorno
\r
339 self update-entorno.
\r
341 # Chequeo de objetivo
\r
342 if (self near position posicion-final with-error 5.0):
\r
344 print "Llegamos al FINAL!!!".
\r
345 self set-global-velocity to 0.
\r
350 if (plan-finished):
\r
352 # Actualiza entorno indicando que no se movió para que
\r
353 # el planificador actue
\r
354 sa set-entorno value 0 with-name "movido".
\r
355 sa plan. # Si no tenemos plan, lo hacemos
\r
358 if (! (sa has-next-theory)):
\r
361 print "El planificador no encuentra PLAN!!!".
\r
366 # Ejecución de teoría
\r
369 posicion-inicial = (self get-location).
\r
370 if (sa has-next-theory):
\r
372 teoria = sa get-next-theory.
\r
373 if ((teoria get-accion) == "adelante"):
\r
380 if ((teoria get-accion) == "atras"):
\r
387 if ((teoria get-accion) == "izquierda"):
\r
394 if ((teoria get-accion) == "derecha"):
\r
404 # Validación de teoría
\r
405 if (iterate == CELDAS_TURNO):
\r
407 # Actualiza entorno segun si se movio o no
\r
408 if (self near position posicion-inicial with-error 1.0):
\r
410 sa set-entorno value 0 with-name "movido".
\r
414 sa set-entorno value 1 with-name "movido".
\r
417 if (!(sa validate theory teoria)):
\r
428 # Movimiento del robot
\r
430 self set-global-velocity to (15).
\r
431 if (retrocediendo):
\r
432 self set-global-velocity to (-15).
\r
440 Stationary : CeldasObstacle (aka CeldasObstacles) {
\r
441 % A CeldasObstacle is used in conjunction with OBJECT(CeldasControl)
\r
442 % and OBJECT(CeldasVehicle). It is what the OBJECT(CeldasSensor)
\r
443 % objects on the CeldasVehicle detect.
\r
445 % There are no special behaviors associated with the walls--they're
\r
446 % basically just plain OBJECT(Stationary) objects.
\r
450 direction (vector).
\r
453 + 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
454 self init-with-shape shape (new Shape init-with-cube size theSize) color theColor at-location theLocation with-rotation theRotation.
\r
457 + 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
458 self register with-shape theShape at-location theLocation with-rotation theRotation.
\r
459 self set-color to theColor.
\r
464 + to set-direction at theDirection (vector):
\r
465 direction=theDirection.
\r
467 + to get-direction:
\r
471 Link : CeldasWheel (aka CeldasWheels) {
\r
472 % A CeldasWheel is used in conjunction with OBJECT(CeldasVehicle)
\r
473 % to build Celdas vehicles. This class is typically not instantiated
\r
474 % manually, since OBJECT(CeldasVehicle) creates one for you when you
\r
475 % add a wheel to the vehicle.
\r
484 - to set-joint to j (object):
\r
489 + section "Configuring the Wheel's Velocity"
\r
491 + to set-velocity to n (float):
\r
492 % Sets the velocity of this wheel.
\r
494 if n > CELDAS_MAX_VELOCITY: n = CELDAS_MAX_VELOCITY.
\r
497 joint set-joint-velocity to velocity.
\r
500 % Gets the velocity of this wheel.
\r
506 Link : CeldasSensor (aka CeldasSensors) {
\r
507 % A CeldasSensor is used in conjunction with OBJECT(CeldasVehicle)
\r
508 % to build Celdas vehicles. This class is typically not instantiated
\r
509 % manually, since OBJECT(CeldasVehicle) creates one for you when you
\r
510 % add a sensor to the vehicle.
\r
513 direction (vector).
\r
514 positiveDirection(vector).
\r
515 sensorAngle (float).
\r
522 direction = (1,0,1).
\r
523 positiveDirection= (1,0,1).
\r
526 draw = new Drawing.
\r
529 + section "Configuring the Sensor Values"
\r
530 + to set-id at n (int):
\r
533 + to set-body at robotBody(object):
\r
536 + to set-sensor-angle to n (float):
\r
537 % Sets the angle in which this sensor can detect obstacles. The default
\r
538 % value of 1.6 means that the sensor can see most of everything in
\r
539 % front of it. Setting the value to be any higher leads to general
\r
540 % wackiness, so I don't suggest it.
\r
544 + to set-direction to n (vector):
\r
546 positiveDirection::x=|n::x|.
\r
547 positiveDirection::y=|n::y|.
\r
548 positiveDirection::z=|n::z|.
\r
550 + section "Getting the Sensor Values"
\r
552 + to get-sensor-value:
\r
553 % Gets the sensor value. This should be used from post-iterate,
\r
554 % if not, the sensor reading correspond to the previous
\r
558 val = self get-data.
\r
559 if (val > CELDAS_SENSOR_THRESHOLD): return 0.
\r
570 wallBegin,wallEnd,wallCenter (float).
\r
572 posObstacle,destiny,yo(vector).
\r
578 foreach i in (all CeldasObstacles):
\r
580 posObstacle=i get-location.
\r
581 v = (body get-location) - (self get-location ).
\r
582 obsLoc::y=posObstacle::y.
\r
584 if (dot((i get-direction),(1,0,0))):
\r
586 obsLoc::x=((self get-location)::x + ((posObstacle::z - (self get-location)::z)*v::x/v::z)).
\r
587 obsLoc::z=posObstacle::z.
\r
591 obsLoc::z=((self get-location)::z + ((posObstacle::x - (self get-location)::x)*v::z/v::x)).
\r
592 obsLoc::x=posObstacle::x.
\r
596 if(dot((i get-direction),direction)==0):
\r
603 if(dot(direction,(1,1,1))<0):
\r
605 if((dot((self get-location),positiveDirection))>(dot(obsLoc,positiveDirection))):
\r
610 if((dot((self get-location),positiveDirection))<(dot(obsLoc,positiveDirection))):
\r
615 #Compruebo que el robot este frente a la pared
\r
616 wallCenter=dot((i get-location),(i get-direction)).
\r
617 wallBegin=wallCenter- (i get-large)/2.
\r
618 wallEnd=wallCenter + (i get-large)/2.
\r
621 yo=self get-location.
\r
622 destiny=i get-direction.
\r
626 if (dot((self get-location),(i get-direction)) > wallBegin) && (dot((self get-location),(i get-direction)) < wallEnd):
\r
634 if ((des2) && (des3)):
\r
638 dist=|obsLoc - (self get-location)|.
\r
639 if( (j==0) || (min>dist) ):
\r
644 #print "sensor: $id obstaculo: $posObstacle direP: $destiny direS: $direction yo: $yo ".
\r
655 draw set-color to (1, 0, 0).
\r
656 draw draw-line from (self get-location) to (obs).
\r