+++ /dev/null
-@use PhysicalControl.\r
-@use Shape.\r
-@use Stationary.\r
-@use Link.\r
-@use MultiBody.\r
-@use Drawing.\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
- + 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
- \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 (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 with-velocity velocity (float): \r
- lWheel set-velocity to velocity.\r
- rWheel set-velocity to -velocity.\r
-\r
- + to turn-left with-velocity velocity (float):\r
-# vehicle rotate around-axis (0,1,0) by 1. \r
- lWheel set-velocity to -velocity. \r
- rWheel set-velocity to velocity.\r
-\r
- + to get-sensor-value:\r
- return (fSensor get-sensor-value).\r
-\r
- +to init:\r
- fSensor = (self add-sensor at (2.0, .4, 0)). \r
- fSensor set-direction to (1,0,0).\r
- fSensor set-id at 1.\r
- bSensor = (self add-sensor at (-2.0, .4, 0)).\r
- bSensor set-direction to (-1,0,0).\r
- bSensor set-id at 2.\r
- #bSensor set-sensor-angle to (-1.6).\r
- #lSensor = (self add-sensor at (1.0, .4, 1.4)).\r
- #lSensor set-direction to (0,0,1).\r
- #rSensor = (self add-sensor at (1.0, .4, -1.4)).\r
- #rSensor set-direction to (0,0,-1).\r
- lWheel = (self add-wheel at (0, 0, -1.5)).\r
- rWheel = (self add-wheel at (0, 0, 1.5)).\r
- \r
- +to iterate: \r
- #+ to post-iterate:\r
- valuef,valueb,valuer,valuel (float).\r
- fl, fr(float).\r
- \r
- valuef=fSensor get-data.\r
- valueb=bSensor get-data.\r
- #valuel=lSensor get-data.\r
- #valuer=rSensor get-data.\r
- \r
- #value = sensor get-data.\r
- #value = self get-sensor-value.\r
- #valueb = sensor2 get-sensor-value.\r
-\r
- if valuef >7: \r
- self set-global-velocity to (15).\r
- else if (valuef <=7) && (valuef > 0):\r
- { \r
- self set-global-velocity to (0).\r
- #self turn-left with-velocity(2).\r
- #self turn-right with-velocity(2).\r
- #self set-global-velocity to (0).\r
- }\r
- #print "sensor valuef: $valuef valueb: $valueb".\r
- \r
- #else if value < 0.1: self turn-left with-velocity CELDAS_MAX_TURN_VELOCITY.\r
- #else if value > 10: self set-global-velocity to ((self get-global-velocity) - 1).\r
-\r
- #fl = (flWheel get-velocity).\r
- #fr = (frWheel get-velocity).\r
- #print " sensorf: $value sensorb $valueb, fr: $fr, fl: $fl". \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
-\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=10.\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
-\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
- 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-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
- strength, angle (float).\r
- toObstacle, transDir (vector).\r
- largeWall (float).\r
- source,destiny (vector).\r
- obsLoc (vector). \r
- location (vector).\r
- posObstacle,posSensor (vector).\r
- angulo(double).\r
- des(int).\r
- \r
- transDir = (self get-rotation) * direction.\r
- \r
- value = 0.0.\r
- foreach i in (all CeldasObstacles): { \r
- \r
- posObstacle=i get-location.\r
- posObstacle::z = (self get-location)::z -posObstacle::z. \r
- \r
- #print " posObstacle: $posObstacle". \r
- \r
- toObstacle = (posObstacle) - (self get-location).\r
- \r
- angle = angle(toObstacle, transDir).\r
- #angle = dot((1,1,1),direction)*angle(toObstacle, (0,0,1)).\r
- \r
- #print "toObstacle: $toObstacle, angle: $angle".\r
- \r
- posSensor=self get-location.\r
- source = self get-location.\r
- obsLoc = posObstacle.\r
- destiny = obsLoc. \r
- \r
- draw clear. \r
- \r
- if(dot(direction,(1,1,1))<0): \r
- des = ((dot((self get-location),positiveDirection))>(dot(posObstacle,positiveDirection))). \r
- else\r
- des = ((dot((self get-location),positiveDirection))<(dot(posObstacle,positiveDirection))). \r
- \r
- if (angle < sensorAngle) && (des): {\r
- #if angle < sensorAngle: {\r
- \r
- strength = | (self get-location) - (i get-location) |.\r
- largeWall=i get-large.\r
- \r
- print "id: $id, distancia= $toObstacle, angle $angle".\r
- \r
- draw set-color to (1, 0, 0).\r
- draw draw-line from source to posObstacle.\r
- \r
- if |toObstacle::z|<=largeWall/2:{\r
- value = |toObstacle::x|.\r
- }\r
- else{ \r
- value = -1.\r
- }\r
- return value. \r
- }\r
- }\r
- value = -1.\r
- return value.\r
-\r
-\r
-}\r
+++ /dev/null
-@use PhysicalControl.\r
-@use Shape.\r
-@use Stationary.\r
-@use Link.\r
-@use MultiBody.\r
-@use Drawing.\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
- \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 (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 with-velocity velocity (float): \r
- lWheel set-velocity to velocity.\r
- rWheel set-velocity to -velocity.\r
-\r
- + to turn-left with-velocity velocity (float):\r
-# vehicle rotate around-axis (0,1,0) by 1. \r
- lWheel set-velocity to -velocity. \r
- rWheel set-velocity to velocity.\r
-\r
- + to get-sensor-value:\r
- return (fSensor get-sensor-value).\r
-\r
- +to init:\r
- fSensor = (self add-sensor at (2.0, .4, 0)). \r
- fSensor set-direction to (1,0,0).\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-id at 2.\r
- bSensor set-body at self.\r
- #bSensor set-sensor-angle to (-1.6).\r
- lSensor = (self add-sensor at (1.0, .4, 1.4)).\r
- lSensor set-direction to (0,0,1).\r
- bSensor set-id at 3.\r
- bSensor set-body at self.\r
-\r
- #rSensor = (self add-sensor at (1.0, .4, -1.4)).\r
- #rSensor set-direction to (0,0,-1).\r
- lWheel = (self add-wheel at (0, 0, -1.5)).\r
- rWheel = (self add-wheel at (0, 0, 1.5)).\r
- \r
- +to iterate: \r
- #+ to post-iterate:\r
- valuef,valueb,valuer,valuel (float).\r
- fl, fr(float).\r
- \r
- valuef=fSensor get-data.\r
- valueb=bSensor get-data.\r
- #valuel=lSensor get-data.\r
- #valuer=rSensor get-data.\r
- \r
- #value = sensor get-data.\r
- #value = self get-sensor-value.\r
- #valueb = sensor2 get-sensor-value.\r
-\r
- if valuef >7: \r
- self set-global-velocity to (15).\r
- else if (valuef <=7) && (valuef > 0):\r
- { \r
- self set-global-velocity to (0).\r
- #self turn-left with-velocity(2).\r
- #self turn-right with-velocity(2).\r
- #self set-global-velocity to (0).\r
- }\r
- #print "sensor valuef: $valuef valueb: $valueb".\r
- \r
- #else if value < 0.1: self turn-left with-velocity CELDAS_MAX_TURN_VELOCITY.\r
- #else if value > 10: self set-global-velocity to ((self get-global-velocity) - 1).\r
-\r
- #fl = (flWheel get-velocity).\r
- #fr = (frWheel get-velocity).\r
- #print " sensorf: $value sensorb $valueb, fr: $fr, fl: $fl". \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
-\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=10.\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
-\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
- x,y,z (float).\r
- v(vector).\r
- aux(float).\r
-\r
- \r
- strength, angle (float).\r
- toObstacle, transDir (vector).\r
- largeWall (float).\r
- source,destiny (vector).\r
- obsLoc (vector). \r
- location (vector).\r
- posObstacle,posSensor (vector).\r
- angulo(double).\r
- des(int).\r
- \r
- transDir = (self get-rotation) * direction.\r
- \r
- value = 0.0.\r
- foreach i in (all CeldasObstacles): { \r
- \r
- posObstacle=i get-location.\r
- print "$i".\r
- \r
- \r
- \r
- if(dot(direction,(1,1,1))<0): \r
- des = ((dot((self get-location),positiveDirection))>(dot(posObstacle,positiveDirection))). \r
- else\r
- des = ((dot((self get-location),positiveDirection))<(dot(posObstacle,positiveDirection))). \r
- \r
-\r
- if (des):\r
- {\r
- #posObstacle::z = (self get-location)::z -posObstacle::z. \r
- \r
- #print " posObstacle: $posObstacle". \r
- \r
- angle = angle(toObstacle, transDir). \r
- posSensor=self get-location.\r
- source = self get-location.\r
- #obsLoc = posObstacle. \r
- \r
- #v = (self get-rotation) * (direction).\r
- v = (body get-location) - (self get-location ).\r
- #v = (direction).\r
- #aux=(self get-rotation).\r
- #print "rot: $aux".\r
- aux=v::z/v::x.\r
- print "m: $aux".\r
- \r
- obsLoc::z=z=((self get-location)::z + ((posObstacle::x - (self get-location)::x)*v::z/v::x)).\r
- obsLoc::y=y=posObstacle::y.\r
- obsLoc::x=x=posObstacle::x.\r
- \r
- destiny = obsLoc. \r
- print "v:$direction x,y,z: $x,$y,$z". \r
- #print "v: $v". \r
- draw clear. \r
- \r
- draw set-color to (1, 0, 0).\r
- draw draw-line from source to obsLoc.\r
- }\r
- \r
- }\r
- value = -1.\r
- return value.\r
-\r
-\r
-}\r
+++ /dev/null
-@use PhysicalControl.\r
-@use Shape.\r
-@use Stationary.\r
-@use Link.\r
-@use MultiBody.\r
-@use Drawing.\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
- \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 (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 with-velocity velocity (float): \r
- lWheel set-velocity to velocity.\r
- rWheel set-velocity to -velocity.\r
-\r
- + to turn-left with-velocity velocity (float):\r
-# vehicle rotate around-axis (0,1,0) by 1. \r
- lWheel set-velocity to -velocity. \r
- rWheel set-velocity to velocity.\r
-\r
- + to get-sensor-value:\r
- return (fSensor get-sensor-value).\r
-\r
- +to init:\r
- fSensor = (self add-sensor at (2.0, .4, 0)). \r
- fSensor set-direction to (1,0,0).\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-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-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-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
- +to iterate: \r
- #+ to post-iterate:\r
- valuef,valueb,valuer,valuel (float).\r
- fl, fr(float).\r
- \r
- valuef=fSensor get-data.\r
- valueb=bSensor get-data.\r
- valuel=lSensor get-data.\r
- valuer=rSensor get-data.\r
-\r
- self turn-left with-velocity(20).\r
- self set-global-velocity to (15).\r
- if valuef >7: \r
- self set-global-velocity to (15).\r
- else if (valuef <=7) && (valuef > 0):\r
- { \r
- self set-global-velocity to (0).\r
- #self turn-left with-velocity(2).\r
- #self turn-right with-velocity(2).\r
- #self set-global-velocity to (0).\r
- }\r
- #print "sensor valuef: $valuef valueb: $valueb".\r
- \r
- #else if value < 0.1: self turn-left with-velocity CELDAS_MAX_TURN_VELOCITY.\r
- #else if value > 10: self set-global-velocity to ((self get-global-velocity) - 1).\r
-\r
- #fl = (flWheel get-velocity).\r
- #fr = (frWheel get-velocity).\r
- #print " sensorf: $value sensorb $valueb, fr: $fr, fl: $fl". \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
- x,y,z (float).\r
- min,dist (float).\r
- v,obs(vector).\r
- aux(float).\r
- j (int).\r
- des1,des2,des3(int).\r
- wallBegin,wallEnd (float).\r
- \r
- aux1,aux2,aux3,aux4 (float).\r
- yo,toObstacle, transDir (vector).\r
- largeWall (float).\r
- source,destiny (vector).\r
- obsLoc (vector). \r
- location (vector).\r
- posObstacle,posSensor (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
- \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(posObstacle,positiveDirection))):\r
- des2=1. \r
- }\r
- else\r
- {\r
- if((dot((self get-location),positiveDirection))<(dot(posObstacle,positiveDirection))):\r
- des2=1. \r
- } \r
-\r
-\r
- #Compruebo que el robot este frente a la pared\r
- wallBegin=dot((i get-location),(i get-direction) )- (i get-large)/2.\r
- wallEnd=dot((i get-location),(i get-direction) )+ (i get-large)/2. \r
-\r
-\r
- #print "begin: $wallBegin end: $wallEnd".\r
- yo=self get-location.\r
- destiny=i get-direction.\r
-\r
- v = (body get-location) - (self get-location ).\r
- obsLoc::y=y=posObstacle::y. \r
- \r
- if (dot((i get-direction),(1,0,0))):\r
- {\r
- obsLoc::x=x=((self get-location)::x + ((posObstacle::z - (self get-location)::z)*v::x/v::z)).\r
- obsLoc::z=z=posObstacle::z.\r
- } \r
- else\r
- {\r
- obsLoc::z=z=((self get-location)::z + ((posObstacle::x - (self get-location)::x)*v::z/v::x)).\r
- obsLoc::x=x=posObstacle::x.\r
- } \r
-\r
-\r
- if (dot((obsLoc),(i get-direction)) > wallBegin) && (dot((obsLoc),(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 "dist: $aux1 begin: $wallBegin end: $wallEnd".\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
- dist=|obsLoc - (self get-location)|.\r
- if( (j==0) || (min>dist) ):\r
- {\r
- min=dist.\r
- obs=obsLoc.\r
- j++. \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
+++ /dev/null
-@use PhysicalControl.\r
-@use Shape.\r
-@use Stationary.\r
-@use Link.\r
-@use MultiBody.\r
-@use Drawing.\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
- lfWheel,rfWheel,lbWheel,rbWheel (object).\r
- tleft,tright (int). \r
- \r
- avanzando,retrocediendo,girando(int). \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 (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
-\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
- 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
- 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. #Debe ser inicializado en 0 esta asi para probar!!!!!!!!!!!!!!!!!!!!!!!!\r
- avanzando=1.\r
- retrocediendo=0.\r
- girando=0. \r
- \r
- +to iterate:\r
- valuef,valueb,valuer,valuel (float).\r
- fl, fr(float).\r
- \r
- valuef=fSensor get-data.\r
- valueb=bSensor get-data.\r
- valuel=lSensor get-data.\r
- valuer=rSensor get-data.\r
- \r
- print "senforl: $valuel".\r
-\r
- if(avanzando):\r
- if(valuef>15):\r
- self set-global-velocity to (15).\r
- else \r
- { \r
- avanzando=0.\r
- retrocediendo=1.\r
- } \r
-\r
-\r
- if(retrocediendo):\r
- if(valueb>15):\r
- self set-global-velocity to (-15).\r
- else \r
- { \r
- tleft=1.\r
- retrocediendo=0.\r
- }\r
-\r
-\r
- if((tleft) && (valuel>10)): \r
- {\r
- #self set-global-velocity to (0).\r
- self turn-left.\r
- }\r
- else\r
- {\r
- avanzando=1.\r
- #tright=1.\r
- }\r
-\r
-#!\r
- if((tright) && (valuer>10)): \r
- {\r
- #self set-global-velocity to (0).\r
- self turn-right.\r
- }\r
- else\r
- {\r
- avanzando=1.\r
- #tright=1.\r
- } \r
-!# \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
- if ((des2) && (des3)):\r
- { \r
- draw clear. \r
-\r
- dist=|obsLoc - (self get-location)|.\r
-\r
- if( (j==0) || (min>dist) ):\r
- {\r
- min=dist.\r
- obs=obsLoc.\r
- \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
-
\ No newline at end of file
+++ /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
-@define CELDAS_TURNO 100.\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
- 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
- \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 (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
-\r
- self rotate around-axis (0,1,0) by (-1.5709/CELDAS_TURNO)*tright. \r
- \r
- if (tright == CELDAS_TURNO): tright=0.\r
-\r
-\r
- + to turn-left:\r
- tleft++.\r
-\r
- self rotate around-axis (0,1,0) by (1.5709/CELDAS_TURNO)*tleft. \r
- \r
- if (tleft == CELDAS_TURNO): tleft=0.\r
-\r
-\r
- + to get-sensor-value:\r
- return (fSensor get-sensor-value).\r
-\r
- +to update-entorno:\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
- entorno{"movido"} = 0. # TODO\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
- # Configuracion de sistema autonomo\r
- sa = new SistemaAutonomo.\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".\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".\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
- fl, fr(float).\r
-\r
- if (0): # TODO posicion_final == posicion_actual\r
- {\r
- print "Llegamos al FINAL!!!".\r
- return.\r
- }\r
-\r
- if (plan_finished):\r
- {\r
- self update-entorno.\r
- sa plan. # Si no tenemos plan, lo hacemos\r
- plan_finished = 0.\r
- # TODO posicion_inicial = posicion_actual\r
- if (! sa-has-next-theory):\r
- {\r
- print "El planificador no encuentra PLAN!!!".\r
- return.\r
- }\r
- }\r
-\r
- if (iterate == 0):\r
- {\r
- print "iteracion 0".\r
- if (sa has-next-theory):\r
- {\r
- print "hay teoria".\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
- }\r
-\r
- if (iterate == CELDAS_TURNO):\r
- {\r
- self update-entorno.\r
- # TODO if (posicion_actual == posicion_inicial): movido = false. else movido = true.\r
- if (sa validate theory teoria):\r
- {\r
- print "valida".\r
- }\r
- else\r
- {\r
- print "Teoria no valida".\r
- plan_finished = 1.\r
- }\r
- }\r
-\r
- iterate++.\r
- if (iterate == CELDAS_TURNO + 1):\r
- iterate = 0.\r
-\r
- # Movimiento del robot\r
- if (avanzando):\r
- self set-global-velocity to (15).\r
- if (retrocediendo):\r
- self set-global-velocity to (-15).\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
- \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
- 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
+++ /dev/null
-
-@use Celdas-2-2.
-\r
-@define CELDAS_MAX_TURN_VELOCITY 2.\r
-
-Controller DemoController.
-
-CeldasControl : DemoController {
- + variables:
- vehicle (object).
- n (int).
-
- + to init:
-
- new CeldasObstacle init at-location (0,1,0). \r
- new CeldasObstacle init at-location (-25,1,0).
-
- vehicle = new CeldasVehicle.
- self watch item vehicle.
-
- vehicle move to (-20, 0.8, -3).
-
- vehicle set-global-velocity to 15.0.
-}
+++ /dev/null
-
-@use Celdas-2-3.
-\r
-@define CELDAS_MAX_TURN_VELOCITY 2.\r
-
-Controller DemoController.
-
-CeldasControl : DemoController {
- + variables:
- vehicle (object).
- n (int).
-
- + to init:
-
- new CeldasObstacle init at-location (0,1,0). \r
- new CeldasObstacle init at-location (-25,1,0).
-
- vehicle = new CeldasVehicle.
- self watch item vehicle.
-
- vehicle move to (-20, 0.8, -3).
-
- vehicle set-global-velocity to 15.0.
-}
+++ /dev/null
-
-@use Celdas-2-6.
-\r
-@define CELDAS_MAX_TURN_VELOCITY 2.\r
-
-Controller DemoController.
-
-CeldasControl : DemoController {
- + variables:
- vehicle (object).
- n (int).
-
- + to init: \r
- obs(object).\r
-\r
-\r
- vehicle = new CeldasVehicle.
- self watch item vehicle.
-
- vehicle move to (-20, 0.8, -3).
-
- vehicle set-global-velocity to 15.0.
-}
+++ /dev/null
-@use Control.\r
-@use Stationary.\r
-@use Celdas-2-7.\r
-\r
-@define altoPared 5.\r
-@define posYPared 25.\r
-@define seccion 20.\r
-@define distanciaTotalX 240.\r
-@define distanciaTotalZ 240.\r
-\r
-Controller myControl.\r
-\r
-\r
-CeldasObstacle : Wall{\r
-\r
-\r
- +to Create to-posX posX = 0 (float) to-posY posY = 2.5 (float) to-posZ posZ = 0 (float) to-widthX widthX = 5(float) to-widthZ widthZ = 5(float):\r
- \r
- if(widthX>widthZ):\r
- {\r
- large=widthX.\r
- direction=(1,0,0).\r
- }\r
- else \r
- {\r
- large=widthZ.\r
- direction=(0,0,1).\r
- } \r
- self register with-shape (new Cube init-with size (widthX,altoPared,widthZ)) at-location (posX,posY,posZ).\r
- self set-color to (0,1,0).\r
-\r
-\r
-}\r
-\r
-\r
-CeldasObstacle : Labyrinth{\r
-\r
- +to init:\r
- wall(object).\r
- \r
- wall=new Wall.\r
- #Contorno del laberinto\r
- wall Create to-widthX 11.25*seccion.\r
- wall=new Wall.\r
- wall Create to-posX -5.5*seccion to-widthX 5 to-widthZ 11.25*seccion to-posZ 5.5*seccion .\r
- wall=new Wall.\r
- wall Create to-posX -5.5*seccion to-widthX 5 to-widthZ 11.25*seccion to-posZ 5.5*seccion to-posX 5.5*seccion .\r
- wall=new Wall.\r
- wall Create to-widthX 5.25*seccion to-posZ 11*seccion to-posX -3*seccion.\r
- wall=new Wall.\r
- wall Create to-widthX 5.25*seccion to-posZ 11*seccion to-posX 3*seccion.\r
- #fin contorno\r
- \r
- #Paredesd verticales (en el diagrama)\r
- wall=new Wall.\r
- wall Create to-widthZ seccion to-widthX 5 to-posZ 10.5*seccion to-posX 0.5*seccion. \r
- wall=new Wall.\r
- wall Create to-widthZ 2*seccion to-widthX 5 to-posZ 10*seccion to-posX -1.5*seccion.\r
- wall=new Wall.\r
- wall Create to-widthZ seccion to-widthX 5 to-posZ 10.5*seccion to-posX -3.5*seccion.\r
- \r
- wall=new Wall.\r
- wall Create to-widthZ 2*seccion to-widthX 5 to-posZ 9*seccion to-posX -0.5*seccion.\r
- wall=new Wall.\r
- wall Create to-widthZ seccion to-widthX 5 to-posZ 9.5*seccion to-posX -4.5*seccion.\r
- wall=new Wall.\r
- wall Create to-widthZ 3*seccion to-widthX 5 to-posZ 8.5*seccion to-posX -2.5*seccion.\r
- wall=new Wall.\r
- wall Create to-widthZ 3*seccion to-widthX 5 to-posZ 8.5*seccion to-posX 3.5*seccion.\r
- \r
- wall=new Wall. \r
- wall Create to-widthZ 5*seccion to-widthX 5 to-posZ 5.5*seccion to-posX -4.5*seccion.\r
- wall=new Wall.\r
- wall Create to-widthZ 6*seccion to-widthX 5 to-posZ 4*seccion to-posX -3.5*seccion.\r
- wall=new Wall.\r
- wall Create to-widthZ 3*seccion to-widthX 5 to-posZ 4.5*seccion to-posX -2.5*seccion.\r
- wall=new Wall.\r
- wall Create to-widthZ 3*seccion to-widthX 5 to-posZ 5.5*seccion to-posX -1.5*seccion.\r
- \r
- wall=new Wall.\r
- wall Create to-widthZ seccion to-widthX 5 to-posZ 2.5*seccion to-posX 0.5*seccion.\r
- wall=new Wall.\r
- wall Create to-widthZ seccion to-widthX 5 to-posZ 1.5*seccion to-posX 1.5*seccion.\r
- wall=new Wall.\r
- wall Create to-widthZ seccion to-widthX 5 to-posZ 4.5*seccion to-posX 1.5*seccion.\r
- wall=new Wall.\r
- wall Create to-widthZ seccion to-widthX 5 to-posZ 6.5*seccion to-posX 1.5*seccion.\r
- wall=new Wall.\r
- wall Create to-widthZ seccion to-widthX 5 to-posZ 2.5*seccion to-posX -1.5*seccion. \r
-\r
- wall=new Wall.\r
- wall Create to-widthZ seccion to-widthX 5 to-posZ 1.5*seccion to-posX -2.5*seccion.\r
-\r
- wall=new Wall.\r
- wall Create to-widthZ seccion to-widthX 5 to-posZ 3.5*seccion to-posX -0.5*seccion.\r
- wall=new Wall.\r
- wall Create to-widthZ seccion to-widthX 5 to-posZ 3.5*seccion to-posX 2.5*seccion.\r
-\r
- wall=new Wall.\r
- wall Create to-widthZ 2*seccion to-widthX 5 to-posZ 7*seccion to-posX 2.5*seccion.\r
-\r
- wall=new Wall.\r
- wall Create to-widthZ seccion to-widthX 5 to-posZ 4.5*seccion to-posX 3.5*seccion.\r
-\r
- wall=new Wall.\r
- wall Create to-widthZ 6*seccion to-widthX 5 to-posZ 6*seccion to-posX 4.5*seccion. \r
-\r
- wall=new Wall.\r
- #Paredes horizontales (en el diagrama)\r
- wall Create to-widthZ 5 to-widthX 2.25*seccion to-posZ 10*seccion to-posX 1.5*seccion. #1\r
-\r
- wall=new Wall.\r
- wall Create to-widthZ 5 to-widthX 2.25*seccion to-posZ 10*seccion to-posX 4.5*seccion. #2\r
-\r
- \r
- wall=new Wall.\r
- wall Create to-widthZ 5 to-widthX 2.25*seccion to-posZ 9*seccion to-posX -3.5*seccion. #3\r
-\r
- wall=new Wall.\r
- wall Create to-widthZ 5 to-widthX 1.25*seccion to-posZ 8*seccion to-posX -4*seccion. #4\r
-\r
- wall=new Wall.\r
- wall Create to-widthZ 5 to-widthX 5.25*seccion to-posZ 8*seccion to-posX 0. #5\r
-\r
- \r
- wall=new Wall.\r
- wall Create to-widthZ 5 to-widthX 3*seccion to-posZ 9*seccion to-posX 2*seccion. #6\r
-\r
- \r
- wall=new Wall.\r
- wall Create to-widthZ 5 to-widthX 1.25*seccion to-posZ 7*seccion to-posX -3*seccion. #7\r
-\r
- wall=new Wall.\r
- wall Create to-widthZ 5 to-widthX 3.25*seccion to-posZ 7*seccion to-posX 0. #8\r
-\r
- wall=new Wall.\r
- wall Create to-widthZ 5 to-widthX 2.25*seccion to-posZ 6*seccion to-posX 3.5*seccion. #9\r
-\r
- wall=new Wall.\r
- wall Create to-widthZ 5 to-widthX 1.25*seccion to-posZ 6*seccion to-posX -2*seccion. #10 \r
-\r
- wall=new Wall.\r
- wall Create to-widthZ 5 to-widthX 3.25*seccion to-posZ 4*seccion to-posX 0. #11\r
-\r
- wall=new Wall.\r
- wall Create to-widthZ 5 to-widthX 2.25*seccion to-posZ 5*seccion to-posX 2.5*seccion. #12\r
-\r
-\r
- wall=new Wall. \r
- wall Create to-widthZ 5 to-widthX 1.25*seccion to-posZ 3*seccion to-posX -4*seccion. #13\r
-\r
- wall=new Wall.\r
- wall Create to-widthZ 5 to-widthX 1.25*seccion to-posZ 3*seccion to-posX -2*seccion. #14\r
-\r
- wall=new Wall.\r
- wall Create to-widthZ 5 to-widthX 4.25*seccion to-posZ 3*seccion to-posX 2.5*seccion. #15\r
-\r
- #wall Create to-widthZ 5 to-widthX 4*seccion to-posZ 3*seccion to-posX 2.5*seccion. #15\r
-\r
- wall=new Wall. \r
- wall Create to-widthZ 5 to-widthX seccion to-posZ 2*seccion to-posX -5*seccion. #16\r
- \r
- wall=new Wall.\r
- wall Create to-widthZ 5 to-widthX 2.25*seccion to-posZ 2*seccion to-posX -0.5*seccion. #17\r
- \r
- wall=new Wall.\r
- wall Create to-widthZ 5 to-widthX 4.25*seccion to-posZ 2*seccion to-posX 3.5*seccion. #18\r
- \r
- wall=new Wall.\r
- wall Create to-widthZ 5 to-widthX 9*seccion to-posZ 1*seccion to-posX 0. #19\r
-\r
-}\r
-\r
-\r
-Control: myControl{\r
-+ variables:\r
- agent(object).\r
- pared(object).\r
- labe(object). \r
- \r
- sensor (object).\r
- vehicle (object).\r
- \r
-+ to init:\r
- self point-camera at (0,0,0) from (200,200,200).\r
- new Floor.\r
-\r
- labe = new Labyrinth.\r
- vehicle = new CeldasVehicle.\r
- self watch item vehicle.\r
-\r
- vehicle move to (0, 0, 10).\r
-\r
-+to iterate:\r
- super iterate.\r
-\r
-}\r
-\r
projects / z.facultad / 75.68 / celdas.git / commitdiff