- % 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). - - - 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. - - joint set-joint-velocity to n. - - + to get-velocity: - % Gets the velocity of this wheel. - - return (joint get-joint-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. - - return value. - - + 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 = 100.0 / (strength * strength) . - - if strength > value: value = strength. - } - } - -} +@use PhysicalControl. +@use Shape. +@use Stationary. +@use Link. +@use MultiBody. +@use Drawing. +@use SistemaAutonomo. + +@define CELDAS_MAX_VELOCITY 5. +@define CELDAS_TURNO 30. +@define CELDAS_SENSOR_THRESHOLD 10. + +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). + + + 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) with-direction direction = (0,1,0)(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-direction to direction. + + 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. + +variables: + lSensor, rSensor, fSensor, bSensor (object). + lfWheel,rfWheel,lbWheel,rbWheel (object). + tleft,tright (int). + avanzando,retrocediendo,girando_izq,girando_der(int). + iterate(int). + teorias (list). + sa (object). + teoria (object). + entorno (hash). + datos-finales (hash). + plan-finished (int). + posicion-inicial (vector). + posicion-final (vector). + + - to get-density: + return 20.0. + + - to get-wheel-width: + return 0.4. + + - to get-wheel-radius: + return 0.8. + + - to near position thePosition (vector) with-error error (float): + vectorAux(vector). + vectorAux = (self get-location) - thePosition. + + #print "-----> (pos, other_pos, diff, error): ", (self get-location), thePosition, vectorAux, error. + + if ((|vectorAux::x| < error) && (|vectorAux::z| < error)): + return 1. + + return 0. + + + to set-global-velocity to velocity (float): + rfWheel set-velocity to velocity. + lfWheel set-velocity to velocity. + rbWheel set-velocity to velocity. + lbWheel set-velocity to velocity. + + + to get-global-velocity: + return ((rfWheel get-velocity) + (lfWheel get-velocity)) / 2. + + + to turn-right: + #tright++. + #self rotate around-axis (0,1,0) by (-1.5709/CELDAS_TURNO)*tright. + #if (tright == CELDAS_TURNO): tright=0. + if (tright == 0): self set-global-velocity to 0. + self rotate around-axis (0,1,0) by (-1.5709/CELDAS_TURNO)*tright. + if (tright == CELDAS_TURNO): tright=0. + else tright++. + + + + to turn-left: + #tleft++. + #self rotate around-axis (0,1,0) by (1.5709/CELDAS_TURNO)*tleft. + #if (tleft == CELDAS_TURNO): tleft=0. + if (tleft == 0): self set-global-velocity to 0. + self rotate around-axis (0,1,0) by (1.5709/CELDAS_TURNO)*tleft. + if (tleft == CELDAS_TURNO): tleft=0. + else tleft++. + + + + to get-sensor-value: + return (fSensor get-sensor-value). + + + + +to update-entorno: + entorno{"sensor_f"} = (fSensor get-sensor-value). + entorno{"sensor_b"} = (bSensor get-sensor-value). + entorno{"sensor_r"} = (rSensor get-sensor-value). + entorno{"sensor_l"} = (lSensor get-sensor-value). + sa update-entorno with entorno. + + +to init: + # Configuracion de robot + fSensor = (self add-sensor at (2.0, .4, 0)). + fSensor set-direction to (1,0,0). + #fSensor set-direction to (0,0,1). + fSensor set-id at 1. + fSensor set-body at self. + bSensor = (self add-sensor at (-2.0, .4, 0)). + bSensor set-direction to (-1,0,0). + #bSensor set-direction to (0,0,1). + bSensor set-id at 2. + bSensor set-body at self. + lSensor = (self add-sensor at (0, .4, 1.5)). + lSensor set-direction to (0,0,1). + #lSensor set-direction to (1,0,0). + lSensor set-id at 3. + lSensor set-body at self. + + rSensor = (self add-sensor at (0, .4, -1.5)). + rSensor set-direction to (0,0,-1). + #rSensor set-direction to (-1,0,0). + rSensor set-id at 4. + rSensor set-body at self. + + lfWheel = (self add-wheel at (2, 0, -1.5)). + lbWheel = (self add-wheel at (-2, 0, -1.5)). + rfWheel = (self add-wheel at (2, 0, 1.5)). + rbWheel = (self add-wheel at (-2, 0, 1.5)). + + tleft=tright=0. + avanzando=0. + retrocediendo=0. + girando_izq=0. + girando_der=0. + + posicion-inicial = (self get-location). + posicion-final = (0, 0, 0). + + # Configuracion de sistema autonomo + sa = new SistemaAutonomo. + sa init with-max-pasos 4 with-max-teorias 15. + iterate = 0. + plan-finished = 1. # así planificamos apenas empezamos + + teorias = 4 new Teorias. + teorias{0} init named "Avanzar" with-action "adelante". + teorias{0} set-dato-inicial name "sensor_f" value 0. + teorias{0} set-dato-inicial name "sensor_b" value ANY. + teorias{0} set-dato-inicial name "sensor_r" value ANY. + teorias{0} set-dato-inicial name "sensor_l" value ANY. + teorias{0} set-dato-inicial name "movido" value ANY. + teorias{0} set-dato-final name "sensor_f" value ANY. + teorias{0} set-dato-final name "sensor_b" value ANY. + teorias{0} set-dato-final name "sensor_r" value ANY. + teorias{0} set-dato-final name "sensor_l" value ANY. + teorias{0} set-dato-final name "movido" value 1. + + teorias{1} init named "Retroceder" with-action "atras" executed 2. + teorias{1} set-dato-inicial name "sensor_f" value 1. + teorias{1} set-dato-inicial name "sensor_b" value ANY. + teorias{1} set-dato-inicial name "sensor_r" value ANY. + teorias{1} set-dato-inicial name "sensor_l" value ANY. + teorias{1} set-dato-inicial name "movido" value ANY. + teorias{1} set-dato-final name "sensor_f" value 0. + teorias{1} set-dato-final name "sensor_b" value ANY. + teorias{1} set-dato-final name "sensor_r" value ANY. + teorias{1} set-dato-final name "sensor_l" value ANY. + teorias{1} set-dato-final name "movido" value 1. + + teorias{2} init named "Rotar a derecha" with-action "derecha". + teorias{2} set-dato-inicial name "sensor_f" value 1. + teorias{2} set-dato-inicial name "sensor_b" value ANY. + teorias{2} set-dato-inicial name "sensor_r" value ANY. + teorias{2} set-dato-inicial name "sensor_l" value ANY. + teorias{2} set-dato-inicial name "movido" value ANY. + teorias{2} set-dato-final name "sensor_f" value 0. + teorias{2} set-dato-final name "sensor_b" value ANY. + teorias{2} set-dato-final name "sensor_r" value ANY. + teorias{2} set-dato-final name "sensor_l" value 1. + teorias{2} set-dato-final name "movido" value 0. + + teorias{3} init named "Rotar a izquierda" with-action "izquierda" executed 2. + teorias{3} set-dato-inicial name "sensor_f" value 1. + teorias{3} set-dato-inicial name "sensor_b" value ANY. + teorias{3} set-dato-inicial name "sensor_r" value ANY. + teorias{3} set-dato-inicial name "sensor_l" value ANY. + teorias{3} set-dato-inicial name "movido" value ANY. + teorias{3} set-dato-final name "sensor_f" value 0. + teorias{3} set-dato-final name "sensor_b" value ANY. + teorias{3} set-dato-final name "sensor_r" value 1. + teorias{3} set-dato-final name "sensor_l" value ANY. + teorias{3} set-dato-final name "movido" value 0. + + sa add teoria teorias{0}. + sa add teoria teorias{1}. + sa add teoria teorias{2}. + sa add teoria teorias{3}. + + datos-finales{"movido"} = 1. + sa update-datos-finales with datos-finales. + + +to iterate: + + # Actualiza entorno + self update-entorno. + + # Chequeo de objetivo + if (self near position posicion-final with-error 5.0): + { + print "Llegamos al FINAL!!!". + self set-global-velocity to 0. + return. + } + + # Planificación + if (plan-finished): + { + # Actualiza entorno indicando que no se movió para que + # el planificador actue + sa set-entorno value 0 with-name "movido". + sa plan. # Si no tenemos plan, lo hacemos + plan-finished = 0. + iterate = 0. + if (! (sa has-next-theory)): + { + plan-finished = 1. + print "El planificador no encuentra PLAN!!!". + return. + } + } + + # Ejecución de teoría + if (iterate == 0): + { + posicion-inicial = (self get-location). + if (sa has-next-theory): + { + teoria = sa get-next-theory. + if ((teoria get-accion) == "adelante"): + { + avanzando = 1. + retrocediendo = 0. + girando_izq = 0. + girando_der = 0. + } + if ((teoria get-accion) == "atras"): + { + avanzando = 0. + retrocediendo = 1. + girando_izq = 0. + girando_der = 0. + } + if ((teoria get-accion) == "izquierda"): + { + avanzando = 0. + retrocediendo = 0. + girando_izq = 1. + girando_der = 0. + } + if ((teoria get-accion) == "derecha"): + { + avanzando = 0. + retrocediendo = 0. + girando_izq = 0. + girando_der = 1. + } + } + else + { + plan-finished = 1. + } + } + + # Validación de teoría + if (iterate == CELDAS_TURNO): + { + # Actualiza entorno segun si se movio o no + if (self near position posicion-inicial with-error 2.0): + { + sa set-entorno value 0 with-name "movido". + } + else + { + sa set-entorno value 1 with-name "movido". + } + print iterate. + if (!(sa validate theory teoria)): + { + plan-finished = 1. + } + iterate = 0. + } + else + { + iterate++. + } + + # Movimiento del robot + if (avanzando): + self set-global-velocity to (CELDAS_MAX_VELOCITY). + if (retrocediendo): + self set-global-velocity to (-CELDAS_MAX_VELOCITY). + if (girando_izq): + self turn-left. + if (girando_der): + self turn-right. + +} + +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. + %
+ % There are no special behaviors associated with the walls--they're + % basically just plain OBJECT(Stationary) objects. + + +variables: + large (float). + direction (vector). + + + + 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. + large=20. + + + 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. + + + to get-large: + return large. + + + to set-direction at theDirection (vector): + direction=theDirection. + + + to get-direction: + return direction. +} + +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). + positiveDirection(vector). + sensorAngle (float). + value (float). + draw (object). + body(object). + id(int). + + + to init : + direction = (1,0,1). + positiveDirection= (1,0,1). + sensorAngle = 1.6. + value = 0.0. + draw = new Drawing. + + + + section "Configuring the Sensor Values" + + to set-id at n (int): + id=n. + + + to set-body at robotBody(object): + body=robotBody. + + + 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. + + + to set-direction to n (vector): + direction = n. + positiveDirection::x=|n::x|. + positiveDirection::y=|n::y|. + positiveDirection::z=|n::z|. + + + 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. + val (float). + + val = self get-data. + if (val > CELDAS_SENSOR_THRESHOLD): return 0. + else return 1. + + #+ to iterate: + + + to get-data: + i (object). + min,dist (float). + v,obs(vector). + j (int). + des2,des3(int). + wallBegin,wallEnd,wallCenter (float). + obsLoc (vector). + posObstacle,destiny,yo(vector). + + draw clear. + value = 0.0. + j=0. + min=0. + foreach i in (all CeldasObstacles): + { + posObstacle=i get-location. + v = (body get-location) - (self get-location ). + obsLoc::y=posObstacle::y. + + if (dot((i get-direction),(1,0,0))): + { + obsLoc::x=((self get-location)::x + ((posObstacle::z - (self get-location)::z)*v::x/v::z)). + obsLoc::z=posObstacle::z. + } + else + { + obsLoc::z=((self get-location)::z + ((posObstacle::x - (self get-location)::x)*v::z/v::x)). + obsLoc::x=posObstacle::x. + } + + #! + if(dot((i get-direction),direction)==0): + des1=1. + else + des1=0. + !# + + des2=0. + if(dot(direction,(1,1,1))<0): + { + if((dot((self get-location),positiveDirection))>(dot(obsLoc,positiveDirection))): + des2=1. + } + else + { + if((dot((self get-location),positiveDirection))<(dot(obsLoc,positiveDirection))): + des2=1. + } + + + #Compruebo que el robot este frente a la pared + wallCenter=dot((i get-location),(i get-direction)). + wallBegin=wallCenter- (i get-large)/2. + wallEnd=wallCenter + (i get-large)/2. + + + yo=self get-location. + destiny=i get-direction. + + + + if (dot((self get-location),(i get-direction)) > wallBegin) && (dot((self get-location),(i get-direction)) < wallEnd): + des3=1. + else + { + des3=0. + + } + + if ((des2) && (des3)): + { + draw clear. + + dist=|obsLoc - (self get-location)|. + if( (j==0) || (min>dist) ): + { + min=dist. + obs=obsLoc. + j++. + #print "sensor: $id obstaculo: $posObstacle direP: $destiny direS: $direction yo: $yo ". + } + + } + + + } #end for + + if(j!=0): + { + #Dibujo el laser + draw set-color to (1, 0, 0). + draw draw-line from (self get-location) to (obs). + return min. + } + + + value = -1. + return value. + + +} +