@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-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-id at 2. bSensor set-body at self. #bSensor set-sensor-angle to (-1.6). lSensor = (self add-sensor at (1.0, .4, 1.4)). lSensor set-direction to (0,0,1). bSensor set-id at 3. bSensor set-body at self. #rSensor = (self add-sensor at (1.0, .4, -1.4)). #rSensor set-direction to (0,0,-1). 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. #value = sensor get-data. #value = self get-sensor-value. #valueb = sensor2 get-sensor-value. 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). + 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=10. + 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. } 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). x,y,z (float). v(vector). aux(float). strength, angle (float). toObstacle, transDir (vector). largeWall (float). source,destiny (vector). obsLoc (vector). location (vector). posObstacle,posSensor (vector). angulo(double). des(int). transDir = (self get-rotation) * direction. value = 0.0. foreach i in (all CeldasObstacles): { posObstacle=i get-location. print "$i". if(dot(direction,(1,1,1))<0): des = ((dot((self get-location),positiveDirection))>(dot(posObstacle,positiveDirection))). else des = ((dot((self get-location),positiveDirection))<(dot(posObstacle,positiveDirection))). if (des): { #posObstacle::z = (self get-location)::z -posObstacle::z. #print " posObstacle: $posObstacle". angle = angle(toObstacle, transDir). posSensor=self get-location. source = self get-location. #obsLoc = posObstacle. #v = (self get-rotation) * (direction). v = (body get-location) - (self get-location ). #v = (direction). #aux=(self get-rotation). #print "rot: $aux". aux=v::z/v::x. print "m: $aux". obsLoc::z=z=((self get-location)::z + ((posObstacle::x - (self get-location)::x)*v::z/v::x)). obsLoc::y=y=posObstacle::y. obsLoc::x=x=posObstacle::x. destiny = obsLoc. print "v:$direction x,y,z: $x,$y,$z". #print "v: $v". draw clear. draw set-color to (1, 0, 0). draw draw-line from source to obsLoc. } } value = -1. return value. }