X-Git-Url: https://git.llucax.com/z.facultad/75.68/celdas.git/blobdiff_plain/fd72896c8b832ca19985ed67f6b1e4d570c85da2..3de9a6adae2fe5a00ec315ba34f08f6d91a5b265:/trunk/src/breve/Celdas.tz diff --git a/trunk/src/breve/Celdas.tz b/trunk/src/breve/Celdas.tz index 6ed43f4..ae29a51 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 80. -@define CELDAS_SENSOR_THRESHOLD 9. +@define CELDAS_MAX_VELOCITY 5. +@define CELDAS_TURNO 30. +@define CELDAS_SENSOR_THRESHOLD 10. 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. @@ -192,30 +205,35 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) { return ((rfWheel get-velocity) + (lfWheel get-velocity)) / 2. + to turn-right: - tright++. - + #tright++. + #self rotate around-axis (0,1,0) by (-1.5709/CELDAS_TURNO)*tright. + #if (tright == CELDAS_TURNO): tright=0. + if (tright == 0): self set-global-velocity to 0. self rotate around-axis (0,1,0) by (-1.5709/CELDAS_TURNO)*tright. - if (tright == CELDAS_TURNO): tright=0. + else tright++. + to turn-left: - tleft++. - - self rotate around-axis (0,1,0) by (1.5709/CELDAS_TURNO)*tleft. - + #tleft++. + #self rotate around-axis (0,1,0) by (1.5709/CELDAS_TURNO)*tleft. + #if (tleft == CELDAS_TURNO): tleft=0. + if (tleft == 0): self set-global-velocity to 0. + self rotate around-axis (0,1,0) by (1.5709/CELDAS_TURNO)*tleft. if (tleft == CELDAS_TURNO): tleft=0. + else tleft++. + 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,11 +271,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". @@ -272,7 +293,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. @@ -296,7 +317,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. @@ -317,30 +338,39 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) { sa update-datos-finales with datos-finales. +to iterate: + + # 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. @@ -373,29 +403,41 @@ 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 2.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. + } + else + { + iterate++. + } # Movimiento del robot if (avanzando): - self set-global-velocity to (15). + self set-global-velocity to (CELDAS_MAX_VELOCITY). if (retrocediendo): - self set-global-velocity to (-15). + self set-global-velocity to (-CELDAS_MAX_VELOCITY). if (girando_izq): self turn-left. if (girando_der):