-@use PhysicalControl.
-@use Shape.
-@use Stationary.
-@use Link.
-@use MultiBody.
-
-@define CELDAS_MAX_VELOCITY 30.
-
-PhysicalControl : CeldasControl {
- % This class is used for building simple vehicle
- % simulations. To create a vehicle simulation,
- % subclass CeldasControl and use the init method to
- % create OBJECT(CeldasObstacle) and
- % OBJECT(CeldasVehicle) objects.
-
- + variables:
- floor (object).
- floorShape (object).
- cloudTexture (object).
-
- + to init:
- self enable-lighting.
- #self enable-smooth-drawing.
-
- floorShape = new Shape.
- floorShape init-with-cube size (200, .2, 200).
-
- floor = new Stationary.
- floor register with-shape floorShape at-location (0, 0, 0).
- #floor catch-shadows.
-
- self point-camera at (0, 0, 0) from (3, 3, 24).
-
- #self enable-shadows.
- #self enable-reflections.
-
- cloudTexture = (new Image load from "images/clouds.png").
- self set-background-color to (.4, .6, .9).
- self set-background-texture-image to cloudTexture.
-}
-
-MultiBody : CeldasLightVehicle (aka CeldasLightVehicles) {
- % This object is used in conjunction with OBJECT(CeldasControl) to
- % create simple vehicles.
-
- + variables:
- bodyShape (object).
- wheelShape (object).
- sensorShape (object).
- bodyLink (object).
-
- wheels (list).
- sensors (list).
-
- + to init:
- bodyShape = new Shape.
- bodyShape init-with-cube size (4.0, .75, 3.0).
-
- wheelShape = new Shape.
- wheelShape init-with-polygon-disk radius ( self get-wheel-radius ) sides 20 height ( self get-wheel-width ).
- # 40
-
- sensorShape = new Shape.
- sensorShape init-with-polygon-cone radius .2 sides 5 height .5.
- # 10
-
- bodyShape set-density to ( self get-density ).
- bodyLink = new Link.
- bodyLink set-shape to bodyShape.
- bodyLink set-mu to -1.0.
- bodyLink set-eT to .8.
-
- self set-root to bodyLink.
-
- self move to (0, 0.9, 0).
- self set-texture-scale to 1.5.
-
- - to get-density:
- return 1.0.
-
- - to get-wheel-width:
- return 0.1.
-
- - to get-wheel-radius:
- return 0.6.
-
- + section "Adding Wheels and Sensors to a Vehicle"
-
- + to add-wheel at location (vector):
- % Adds a wheel at location on the vehicle. This method returns
- % the wheel which is created, a OBJECT(CeldasWheel).
-
- wheel, joint (object).
-
- wheel = new CeldasWheel.
- wheel set-shape to wheelShape.
-
- joint = new RevoluteJoint.
-
- joint set-relative-rotation around-axis (1, 0, 0) by 1.5708.
- joint link parent bodyLink to-child wheel with-normal (0, 0, 1)
- with-parent-point location with-child-point (0, 0, 0).
-
- wheel set-eT to .8.
- wheel set-texture to 0.
- wheel set-joint to joint.
- joint set-strength-limit to (joint get-strength-hard-limit) / 2.
- wheel set-color to (.6, .6, .6).
- wheel set-mu to 100000.
-
- self add-dependency on joint.
- self add-dependency on wheel.
-
- push wheel onto wheels.
-
- return wheel.
-
- + to add-sensor at location (vector):
- % Adds a sensor at location on the vehicle. This method returns
- % the sensor which is created, a OBJECT(CeldasSensor).
-
- sensor, joint (object).
-
- sensor = new CeldasSensor.
- sensor set-shape to sensorShape.
-
- joint = new RevoluteJoint.
-
- joint set-relative-rotation around-axis (0, 0, 1) by -1.57.
- joint link parent bodyLink to-child sensor with-normal (1, 0, 0)
- with-parent-point location with-child-point (0, 0, 0).
-
- joint set-double-spring with-strength 300 with-max 0.01 with-min -0.01.
-
- self add-dependency on joint.
- self add-dependency on sensor.
-
- sensor set-color to (0, 0, 0).
-
- push sensor onto sensors.
-
- return sensor.
-
- + to destroy:
- free sensorShape.
- free wheelShape.
- free bodyShape.
-
- super destroy.
-}
-
-CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
- % A heavy duty version of OBJECT(CeldasLightVehicle), this
- % vehicle is heavier and harder to control, but more stable
- % at higher speeds.
-
- - to get-density:
- return 20.0.
-
- - to get-wheel-width:
- return 0.4.
-
- - to get-wheel-radius:
- return 0.8.
-}
-
-Stationary : CeldasObstacle (aka CeldasObstacles) {
- % A CeldasObstacle is used in conjunction with OBJECT(CeldasControl)
- % and OBJECT(CeldasVehicle). It is what the OBJECT(CeldasSensor)
- % objects on the CeldasVehicle detect.
- % <p>
- % There are no special behaviors associated with the walls--they're
- % basically just plain OBJECT(Stationary) objects.
-
- + 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):
- self init-with-shape shape (new Shape init-with-cube size theSize) color theColor at-location theLocation with-rotation theRotation.
-
- + 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):
- self register with-shape theShape at-location theLocation with-rotation theRotation.
- self set-color to theColor.
-}
-
-Link : CeldasWheel (aka CeldasWheels) {
- % A CeldasWheel is used in conjunction with OBJECT(CeldasVehicle)
- % to build Celdas vehicles. This class is typically not instantiated
- % manually, since OBJECT(CeldasVehicle) creates one for you when you
- % add a wheel to the vehicle.
-
- + variables:
- joint (object).
- velocity (float).
-
- + to init:
- velocity = 0.
-
- - to set-joint to j (object):
- % Used internally.
-
- joint = j.
-
- + section "Configuring the Wheel's Velocity"
-
- + to set-velocity to n (float):
- % Sets the velocity of this wheel.
-
- if n > CELDAS_MAX_VELOCITY: n = CELDAS_MAX_VELOCITY.
- velocity = n.
-
- joint set-joint-velocity to velocity.
-
- + to get-velocity:
- % Gets the velocity of this wheel.
-
- return velocity.
-
-}
-
-Link : CeldasSensor (aka CeldasSensors) {
- % A CeldasSensor is used in conjunction with OBJECT(CeldasVehicle)
- % to build Celdas vehicles. This class is typically not instantiated
- % manually, since OBJECT(CeldasVehicle) creates one for you when you
- % add a sensor to the vehicle.
-
- + variables:
- direction (vector).
- sensorAngle (float).
- value (float).
-
- + to init:
- direction = (0, 1, 0).
- sensorAngle = 1.6.
- value = 0.0.
-
- + section "Configuring the Sensor Values"
-
- + to set-sensor-angle to n (float):
- % Sets the angle in which this sensor can detect obstacles. The default
- % value of 1.6 means that the sensor can see most of everything in
- % front of it. Setting the value to be any higher leads to general
- % wackiness, so I don't suggest it.
-
- sensorAngle = n.
-
- + section "Getting the Sensor Values"
-
- + to get-sensor-value:
- % Gets the sensor value. This should be used from post-iterate,
- % if not, the sensor reading correspond to the previous
- % iteration.
-
- + to iterate:
- i (object).
- strength, angle (float).
- toObstacle, transDir (vector).
-
- transDir = (self get-rotation) * direction.
-
- value = 0.0.
-
- foreach i in (all CeldasObstacles): {
- toObstacle = (i get-location) - (self get-location).
- angle = angle(toObstacle, transDir).
-
- if angle < sensorAngle: {
- strength = | (self get-location) - (i get-location) |.
- strength = 1.0 / (strength * strength) .
-
- if strength > 10: strength = 10.
-
- if strength > value: value = strength.
- }
- }
-
-}
+@use PhysicalControl.\r
+@use Shape.\r
+@use Stationary.\r
+@use Link.\r
+@use MultiBody.\r
+@use Drawing.\r
+@use SistemaAutonomo.\r
+\r
+@define CELDAS_MAX_VELOCITY 5.\r
+@define CELDAS_TURNO 30.\r
+@define CELDAS_SENSOR_THRESHOLD 10.\r
+\r
+PhysicalControl : CeldasControl {\r
+ % This class is used for building simple vehicle \r
+ % simulations. To create a vehicle simulation, \r
+ % subclass CeldasControl and use the init method to \r
+ % create OBJECT(CeldasObstacle) and \r
+ % OBJECT(CeldasVehicle) objects.\r
+\r
+ + variables:\r
+ floor (object).\r
+ floorShape (object).\r
+ cloudTexture (object).\r
+\r
+\r
+ + to init:\r
+ self enable-lighting.\r
+ #self enable-smooth-drawing.\r
+\r
+ floorShape = new Shape.\r
+ floorShape init-with-cube size (200, .2, 200).\r
+\r
+ floor = new Stationary.\r
+ floor register with-shape floorShape at-location (0, 0, 0).\r
+ #floor catch-shadows.\r
+\r
+ self point-camera at (0, 0, 0) from (3, 3, 24).\r
+\r
+ #self enable-shadows.\r
+ #self enable-reflections.\r
+\r
+ cloudTexture = (new Image load from "images/clouds.png"). \r
+ self set-background-color to (.4, .6, .9).\r
+ self set-background-texture-image to cloudTexture.\r
+\r
+}\r
+\r
+MultiBody : CeldasLightVehicle (aka CeldasLightVehicles) {\r
+ % This object is used in conjunction with OBJECT(CeldasControl) to\r
+ % create simple vehicles.\r
+\r
+ + variables:\r
+ bodyShape (object).\r
+ wheelShape (object).\r
+ sensorShape (object).\r
+ bodyLink (object).\r
+\r
+ wheels (list).\r
+\r
+ + to init:\r
+ bodyShape = new Shape.\r
+ bodyShape init-with-cube size (4.0, .75, 3.0). \r
+\r
+ wheelShape = new Shape.\r
+ wheelShape init-with-polygon-disk radius ( self get-wheel-radius ) sides 20 height ( self get-wheel-width ).\r
+ # 40\r
+\r
+ sensorShape = new Shape.\r
+ sensorShape init-with-polygon-cone radius .2 sides 5 height .5.\r
+ # 10\r
+\r
+ bodyShape set-density to ( self get-density ).\r
+ bodyLink = new Link.\r
+ bodyLink set-shape to bodyShape. \r
+ bodyLink set-mu to -1.0.\r
+ bodyLink set-eT to .8.\r
+\r
+ self set-root to bodyLink.\r
+\r
+ self move to (0, 0.9, 0).\r
+ self set-texture-scale to 1.5.\r
+\r
+ - to get-density:\r
+ return 1.0.\r
+\r
+ - to get-wheel-width:\r
+ return 0.1.\r
+\r
+ - to get-wheel-radius:\r
+ return 0.6.\r
+\r
+ + section "Adding Wheels and Sensors to a Vehicle"\r
+\r
+ + to add-wheel at location (vector):\r
+ % Adds a wheel at location on the vehicle. This method returns\r
+ % the wheel which is created, a OBJECT(CeldasWheel).\r
+\r
+ wheel, joint (object).\r
+\r
+ wheel = new CeldasWheel.\r
+ wheel set-shape to wheelShape.\r
+\r
+ joint = new RevoluteJoint.\r
+\r
+ joint set-relative-rotation around-axis (1, 0, 0) by 1.5708.\r
+ joint link parent bodyLink to-child wheel with-normal (0, 0, 1)\r
+ with-parent-point location with-child-point (0, 0, 0).\r
+\r
+ wheel set-eT to .8.\r
+ wheel set-texture to 0.\r
+ wheel set-joint to joint.\r
+ joint set-strength-limit to (joint get-strength-hard-limit) / 2.\r
+ wheel set-color to (.6, .6, .6).\r
+ wheel set-mu to 100000.\r
+\r
+ self add-dependency on joint.\r
+ self add-dependency on wheel.\r
+\r
+ push wheel onto wheels.\r
+\r
+ return wheel.\r
+\r
+ + to add-sensor at location (vector) with-direction direction = (0,1,0)(vector) :\r
+ % Adds a sensor at location on the vehicle. This method returns\r
+ % the sensor which is created, a OBJECT(CeldasSensor).\r
+\r
+ sensor, joint (object).\r
+\r
+ sensor = new CeldasSensor.\r
+ sensor set-direction to direction.\r
+ \r
+ sensor set-shape to sensorShape.\r
+\r
+ joint = new RevoluteJoint.\r
+\r
+ joint set-relative-rotation around-axis (0, 0, 1) by -1.57.\r
+ joint link parent bodyLink to-child sensor with-normal (1, 0, 0)\r
+ with-parent-point location with-child-point (0, 0, 0).\r
+\r
+ joint set-double-spring with-strength 300 with-max 0.01 with-min -0.01.\r
+\r
+ self add-dependency on joint.\r
+ self add-dependency on sensor.\r
+\r
+ sensor set-color to (0, 0, 0).\r
+\r
+ #push sensor onto sensors.\r
+\r
+ return sensor.\r
+\r
+ + to destroy:\r
+ free sensorShape.\r
+ free wheelShape.\r
+ free bodyShape.\r
+\r
+ super destroy.\r
+}\r
+\r
+CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {\r
+ % A heavy duty version of OBJECT(CeldasLightVehicle), this\r
+ % vehicle is heavier and harder to control, but more stable\r
+ % at higher speeds.\r
+ +variables:\r
+ lSensor, rSensor, fSensor, bSensor (object).\r
+ lfWheel,rfWheel,lbWheel,rbWheel (object).\r
+ tleft,tright (int). \r
+ avanzando,retrocediendo,girando_izq,girando_der(int). \r
+ iterate(int).\r
+ teorias (list).\r
+ sa (object).\r
+ teoria (object).\r
+ entorno (hash).\r
+ datos-finales (hash).\r
+ plan-finished (int).\r
+ posicion-inicial (vector).\r
+ posicion-final (vector).\r
+ \r
+ - to get-density:\r
+ return 20.0.\r
+\r
+ - to get-wheel-width:\r
+ return 0.4.\r
+\r
+ - to get-wheel-radius:\r
+ return 0.8.\r
+\r
+ - to near position thePosition (vector) with-error error (float):\r
+ vectorAux(vector).\r
+ vectorAux = (self get-location) - thePosition.\r
+\r
+ #print "-----> (pos, other_pos, diff, error): ", (self get-location), thePosition, vectorAux, error.\r
+\r
+ if ((|vectorAux::x| < error) && (|vectorAux::z| < error)):\r
+ return 1.\r
+\r
+ return 0.\r
+\r
+ + to set-global-velocity to velocity (float):\r
+ rfWheel set-velocity to velocity.\r
+ lfWheel set-velocity to velocity.\r
+ rbWheel set-velocity to velocity.\r
+ lbWheel set-velocity to velocity.\r
+\r
+ + to get-global-velocity:\r
+ return ((rfWheel get-velocity) + (lfWheel get-velocity)) / 2.\r
+\r
+ + to turn-right: \r
+ #tright++.\r
+ #self rotate around-axis (0,1,0) by (-1.5709/CELDAS_TURNO)*tright. \r
+ #if (tright == CELDAS_TURNO): tright=0.\r
+ if (tright == 0): self set-global-velocity to 0.\r
+ self rotate around-axis (0,1,0) by (-1.5709/CELDAS_TURNO)*tright. \r
+ if (tright == CELDAS_TURNO): tright=0.\r
+ else tright++.\r
+\r
+\r
+ + to turn-left:\r
+ #tleft++.\r
+ #self rotate around-axis (0,1,0) by (1.5709/CELDAS_TURNO)*tleft. \r
+ #if (tleft == CELDAS_TURNO): tleft=0.\r
+ if (tleft == 0): self set-global-velocity to 0.\r
+ self rotate around-axis (0,1,0) by (1.5709/CELDAS_TURNO)*tleft.\r
+ if (tleft == CELDAS_TURNO): tleft=0.\r
+ else tleft++.\r
+\r
+\r
+ + to get-sensor-value:\r
+ return (fSensor get-sensor-value).\r
+\r
+\r
+\r
+ +to update-entorno:\r
+ entorno{"sensor_f"} = (fSensor get-sensor-value).\r
+ entorno{"sensor_b"} = (bSensor get-sensor-value).\r
+ entorno{"sensor_r"} = (rSensor get-sensor-value).\r
+ entorno{"sensor_l"} = (lSensor get-sensor-value).\r
+ sa update-entorno with entorno. \r
+\r
+ +to init:\r
+ # Configuracion de robot\r
+ fSensor = (self add-sensor at (2.0, .4, 0)). \r
+ fSensor set-direction to (1,0,0).\r
+ #fSensor set-direction to (0,0,1).\r
+ fSensor set-id at 1.\r
+ fSensor set-body at self.\r
+ bSensor = (self add-sensor at (-2.0, .4, 0)).\r
+ bSensor set-direction to (-1,0,0).\r
+ #bSensor set-direction to (0,0,1).\r
+ bSensor set-id at 2.\r
+ bSensor set-body at self.\r
+ lSensor = (self add-sensor at (0, .4, 1.5)).\r
+ lSensor set-direction to (0,0,1).\r
+ #lSensor set-direction to (1,0,0).\r
+ lSensor set-id at 3.\r
+ lSensor set-body at self.\r
+\r
+ rSensor = (self add-sensor at (0, .4, -1.5)).\r
+ rSensor set-direction to (0,0,-1).\r
+ #rSensor set-direction to (-1,0,0).\r
+ rSensor set-id at 4.\r
+ rSensor set-body at self.\r
+\r
+ lfWheel = (self add-wheel at (2, 0, -1.5)).\r
+ lbWheel = (self add-wheel at (-2, 0, -1.5)).\r
+ rfWheel = (self add-wheel at (2, 0, 1.5)).\r
+ rbWheel = (self add-wheel at (-2, 0, 1.5)).\r
+\r
+ tleft=tright=0.\r
+ avanzando=0.\r
+ retrocediendo=0.\r
+ girando_izq=0. \r
+ girando_der=0. \r
+\r
+ posicion-inicial = (self get-location).\r
+ posicion-final = (0, 0, 0).\r
+\r
+ # Configuracion de sistema autonomo\r
+ sa = new SistemaAutonomo.\r
+ sa init with-max-pasos 4 with-max-teorias 15.\r
+ iterate = 0.\r
+ plan-finished = 1. # así planificamos apenas empezamos\r
+\r
+ teorias = 4 new Teorias.\r
+ teorias{0} init named "Avanzar" with-action "adelante".\r
+ teorias{0} set-dato-inicial name "sensor_f" value 0.\r
+ teorias{0} set-dato-inicial name "sensor_b" value ANY.\r
+ teorias{0} set-dato-inicial name "sensor_r" value ANY.\r
+ teorias{0} set-dato-inicial name "sensor_l" value ANY.\r
+ teorias{0} set-dato-inicial name "movido" value ANY.\r
+ teorias{0} set-dato-final name "sensor_f" value ANY.\r
+ teorias{0} set-dato-final name "sensor_b" value ANY.\r
+ teorias{0} set-dato-final name "sensor_r" value ANY.\r
+ teorias{0} set-dato-final name "sensor_l" value ANY.\r
+ teorias{0} set-dato-final name "movido" value 1.\r
+\r
+ teorias{1} init named "Retroceder" with-action "atras" executed 2.\r
+ teorias{1} set-dato-inicial name "sensor_f" value 1.\r
+ teorias{1} set-dato-inicial name "sensor_b" value ANY.\r
+ teorias{1} set-dato-inicial name "sensor_r" value ANY.\r
+ teorias{1} set-dato-inicial name "sensor_l" value ANY.\r
+ teorias{1} set-dato-inicial name "movido" value ANY.\r
+ teorias{1} set-dato-final name "sensor_f" value 0.\r
+ teorias{1} set-dato-final name "sensor_b" value ANY.\r
+ teorias{1} set-dato-final name "sensor_r" value ANY.\r
+ teorias{1} set-dato-final name "sensor_l" value ANY.\r
+ teorias{1} set-dato-final name "movido" value 1.\r
+\r
+ teorias{2} init named "Rotar a derecha" with-action "derecha".\r
+ teorias{2} set-dato-inicial name "sensor_f" value 1.\r
+ teorias{2} set-dato-inicial name "sensor_b" value ANY.\r
+ teorias{2} set-dato-inicial name "sensor_r" value ANY.\r
+ teorias{2} set-dato-inicial name "sensor_l" value ANY.\r
+ teorias{2} set-dato-inicial name "movido" value ANY.\r
+ teorias{2} set-dato-final name "sensor_f" value 0.\r
+ teorias{2} set-dato-final name "sensor_b" value ANY.\r
+ teorias{2} set-dato-final name "sensor_r" value ANY.\r
+ teorias{2} set-dato-final name "sensor_l" value 1.\r
+ teorias{2} set-dato-final name "movido" value 0.\r
+\r
+ teorias{3} init named "Rotar a izquierda" with-action "izquierda" executed 2.\r
+ teorias{3} set-dato-inicial name "sensor_f" value 1.\r
+ teorias{3} set-dato-inicial name "sensor_b" value ANY.\r
+ teorias{3} set-dato-inicial name "sensor_r" value ANY.\r
+ teorias{3} set-dato-inicial name "sensor_l" value ANY.\r
+ teorias{3} set-dato-inicial name "movido" value ANY.\r
+ teorias{3} set-dato-final name "sensor_f" value 0.\r
+ teorias{3} set-dato-final name "sensor_b" value ANY.\r
+ teorias{3} set-dato-final name "sensor_r" value 1.\r
+ teorias{3} set-dato-final name "sensor_l" value ANY.\r
+ teorias{3} set-dato-final name "movido" value 0.\r
+\r
+ sa add teoria teorias{0}.\r
+ sa add teoria teorias{1}.\r
+ sa add teoria teorias{2}.\r
+ sa add teoria teorias{3}.\r
+\r
+ datos-finales{"movido"} = 1.\r
+ sa update-datos-finales with datos-finales.\r
+\r
+ +to iterate:\r
+\r
+ # Actualiza entorno\r
+ self update-entorno.\r
+\r
+ # Chequeo de objetivo\r
+ if (self near position posicion-final with-error 5.0):\r
+ {\r
+ print "Llegamos al FINAL!!!".\r
+ self set-global-velocity to 0.\r
+ return.\r
+ }\r
+\r
+ # Planificación\r
+ if (plan-finished):\r
+ {\r
+ # Actualiza entorno indicando que no se movió para que\r
+ # el planificador actue\r
+ sa set-entorno value 0 with-name "movido".\r
+ sa plan. # Si no tenemos plan, lo hacemos\r
+ plan-finished = 0.\r
+ iterate = 0.\r
+ if (! (sa has-next-theory)):\r
+ {\r
+ plan-finished = 1.\r
+ print "El planificador no encuentra PLAN!!!".\r
+ return.\r
+ }\r
+ }\r
+\r
+ # Ejecución de teoría\r
+ if (iterate == 0):\r
+ {\r
+ posicion-inicial = (self get-location).\r
+ if (sa has-next-theory):\r
+ {\r
+ teoria = sa get-next-theory.\r
+ if ((teoria get-accion) == "adelante"):\r
+ {\r
+ avanzando = 1.\r
+ retrocediendo = 0.\r
+ girando_izq = 0.\r
+ girando_der = 0.\r
+ }\r
+ if ((teoria get-accion) == "atras"):\r
+ {\r
+ avanzando = 0.\r
+ retrocediendo = 1.\r
+ girando_izq = 0.\r
+ girando_der = 0.\r
+ }\r
+ if ((teoria get-accion) == "izquierda"):\r
+ {\r
+ avanzando = 0.\r
+ retrocediendo = 0.\r
+ girando_izq = 1.\r
+ girando_der = 0.\r
+ }\r
+ if ((teoria get-accion) == "derecha"):\r
+ {\r
+ avanzando = 0.\r
+ retrocediendo = 0.\r
+ girando_izq = 0.\r
+ girando_der = 1.\r
+ }\r
+ }\r
+ else\r
+ {\r
+ plan-finished = 1.\r
+ }\r
+ }\r
+\r
+ # Validación de teoría\r
+ if (iterate == CELDAS_TURNO):\r
+ {\r
+ # Actualiza entorno segun si se movio o no\r
+ if (self near position posicion-inicial with-error 2.0):\r
+ {\r
+ sa set-entorno value 0 with-name "movido".\r
+ }\r
+ else\r
+ {\r
+ sa set-entorno value 1 with-name "movido".\r
+ }\r
+ print iterate.\r
+ if (!(sa validate theory teoria)):\r
+ {\r
+ plan-finished = 1.\r
+ }\r
+ iterate = 0.\r
+ }\r
+ else\r
+ {\r
+ iterate++.\r
+ }\r
+\r
+ # Movimiento del robot\r
+ if (avanzando):\r
+ self set-global-velocity to (CELDAS_MAX_VELOCITY).\r
+ if (retrocediendo):\r
+ self set-global-velocity to (-CELDAS_MAX_VELOCITY).\r
+ if (girando_izq):\r
+ self turn-left.\r
+ if (girando_der):\r
+ self turn-right.\r
+\r
+}\r
+\r
+Stationary : CeldasObstacle (aka CeldasObstacles) {\r
+ % A CeldasObstacle is used in conjunction with OBJECT(CeldasControl)\r
+ % and OBJECT(CeldasVehicle). It is what the OBJECT(CeldasSensor)\r
+ % objects on the CeldasVehicle detect.\r
+ % <p>\r
+ % There are no special behaviors associated with the walls--they're \r
+ % basically just plain OBJECT(Stationary) objects.\r
+ \r
+ +variables:\r
+ large (float).\r
+ direction (vector). \r
+\r
+\r
+ + 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
+ self init-with-shape shape (new Shape init-with-cube size theSize) color theColor at-location theLocation with-rotation theRotation.\r
+ large=20.\r
+\r
+ + 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
+ self register with-shape theShape at-location theLocation with-rotation theRotation.\r
+ self set-color to theColor.\r
+ \r
+ + to get-large:\r
+ return large.\r
+\r
+ + to set-direction at theDirection (vector):\r
+ direction=theDirection.\r
+\r
+ + to get-direction:\r
+ return direction.\r
+}\r
+\r
+Link : CeldasWheel (aka CeldasWheels) {\r
+ % A CeldasWheel is used in conjunction with OBJECT(CeldasVehicle)\r
+ % to build Celdas vehicles. This class is typically not instantiated\r
+ % manually, since OBJECT(CeldasVehicle) creates one for you when you\r
+ % add a wheel to the vehicle.\r
+\r
+ + variables:\r
+ joint (object).\r
+ velocity (float).\r
+\r
+ + to init:\r
+ velocity = 0.\r
+\r
+ - to set-joint to j (object):\r
+ % Used internally.\r
+\r
+ joint = j.\r
+\r
+ + section "Configuring the Wheel's Velocity"\r
+\r
+ + to set-velocity to n (float):\r
+ % Sets the velocity of this wheel.\r
+\r
+ if n > CELDAS_MAX_VELOCITY: n = CELDAS_MAX_VELOCITY.\r
+ velocity = n.\r
+\r
+ joint set-joint-velocity to velocity.\r
+\r
+ + to get-velocity:\r
+ % Gets the velocity of this wheel.\r
+ \r
+ return velocity.\r
+\r
+}\r
+\r
+Link : CeldasSensor (aka CeldasSensors) {\r
+ % A CeldasSensor is used in conjunction with OBJECT(CeldasVehicle)\r
+ % to build Celdas vehicles. This class is typically not instantiated\r
+ % manually, since OBJECT(CeldasVehicle) creates one for you when you\r
+ % add a sensor to the vehicle.\r
+\r
+ + variables:\r
+ direction (vector).\r
+ positiveDirection(vector).\r
+ sensorAngle (float).\r
+ value (float).\r
+ draw (object).\r
+ body(object).\r
+ id(int).\r
+\r
+ + to init :\r
+ direction = (1,0,1).\r
+ positiveDirection= (1,0,1).\r
+ sensorAngle = 1.6.\r
+ value = 0.0.\r
+ draw = new Drawing.\r
+ \r
+\r
+ + section "Configuring the Sensor Values"\r
+ + to set-id at n (int):\r
+ id=n.\r
+\r
+ + to set-body at robotBody(object):\r
+ body=robotBody.\r
+ \r
+ + to set-sensor-angle to n (float):\r
+ % Sets the angle in which this sensor can detect obstacles. The default\r
+ % value of 1.6 means that the sensor can see most of everything in\r
+ % front of it. Setting the value to be any higher leads to general\r
+ % wackiness, so I don't suggest it.\r
+\r
+ sensorAngle = n.\r
+\r
+ + to set-direction to n (vector):\r
+ direction = n.\r
+ positiveDirection::x=|n::x|.\r
+ positiveDirection::y=|n::y|.\r
+ positiveDirection::z=|n::z|.\r
+\r
+ + section "Getting the Sensor Values"\r
+\r
+ + to get-sensor-value:\r
+ % Gets the sensor value. This should be used from post-iterate,\r
+ % if not, the sensor reading correspond to the previous\r
+ % iteration.\r
+ val (float).\r
+\r
+ val = self get-data.\r
+ if (val > CELDAS_SENSOR_THRESHOLD): return 0.\r
+ else return 1.\r
+ \r
+ #+ to iterate:\r
+ \r
+ + to get-data:\r
+ i (object).\r
+ min,dist (float).\r
+ v,obs(vector).\r
+ j (int).\r
+ des2,des3(int).\r
+ wallBegin,wallEnd,wallCenter (float).\r
+ obsLoc (vector). \r
+ posObstacle,destiny,yo(vector).\r
+ \r
+ draw clear.\r
+ value = 0.0.\r
+ j=0.\r
+ min=0.\r
+ foreach i in (all CeldasObstacles): \r
+ {\r
+ posObstacle=i get-location.\r
+ v = (body get-location) - (self get-location ).\r
+ obsLoc::y=posObstacle::y.\r
+ \r
+ if (dot((i get-direction),(1,0,0))):\r
+ {\r
+ obsLoc::x=((self get-location)::x + ((posObstacle::z - (self get-location)::z)*v::x/v::z)).\r
+ obsLoc::z=posObstacle::z.\r
+ } \r
+ else\r
+ {\r
+ obsLoc::z=((self get-location)::z + ((posObstacle::x - (self get-location)::x)*v::z/v::x)).\r
+ obsLoc::x=posObstacle::x.\r
+ } \r
+ \r
+ #!\r
+ if(dot((i get-direction),direction)==0):\r
+ des1=1.\r
+ else\r
+ des1=0.\r
+ !#\r
+\r
+ des2=0.\r
+ if(dot(direction,(1,1,1))<0):\r
+ { \r
+ if((dot((self get-location),positiveDirection))>(dot(obsLoc,positiveDirection))):\r
+ des2=1. \r
+ }\r
+ else\r
+ {\r
+ if((dot((self get-location),positiveDirection))<(dot(obsLoc,positiveDirection))):\r
+ des2=1. \r
+ } \r
+\r
+\r
+ #Compruebo que el robot este frente a la pared\r
+ wallCenter=dot((i get-location),(i get-direction)).\r
+ wallBegin=wallCenter- (i get-large)/2.\r
+ wallEnd=wallCenter + (i get-large)/2. \r
+\r
+ \r
+ yo=self get-location.\r
+ destiny=i get-direction.\r
+\r
+ \r
+\r
+ if (dot((self get-location),(i get-direction)) > wallBegin) && (dot((self get-location),(i get-direction)) < wallEnd):\r
+ des3=1.\r
+ else\r
+ {\r
+ des3=0.\r
+ \r
+ } \r
+ \r
+ if ((des2) && (des3)):\r
+ { \r
+ draw clear.\r
+\r
+ dist=|obsLoc - (self get-location)|.\r
+ if( (j==0) || (min>dist) ):\r
+ {\r
+ min=dist.\r
+ obs=obsLoc.\r
+ j++.\r
+ #print "sensor: $id obstaculo: $posObstacle direP: $destiny direS: $direction yo: $yo ". \r
+ }\r
+\r
+ } \r
+\r
+ \r
+ } #end for\r
+\r
+ if(j!=0):\r
+ {\r
+ #Dibujo el laser\r
+ draw set-color to (1, 0, 0).\r
+ draw draw-line from (self get-location) to (obs).\r
+ return min.\r
+ }\r
+ \r
+\r
+ value = -1.\r
+ return value.\r
+\r
+\r
+}\r
+ \r
projects / z.facultad / 75.68 / celdas.git / blobdiff