@use PhysicalControl. @use Shape. @use Stationary. @use Link. @use MultiBody. @use Drawing. @define CELDAS_MAX_VELOCITY 30. PhysicalControl : CeldasControl { % This class is used for building simple vehicle % simulations. To create a vehicle simulation, % subclass CeldasControl and use the init method to % create OBJECT(CeldasObstacle) and % OBJECT(CeldasVehicle) objects. + variables: floor (object). floorShape (object). cloudTexture (object). + to init: self enable-lighting. #self enable-smooth-drawing. floorShape = new Shape. floorShape init-with-cube size (200, .2, 200). floor = new Stationary. floor register with-shape floorShape at-location (0, 0, 0). #floor catch-shadows. self point-camera at (0, 0, 0) from (3, 3, 24). #self enable-shadows. #self enable-reflections. cloudTexture = (new Image load from "images/clouds.png"). self set-background-color to (.4, .6, .9). self set-background-texture-image to cloudTexture. } MultiBody : CeldasLightVehicle (aka CeldasLightVehicles) { % This object is used in conjunction with OBJECT(CeldasControl) to % create simple vehicles. + variables: bodyShape (object). wheelShape (object). sensorShape (object). bodyLink (object). wheels (list). sensors (list). + to init: bodyShape = new Shape. bodyShape init-with-cube size (4.0, .75, 3.0). wheelShape = new Shape. wheelShape init-with-polygon-disk radius ( self get-wheel-radius ) sides 20 height ( self get-wheel-width ). # 40 sensorShape = new Shape. sensorShape init-with-polygon-cone radius .2 sides 5 height .5. # 10 bodyShape set-density to ( self get-density ). bodyLink = new Link. bodyLink set-shape to bodyShape. bodyLink set-mu to -1.0. bodyLink set-eT to .8. self set-root to bodyLink. self move to (0, 0.9, 0). self set-texture-scale to 1.5. - to get-density: return 1.0. - to get-wheel-width: return 0.1. - to get-wheel-radius: return 0.6. + section "Adding Wheels and Sensors to a Vehicle" + to add-wheel at location (vector): % Adds a wheel at location on the vehicle. This method returns % the wheel which is created, a OBJECT(CeldasWheel). wheel, joint (object). wheel = new CeldasWheel. wheel set-shape to wheelShape. joint = new RevoluteJoint. joint set-relative-rotation around-axis (1, 0, 0) by 1.5708. joint link parent bodyLink to-child wheel with-normal (0, 0, 1) with-parent-point location with-child-point (0, 0, 0). wheel set-eT to .8. wheel set-texture to 0. wheel set-joint to joint. joint set-strength-limit to (joint get-strength-hard-limit) / 2. wheel set-color to (.6, .6, .6). wheel set-mu to 100000. self add-dependency on joint. self add-dependency on wheel. push wheel onto wheels. return wheel. + to add-sensor at location (vector) with-direction direction = (0,1,0)(vector) : % Adds a sensor at location on the vehicle. This method returns % the sensor which is created, a OBJECT(CeldasSensor). sensor, joint (object). sensor = new CeldasSensor. sensor set-direction to direction. sensor set-shape to sensorShape. joint = new RevoluteJoint. joint set-relative-rotation around-axis (0, 0, 1) by -1.57. joint link parent bodyLink to-child sensor with-normal (1, 0, 0) with-parent-point location with-child-point (0, 0, 0). joint set-double-spring with-strength 300 with-max 0.01 with-min -0.01. self add-dependency on joint. self add-dependency on sensor. sensor set-color to (0, 0, 0). #push sensor onto sensors. return sensor. + to destroy: free sensorShape. free wheelShape. free bodyShape. super destroy. } CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) { % A heavy duty version of OBJECT(CeldasLightVehicle), this % vehicle is heavier and harder to control, but more stable % at higher speeds. +variables: lSensor, rSensor, fSensor, bSensor (object). lWheel,rWheel (object). - to get-density: return 20.0. - to get-wheel-width: return 0.4. - to get-wheel-radius: return 0.8. + to set-global-velocity to velocity (float): rWheel set-velocity to velocity. lWheel set-velocity to velocity. + to get-global-velocity: return ((rWheel get-velocity) + (lWheel get-velocity)) / 2. + to turn-right with-velocity velocity (float): lWheel set-velocity to velocity. rWheel set-velocity to -velocity. + to turn-left with-velocity velocity (float): # vehicle rotate around-axis (0,1,0) by 1. lWheel set-velocity to -velocity. rWheel set-velocity to velocity. + to get-sensor-value: return (fSensor get-sensor-value). +to init: fSensor = (self add-sensor at (2.0, .4, 0)). fSensor set-direction to (1,0,0). #fSensor set-direction to (0,0,1). fSensor set-id at 1. fSensor set-body at self. bSensor = (self add-sensor at (-2.0, .4, 0)). bSensor set-direction to (-1,0,0). #bSensor set-direction to (0,0,1). bSensor set-id at 2. bSensor set-body at self. lSensor = (self add-sensor at (0, .4, 1.5)). lSensor set-direction to (0,0,1). #lSensor set-direction to (1,0,0). lSensor set-id at 3. lSensor set-body at self. rSensor = (self add-sensor at (0, .4, -1.5)). rSensor set-direction to (0,0,-1). #rSensor set-direction to (-1,0,0). rSensor set-id at 4. rSensor set-body at self. lWheel = (self add-wheel at (0, 0, -1.5)). rWheel = (self add-wheel at (0, 0, 1.5)). +to iterate: #+ to post-iterate: valuef,valueb,valuer,valuel (float). fl, fr(float). valuef=fSensor get-data. valueb=bSensor get-data. valuel=lSensor get-data. valuer=rSensor get-data. self turn-left with-velocity(20). self set-global-velocity to (15). if valuef >7: self set-global-velocity to (15). else if (valuef <=7) && (valuef > 0): { self set-global-velocity to (0). #self turn-left with-velocity(2). #self turn-right with-velocity(2). #self set-global-velocity to (0). } #print "sensor valuef: $valuef valueb: $valueb". #else if value < 0.1: self turn-left with-velocity CELDAS_MAX_TURN_VELOCITY. #else if value > 10: self set-global-velocity to ((self get-global-velocity) - 1). #fl = (flWheel get-velocity). #fr = (frWheel get-velocity). #print " sensorf: $value sensorb $valueb, fr: $fr, fl: $fl". } Stationary : CeldasObstacle (aka CeldasObstacles) { % A CeldasObstacle is used in conjunction with OBJECT(CeldasControl) % and OBJECT(CeldasVehicle). It is what the OBJECT(CeldasSensor) % objects on the CeldasVehicle detect. %

% There are no special behaviors associated with the walls--they're % basically just plain OBJECT(Stationary) objects. +variables: large (float). direction (vector). + to init with-size theSize = (10, 3, .1) (vector) with-color theColor = (1, 0, 0) (vector) at-location theLocation = (0, 0, 0) (vector) with-rotation theRotation = [ ( 0, 0, 1 ), ( 0, 1, 0 ), ( 1, 0, 0 ) ] (matrix): self init-with-shape shape (new Shape init-with-cube size theSize) color theColor at-location theLocation with-rotation theRotation. large=20. + to init-with-shape shape theShape (object) color theColor = (1, 0, 0) (vector) at-location theLocation = (0, 0, 0) (vector) with-rotation theRotation = [ ( 1, 0, 0 ), ( 0, 1, 0 ), ( 0, 0, 1 ) ] (matrix): self register with-shape theShape at-location theLocation with-rotation theRotation. self set-color to theColor. + to get-large: return large. + to set-direction at theDirection (vector): direction=theDirection. + to get-direction: return direction. } Link : CeldasWheel (aka CeldasWheels) { % A CeldasWheel is used in conjunction with OBJECT(CeldasVehicle) % to build Celdas vehicles. This class is typically not instantiated % manually, since OBJECT(CeldasVehicle) creates one for you when you % add a wheel to the vehicle. + variables: joint (object). velocity (float). + to init: velocity = 0. - to set-joint to j (object): % Used internally. joint = j. + section "Configuring the Wheel's Velocity" + to set-velocity to n (float): % Sets the velocity of this wheel. if n > CELDAS_MAX_VELOCITY: n = CELDAS_MAX_VELOCITY. velocity = n. joint set-joint-velocity to velocity. + to get-velocity: % Gets the velocity of this wheel. return velocity. } Link : CeldasSensor (aka CeldasSensors) { % A CeldasSensor is used in conjunction with OBJECT(CeldasVehicle) % to build Celdas vehicles. This class is typically not instantiated % manually, since OBJECT(CeldasVehicle) creates one for you when you % add a sensor to the vehicle. + variables: direction (vector). positiveDirection(vector). sensorAngle (float). value (float). draw (object). body(object). id(int). + to init : direction = (1,0,1). positiveDirection= (1,0,1). sensorAngle = 1.6. value = 0.0. draw = new Drawing. + section "Configuring the Sensor Values" + to set-id at n (int): id=n. + to set-body at robotBody(object): body=robotBody. + to set-sensor-angle to n (float): % Sets the angle in which this sensor can detect obstacles. The default % value of 1.6 means that the sensor can see most of everything in % front of it. Setting the value to be any higher leads to general % wackiness, so I don't suggest it. sensorAngle = n. + to set-direction to n (vector): direction = n. positiveDirection::x=|n::x|. positiveDirection::y=|n::y|. positiveDirection::z=|n::z|. + section "Getting the Sensor Values" + to get-sensor-value: % Gets the sensor value. This should be used from post-iterate, % if not, the sensor reading correspond to the previous % iteration. #+ to iterate: + to get-data: i (object). min,dist (float). v,obs(vector). aux(float). j (int). des1,des2,des3(int). wallBegin,wallEnd,wallCenter (float). aux1,aux2,aux3,aux4 (float). yo,toObstacle, transDir (vector). largeWall (float). source,destiny (vector). obsLoc (vector). location (vector). posObstacle,posSensor (vector). draw clear. value = 0.0. j=0. min=0. foreach i in (all CeldasObstacles): { posObstacle=i get-location. v = (body get-location) - (self get-location ). obsLoc::y=posObstacle::y. if (dot((i get-direction),(1,0,0))): { obsLoc::x=((self get-location)::x + ((posObstacle::z - (self get-location)::z)*v::x/v::z)). obsLoc::z=posObstacle::z. } else { obsLoc::z=((self get-location)::z + ((posObstacle::x - (self get-location)::x)*v::z/v::x)). obsLoc::x=posObstacle::x. } #! if(dot((i get-direction),direction)==0): des1=1. else des1=0. !# des2=0. if(dot(direction,(1,1,1))<0): { if((dot((self get-location),positiveDirection))>(dot(obsLoc,positiveDirection))): des2=1. } else { if((dot((self get-location),positiveDirection))<(dot(obsLoc,positiveDirection))): des2=1. } #Compruebo que el robot este frente a la pared wallCenter=dot((i get-location),(i get-direction)). wallBegin=wallCenter- (i get-large)/2. wallEnd=wallCenter + (i get-large)/2. yo=self get-location. destiny=i get-direction. if (dot((self get-location),(i get-direction)) > wallBegin) && (dot((self get-location),(i get-direction)) < wallEnd): des3=1. else { des3=0. } aux1=dot((self get-location),(i get-direction)). #print "sensor: $id obstaculo: $posObstacle direP: $destiny direS: $direction yo: $yo ". #print "dist: $aux1 begin: $wallBegin end: $wallEnd ". #print "begin: $wallBegin end: $wallEnd". if ((i get-location)::z==20): print "------>sensor: $id obstaculo: $posObstacle direP: $destiny direS: $direction yo: $yo des2: $des2 des1: $des1". #print "sensor: $id , des1: $des1, des2: $des2, des3: $des3". if ((des2) && (des3)): { draw clear. #print " posObstacle: $posObstacle". #print "sensor: $id obstaculo: $posObstacle direP: $destiny direS: $direction yo: $yo ". #print "dist: $aux1 begin: $wallBegin end: $wallEnd ". dist=|obsLoc - (self get-location)|. if( (j==0) || (min>dist) ): { min=dist. obs=obsLoc. j++. print "sensor: $id obstaculo: $posObstacle direP: $destiny direS: $direction yo: $yo ". } } } #end for if(j!=0): { #Dibujo el laser draw set-color to (1, 0, 0). draw draw-line from (self get-location) to (obs). return dist. } value = -1. return value. }