--- /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 (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
+ #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
+ 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
+ a,b (float).\r
+ v(vector).\r
+ aux(float).\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
+ angulo(double).\r
+ \r
+ \r
+ transDir = (self get-rotation) * direction.\r
+ draw clear.\r
+ value = 0.0.\r
+ foreach i in (all CeldasObstacles): { \r
+ \r
+ posObstacle=i get-location.\r
+ #print "$i".\r
+ \r
+ \r
+ if(dot((i get-direction),direction)==0):\r
+ des1=1.\r
+ else\r
+ des1=0.\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
+ #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
+ \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 "dist: $aux1 begin: $wallBegin end: $wallEnd".\r
+\r
+ v = (body get-location) - (self get-location ).\r
+\r
+ \r
+\r
+ print "sensor: $id , des1: $des1, des2: $des2, des3: $des3".\r
+ if ((des1) && (des2) && (des3)):\r
+ { \r
+ draw clear. \r
+ #print " posObstacle: $posObstacle". \r
+ \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
+ #Dibujo el laser\r
+ \r
+ draw set-color to (1, 0, 0).\r
+ draw draw-line from (self get-location) to obsLoc.\r
+ }\r
+ \r
+ }\r
+ value = -1.\r
+ return value.\r
+\r
+\r
+}\r
projects / z.facultad / 75.68 / celdas.git / commitdiff