X-Git-Url: https://git.llucax.com/z.facultad/75.68/celdas.git/blobdiff_plain/a88df59fdcc06b0ff24616e1bf0c8bf58bbca41d..22822a747c21df2f9d99acd8dd2f3f3a9ea54a9a:/trunk/src/breve/Celdas.tz diff --git a/trunk/src/breve/Celdas.tz b/trunk/src/breve/Celdas.tz index 7c1afdc..481c28c 100644 --- a/trunk/src/breve/Celdas.tz +++ b/trunk/src/breve/Celdas.tz @@ -6,9 +6,9 @@ @use Drawing. @use SistemaAutonomo. -@define CELDAS_MAX_VELOCITY 30. -@define CELDAS_TURNO 100. -@define CELDAS_SENSOR_THRESHOLD 9. +@define CELDAS_MAX_VELOCITY 5. +@define CELDAS_TURNO 30. +@define CELDAS_SENSOR_THRESHOLD 6. PhysicalControl : CeldasControl { % This class is used for building simple vehicle @@ -171,7 +171,9 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) { teoria (object). entorno (hash). datos-finales (hash). - plan_finished (int). + plan-finished (int). + posicion-inicial (vector). + posicion-final (vector). - to get-density: return 20.0. @@ -182,6 +184,17 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) { - to get-wheel-radius: return 0.8. + - to near position thePosition (vector) with-error error (float): + vectorAux(vector). + vectorAux = (self get-location) - thePosition. + + #print "-----> (pos, other_pos, diff, error): ", (self get-location), thePosition, vectorAux, error. + + if ((|vectorAux::x| < error) && (|vectorAux::z| < error)): + return 1. + + return 0. + + to set-global-velocity to velocity (float): rfWheel set-velocity to velocity. lfWheel set-velocity to velocity. @@ -193,29 +206,41 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) { + to turn-right: tright++. - self rotate around-axis (0,1,0) by (-1.5709/CELDAS_TURNO)*tright. - - if (tright == CELDAS_TURNO): tright=0. +#! + if (tright == CELDAS_TURNO): + { + fSensor set-direction to (-1,0,0). + bSensor set-direction to (1,0,0). + lSensor set-direction to (0,0,-1). + rSensor set-direction to (0,0,1). + } +!# + to turn-left: tleft++. - self rotate around-axis (0,1,0) by (1.5709/CELDAS_TURNO)*tleft. - - if (tleft == CELDAS_TURNO): tleft=0. - +#! + if (tleft == CELDAS_TURNO): + { + fSensor set-direction to (-1,0,0). + bSensor set-direction to (1,0,0). + lSensor set-direction to (0,0,-1). + rSensor set-direction to (0,0,1). + } +!# + to get-sensor-value: return (fSensor get-sensor-value). + + +to update-entorno: entorno{"sensor_f"} = (fSensor get-sensor-value). entorno{"sensor_b"} = (bSensor get-sensor-value). entorno{"sensor_r"} = (rSensor get-sensor-value). entorno{"sensor_l"} = (lSensor get-sensor-value). - entorno{"movido"} = 0. # TODO sa update-entorno with entorno. +to init: @@ -253,10 +278,14 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) { girando_izq=0. girando_der=0. + posicion-inicial = (self get-location). + posicion-final = (0, 0, 0). + # Configuracion de sistema autonomo sa = new SistemaAutonomo. + sa init with-max-pasos 4 with-max-teorias 15. iterate = 0. - plan_finished = 1. # así planificamos apenas empezamos + plan-finished = 1. # así planificamos apenas empezamos teorias = 4 new Teorias. teorias{0} init named "Avanzar" with-action "adelante". @@ -271,7 +300,7 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) { teorias{0} set-dato-final name "sensor_l" value ANY. teorias{0} set-dato-final name "movido" value 1. - teorias{1} init named "Retroceder" with-action "atras". + teorias{1} init named "Retroceder" with-action "atras".# executed 2. teorias{1} set-dato-inicial name "sensor_f" value 1. teorias{1} set-dato-inicial name "sensor_b" value ANY. teorias{1} set-dato-inicial name "sensor_r" value ANY. @@ -295,7 +324,7 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) { teorias{2} set-dato-final name "sensor_l" value 1. teorias{2} set-dato-final name "movido" value 0. - teorias{3} init named "Rotar a izquierda" with-action "izquierda". + teorias{3} init named "Rotar a izquierda" with-action "izquierda". # executed 2. teorias{3} set-dato-inicial name "sensor_f" value 1. teorias{3} set-dato-inicial name "sensor_b" value ANY. teorias{3} set-dato-inicial name "sensor_r" value ANY. @@ -316,30 +345,64 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) { sa update-datos-finales with datos-finales. +to iterate: + + # Movimiento del robot + if (avanzando): + { + self set-global-velocity to (CELDAS_MAX_VELOCITY). + } + if (retrocediendo): + { + self set-global-velocity to (-CELDAS_MAX_VELOCITY). + } + if (girando_izq): + { + self set-global-velocity to 0. + self turn-left. + #if (iterate): self rotate around-axis (0,1,0) by (1.570796/CELDAS_TURNO)*iterate. + print "izq: ", (1.570796/CELDAS_TURNO)*iterate. + } + if (girando_der): + { + self set-global-velocity to 0. + self turn-right. + #if (iterate): self rotate around-axis (0,1,0) by (-1.570796/CELDAS_TURNO)*iterate. + print "der: ", (-1.570796/CELDAS_TURNO)*iterate. + } + #print "vel: ", (bodyLink get-velocity). + + # Actualiza entorno self update-entorno. - if (0): # TODO posicion_final == posicion_actual + # Chequeo de objetivo + if (self near position posicion-final with-error 5.0): { print "Llegamos al FINAL!!!". self set-global-velocity to 0. return. } - if (plan_finished): + # Planificación + if (plan-finished): { + # Actualiza entorno indicando que no se movió para que + # el planificador actue + sa set-entorno value 0 with-name "movido". sa plan. # Si no tenemos plan, lo hacemos - plan_finished = 0. - # TODO posicion_inicial = posicion_actual + plan-finished = 0. + iterate = 0. if (! (sa has-next-theory)): { - plan_finished = 1. + plan-finished = 1. print "El planificador no encuentra PLAN!!!". return. } } + # Ejecución de teoría if (iterate == 0): { + posicion-inicial = (self get-location). if (sa has-next-theory): { teoria = sa get-next-theory. @@ -372,33 +435,35 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) { girando_der = 1. } } + else + { + plan-finished = 1. + } } + # Validación de teoría if (iterate == CELDAS_TURNO): { - # TODO if (posicion_actual == posicion_inicial): movido = false. else movido = true. - if (sa validate theory teoria): - { + # Actualiza entorno segun si se movio o no + if (self near position posicion-inicial with-error 1.0): + { + sa set-entorno value 0 with-name "movido". } else { - plan_finished = 1. + sa set-entorno value 1 with-name "movido". + } + print iterate. + if (!(sa validate theory teoria)): + { + plan-finished = 1. } - } - - iterate++. - if (iterate == CELDAS_TURNO + 1): iterate = 0. - - # Movimiento del robot - if (avanzando): - self set-global-velocity to (15). - if (retrocediendo): - self set-global-velocity to (-15). - if (girando_izq): - self turn-left. - if (girando_der): - self turn-right. + } + else + { + iterate++. + } } @@ -567,13 +632,26 @@ Link : CeldasSensor (aka CeldasSensors) { des2=0. if(dot(direction,(1,1,1))<0): { - if((dot((self get-location),positiveDirection))>(dot(obsLoc,positiveDirection))): - des2=1. + if(dot((body get-location),positiveDirection) > dot((self get-location),positiveDirection)): + { + if((dot((self get-location),positiveDirection))>(dot(obsLoc,positiveDirection))): + des2=1. + } + else + if((dot((self get-location),positiveDirection))<(dot(obsLoc,positiveDirection))): + des2=1. } else { - if((dot((self get-location),positiveDirection))<(dot(obsLoc,positiveDirection))): + if(dot((body get-location),positiveDirection) < dot((self get-location),positiveDirection)): + { + if((dot((self get-location),positiveDirection))<(dot(obsLoc,positiveDirection))): des2=1. + } + else + if((dot((self get-location),positiveDirection))>(dot(obsLoc,positiveDirection))): + des2=1. + }