--- /dev/null
+@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 30.\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
+ sensors (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
+ lWheel,rWheel (object).\r
+ tleft,tright (int). \r
+ iterate(int).\r
+\r
+ teorias (list).\r
+ sa (object).\r
+ teoria (object).\r
+ entorno (hash).\r
+ datos-finales (hash).\r
+ \r
+ \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 set-global-velocity to velocity=5 (float):\r
+ rWheel set-velocity to velocity.\r
+ lWheel set-velocity to velocity.\r
+\r
+ + to get-global-velocity:\r
+ return ((rWheel get-velocity) + (lWheel get-velocity)) / 2.\r
+\r
+ + to turn-right: \r
+ tright++.\r
+\r
+ self rotate around-axis (0,1,0) by (-1.5709/10)*tright. \r
+ \r
+ if(tright==10): tright=0.\r
+\r
+\r
+ + to turn-left:\r
+ tleft++.\r
+\r
+ self rotate around-axis (0,1,0) by (1.5709/10)*tleft. \r
+ \r
+ if(tleft==10): tleft=0.\r
+\r
+\r
+ + to get-sensor-value:\r
+ return (fSensor get-sensor-value).\r
+\r
+ +to init:\r
+ sa = new SistemaAutonomo.\r
+ iterate=0.\r
+\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
+\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
+ lWheel = (self add-wheel at (0, 0, -1.5)).\r
+ rWheel = (self add-wheel at (0, 0, 1.5)).\r
+\r
+ tleft=tright=0. #Debe ser inicializado en 0 esta asi para probar!!!!!!!!!!!!!!!!!!!!!!!!\r
+\r
+ entorno{"sensor_f"} = 0.\r
+ entorno{"sensor_b"} = 0.\r
+ entorno{"sensor_r"} = 0.\r
+ entorno{"sensor_l"} = 0.\r
+\r
+ entorno{"movido"} = 0.\r
+ sa update-entorno entorno entorno. \r
+ \r
+\r
+ teorias = 2 new Teorias.\r
+ teorias{0} init named "avanzar" with-action "avanza".\r
+ (teorias{0}) set-dato-inicial name "sensor_f" value ANY.\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 ANY.\r
+\r
+ teorias{1} init named "avanzar2" with-action "avanza".\r
+ (teorias{1}) set-dato-inicial name "sensor_f" value ANY.\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 ANY.\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 ANY.\r
+\r
+\r
+ sa add teoria (teorias{0}).\r
+ sa add teoria (teorias{1}).\r
+\r
+ datos-finales{"movido"} = 1.\r
+\r
+ sa update-datos-finales datos-finales datos-finales.\r
+ sa plan.\r
+\r
+ \r
+ +to iterate:\r
+ #+ to post-iterate:\r
+ valuef,valueb,valuer,valuel (float).\r
+ fl, fr(float).\r
+\r
+\r
+\r
+ if(iterate==0): {\r
+ print "iteracion 0".\r
+ if (sa has-next-theory):\r
+ {\r
+\r
+ print "hay teoria".\r
+ teoria = sa get-next-theory.\r
+ if ((teoria get-accion) == "avanza"): {\r
+ #entorno{"sensor"} = 1.\r
+ #entorno{"movido"} = 1.\r
+ self set-global-velocity.\r
+ \r
+ }\r
+ if ((teoria get-accion) == "retrocede"): {\r
+ self set-global-velocity to -5.\r
+\r
+ }\r
+ }\r
+ }\r
+\r
+ if (iterate==100) && (teoria):\r
+ {\r
+\r
+ entorno{"sensor_f"} = (fSensor get-data).\r
+ entorno{"sensor_b"} = (bSensor get-data).\r
+ entorno{"sensor_r"} = (rSensor get-data).\r
+ entorno{"sensor_l"} = (lSensor get-data).\r
+\r
+ sa update-entorno entorno entorno.\r
+ if (sa validate theory teoria): {\r
+ print "valida".\r
+ }\r
+ else {\r
+ print "Teoria no valida, salimos".\r
+ }\r
+\r
+ }\r
+ \r
+ iterate++.\r
+ if(iterate==101):\r
+ iterate=0.\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
+ \r
+ #+ to iterate:\r
+ \r
+ + to get-data:\r
+ i (object).\r
+ min,dist (float).\r
+ v,obs(vector).\r
+ aux(float).\r
+ j (int).\r
+ des2,des3(int).\r
+ wallBegin,wallEnd,wallCenter (float).\r
+ \r
+ toObstacle(vector).\r
+ largeWall (float).\r
+ \r
+ obsLoc (vector). \r
+ location (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
+ #aux1=dot((self get-location),(i get-direction)).\r
+\r
+ #print "sensor: $id obstaculo: $posObstacle direP: $destiny direS: $direction yo: $yo ".\r
+ #print "begin: $wallBegin end: $wallEnd ".\r
+ #print "begin: $wallBegin end: $wallEnd".\r
+\r
+ \r
+\r
+ #print "sensor: $id , des1: $des1, des2: $des2, des3: $des3".\r
+ if ((des2) && (des3)):\r
+ { \r
+ draw clear. \r
+ #print " posObstacle: $posObstacle". \r
+ \r
+ print "sensor: $id obstaculo: $posObstacle direP: $destiny direS: $direction yo: $yo ".\r
+ print "begin: $wallBegin end: $wallEnd ".\r
+ \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 dist.\r
+ }\r
+ \r
+\r
+ value = -1.\r
+ return value.\r
+\r
+\r
+}\r
+
\ No newline at end of file
projects / z.facultad / 75.68 / celdas.git / commitdiff