X-Git-Url: https://git.llucax.com/z.facultad/75.68/celdas.git/blobdiff_plain/23a9bf3c98b449186144927de87462f33dd47587..586ad2d03cf73d7e75a1a2522db3e0c4eedd5e9c:/trunk/src/breve/robot/Celdas-2-6.tz diff --git a/trunk/src/breve/robot/Celdas-2-6.tz b/trunk/src/breve/robot/Celdas-2-6.tz index e0d352c..9756afe 100644 --- a/trunk/src/breve/robot/Celdas-2-6.tz +++ b/trunk/src/breve/robot/Celdas-2-6.tz @@ -160,8 +160,12 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) { % at higher speeds. +variables: lSensor, rSensor, fSensor, bSensor (object). - lWheel,rWheel (object). + lfWheel,rfWheel,lbWheel,rbWheel (object). + tleft,tright (int). + avanzando,retrocediendo,girando(int). + + - to get-density: return 20.0. @@ -172,20 +176,29 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) { return 0.8. + to set-global-velocity to velocity (float): - rWheel set-velocity to velocity. - lWheel set-velocity to velocity. + rfWheel set-velocity to velocity. + lfWheel set-velocity to velocity. + rbWheel set-velocity to velocity. + lbWheel set-velocity to velocity. + to get-global-velocity: - return ((rWheel get-velocity) + (lWheel get-velocity)) / 2. + return ((rfWheel get-velocity) + (lfWheel get-velocity)) / 2. - + to turn-right with-velocity velocity (float): - lWheel set-velocity to velocity. - rWheel set-velocity to -velocity. + + to turn-right: + tright++. + + self rotate around-axis (0,1,0) by (-1.5709/10)*tright. + + if(tright==10): tright=0. + + + + to turn-left: + tleft++. + + self rotate around-axis (0,1,0) by (1.5709/10)*tleft. + + if(tleft==10): tleft=0. - + 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). @@ -214,11 +227,17 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) { 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: + lfWheel = (self add-wheel at (2, 0, -1.5)). + lbWheel = (self add-wheel at (-2, 0, -1.5)). + rfWheel = (self add-wheel at (2, 0, 1.5)). + rbWheel = (self add-wheel at (-2, 0, 1.5)). + + tleft=tright=0. #Debe ser inicializado en 0 esta asi para probar!!!!!!!!!!!!!!!!!!!!!!!! + avanzando=1. + retrocediendo=0. + girando=0. + + +to iterate: valuef,valueb,valuer,valuel (float). fl, fr(float). @@ -226,26 +245,52 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) { valueb=bSensor get-data. valuel=lSensor get-data. valuer=rSensor get-data. + + print "senforl: $valuel". - 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). + if(avanzando): + if(valuef>15): + self set-global-velocity to (15). + else + { + avanzando=0. + retrocediendo=1. + } + + + if(retrocediendo): + if(valueb>15): + self set-global-velocity to (-15). + else + { + tleft=1. + retrocediendo=0. + } + + + if((tleft) && (valuel>10)): + { + #self set-global-velocity to (0). + self turn-left. + } + else + { + avanzando=1. + #tright=1. + } - #fl = (flWheel get-velocity). - #fr = (frWheel get-velocity). - #print " sensorf: $value sensorb $valueb, fr: $fr, fl: $fl". +#! + if((tright) && (valuer>10)): + { + #self set-global-velocity to (0). + self turn-right. + } + else + { + avanzando=1. + #tright=1. + } +!# } @@ -374,16 +419,15 @@ Link : CeldasSensor (aka CeldasSensors) { v,obs(vector). aux(float). j (int). - des1,des2,des3(int). + des2,des3(int). wallBegin,wallEnd,wallCenter (float). - aux1,aux2,aux3,aux4 (float). - yo,toObstacle, transDir (vector). + toObstacle(vector). largeWall (float). - source,destiny (vector). + obsLoc (vector). location (vector). - posObstacle,posSensor (vector). + posObstacle,destiny,yo(vector). draw clear. value = 0.0. @@ -444,37 +488,20 @@ Link : CeldasSensor (aka CeldasSensors) { 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 ". - - - - + draw clear. 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 ". + #print "sensor: $id obstaculo: $posObstacle direP: $destiny direS: $direction yo: $yo ". } } @@ -487,7 +514,7 @@ Link : CeldasSensor (aka CeldasSensors) { #Dibujo el laser draw set-color to (1, 0, 0). draw draw-line from (self get-location) to (obs). - return dist. + return min. }