From be5105421a39e40bae35e4c3d538a225cc460c3c Mon Sep 17 00:00:00 2001 From: Leandro Lucarella Date: Sat, 16 Dec 2006 23:42:22 +0000 Subject: [PATCH] =?utf8?q?Se=20avanza=20m=C3=A1s=20con=20la=20integraci?= =?utf8?q?=C3=B3n=20(aun=20sin=20probar).?= MIME-Version: 1.0 Content-Type: text/plain; charset=utf8 Content-Transfer-Encoding: 8bit --- trunk/src/breve/robot/Celdas-2-7.tz | 215 ++++++++++++++++++--------- trunk/src/breve/robot/laberintov4.tz | 2 +- 2 files changed, 143 insertions(+), 74 deletions(-) diff --git a/trunk/src/breve/robot/Celdas-2-7.tz b/trunk/src/breve/robot/Celdas-2-7.tz index 46d4bfa..30e36db 100644 --- a/trunk/src/breve/robot/Celdas-2-7.tz +++ b/trunk/src/breve/robot/Celdas-2-7.tz @@ -7,6 +7,7 @@ @use SistemaAutonomo. @define CELDAS_MAX_VELOCITY 30. +@define CELDAS_TURNO 100. PhysicalControl : CeldasControl { % This class is used for building simple vehicle @@ -163,13 +164,14 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) { lSensor, rSensor, fSensor, bSensor (object). lfWheel,rfWheel,lbWheel,rbWheel (object). tleft,tright (int). + avanzando,retrocediendo,girando_izq,girando_der(int). iterate(int). teorias (list). sa (object). teoria (object). entorno (hash). datos-finales (hash). - avanzando,retrocediendo,girando(int). + plan_finished (int). - to get-density: return 20.0. @@ -192,17 +194,17 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) { + to turn-right: tright++. - self rotate around-axis (0,1,0) by (-1.5709/10)*tright. + self rotate around-axis (0,1,0) by (-1.5709/CELDAS_TURNO)*tright. - if(tright==10): tright=0. + if (tright == CELDAS_TURNO): tright=0. + to turn-left: tleft++. - self rotate around-axis (0,1,0) by (1.5709/10)*tleft. + self rotate around-axis (0,1,0) by (1.5709/CELDAS_TURNO)*tleft. - if(tleft==10): tleft=0. + if (tleft == CELDAS_TURNO): tleft=0. + to get-sensor-value: @@ -214,40 +216,10 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) { entorno{"sensor_r"} = (rSensor get-data). entorno{"sensor_l"} = (lSensor get-data). entorno{"movido"} = 0. # TODO - sa update-entorno entorno entorno. + sa update-entorno with entorno. +to init: - sa = new SistemaAutonomo. - iterate=0. - - teorias = 2 new Teorias. - teorias{0} init named "Avanzar" with-action "avanzar". - teorias{0} set-dato-inicial name "sensor_f" value 0. - teorias{0} set-dato-inicial name "sensor_b" value ANY. - teorias{0} set-dato-inicial name "sensor_r" value ANY. - teorias{0} set-dato-inicial name "sensor_l" value ANY. - teorias{0} set-dato-inicial name "movido" value ANY. - teorias{0} set-dato-final name "sensor_f" value ANY. - teorias{0} set-dato-final name "sensor_b" value ANY. - teorias{0} set-dato-final name "sensor_r" value ANY. - teorias{0} set-dato-final name "sensor_l" value ANY. - teorias{0} set-dato-final name "movido" value 1. - - teorias{1} init named "Rotar a derecha" with-action "derecha". - 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. - teorias{1} set-dato-inicial name "sensor_l" value ANY. - teorias{1} set-dato-inicial name "movido" value ANY. - teorias{1} set-dato-final name "sensor_f" value 0. - teorias{1} set-dato-final name "sensor_b" value ANY. - teorias{1} set-dato-final name "sensor_r" value ANY. - teorias{1} set-dato-final name "sensor_l" value ANY. - teorias{1} set-dato-final name "movido" value 0. - - sa add teoria teorias{0}. - sa add teoria teorias{1}. - + # Configuracion de robot fSensor = (self add-sensor at (2.0, .4, 0)). fSensor set-direction to (1,0,0). #fSensor set-direction to (0,0,1). @@ -264,77 +236,174 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) { 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. - 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. + tleft=tright=0. + avanzando=0. retrocediendo=0. - girando=0. + girando_izq=0. + girando_der=0. - +to iterate: - valuef,valueb,valuer,valuel (float). - fl, fr(float). + # Configuracion de sistema autonomo + sa = new SistemaAutonomo. + iterate = 0. + plan_finished = 1. # así planificamos apenas empezamos - valuef=fSensor get-data. - valueb=bSensor get-data. - valuel=lSensor get-data. - valuer=rSensor get-data. + teorias = 4 new Teorias. + teorias{0} init named "Avanzar" with-action "adelante". + teorias{0} set-dato-inicial name "sensor_f" value 0. + teorias{0} set-dato-inicial name "sensor_b" value ANY. + teorias{0} set-dato-inicial name "sensor_r" value ANY. + teorias{0} set-dato-inicial name "sensor_l" value ANY. + teorias{0} set-dato-inicial name "movido" value ANY. + teorias{0} set-dato-final name "sensor_f" value ANY. + teorias{0} set-dato-final name "sensor_b" value ANY. + teorias{0} set-dato-final name "sensor_r" value ANY. + 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} 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. + teorias{1} set-dato-inicial name "sensor_l" value ANY. + teorias{1} set-dato-inicial name "movido" value ANY. + teorias{1} set-dato-final name "sensor_f" value 0. + teorias{1} set-dato-final name "sensor_b" value ANY. + teorias{1} set-dato-final name "sensor_r" value ANY. + teorias{1} set-dato-final name "sensor_l" value ANY. + teorias{1} set-dato-final name "movido" value 1. + + teorias{2} init named "Rotar a derecha" with-action "derecha". + teorias{2} set-dato-inicial name "sensor_f" value 1. + teorias{2} set-dato-inicial name "sensor_b" value ANY. + teorias{2} set-dato-inicial name "sensor_r" value ANY. + teorias{2} set-dato-inicial name "sensor_l" value ANY. + teorias{2} set-dato-inicial name "movido" value ANY. + teorias{2} set-dato-final name "sensor_f" value 0. + teorias{2} set-dato-final name "sensor_b" value ANY. + teorias{2} set-dato-final name "sensor_r" value ANY. + 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} 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. + teorias{3} set-dato-inicial name "sensor_l" value ANY. + teorias{3} set-dato-inicial name "movido" value ANY. + teorias{3} set-dato-final name "sensor_f" value 0. + teorias{3} set-dato-final name "sensor_b" value ANY. + teorias{3} set-dato-final name "sensor_r" value 1. + teorias{3} set-dato-final name "sensor_l" value ANY. + teorias{3} set-dato-final name "movido" value 0. + + sa add teoria teorias{0}. + sa add teoria teorias{1}. + sa add teoria teorias{2}. + sa add teoria teorias{3}. datos-finales{"movido"} = 1. - print "senforl: $valuel". + sa update-datos-finales with datos-finales. + +to iterate: + fl, fr(float). - sa update-datos-finales datos-finales datos-finales. - sa plan. + if (0): # TODO posicion_final == posicion_actual + { + print "Llegamos al FINAL!!!". + return. + } - if (iterate==0): { - print "iteracion 0". - if (sa has-next-theory): + if (plan_finished): + { + self update-entorno. + sa plan. # Si no tenemos plan, lo hacemos + plan_finished = 0. + # TODO posicion_inicial = posicion_actual + if (! sa-has-next-theory): { + print "El planificador no encuentra PLAN!!!". + return. + } + } + if (iterate == 0): + { + print "iteracion 0". + if (sa has-next-theory): + { print "hay teoria". - teoria = sa get-next-theory. - if ((teoria get-accion) == "avanza"): { - #entorno{"sensor"} = 1. - #entorno{"movido"} = 1. - self set-global-velocity. - - } - if ((teoria get-accion) == "retrocede"): { - self set-global-velocity to -5. - + teoria = sa get-next-theory. + if ((teoria get-accion) == "adelante"): + { + avanzando = 1. + retrocediendo = 0. + girando_izq = 0. + girando_der = 0. + } + if ((teoria get-accion) == "atras"): + { + avanzando = 0. + retrocediendo = 1. + girando_izq = 0. + girando_der = 0. + } + if ((teoria get-accion) == "izquierda"): + { + avanzando = 0. + retrocediendo = 0. + girando_izq = 1. + girando_der = 0. + } + if ((teoria get-accion) == "derecha"): + { + avanzando = 0. + retrocediendo = 0. + girando_izq = 0. + girando_der = 1. } } } - if ((iterate==100) && (teoria)): + if (iterate == CELDAS_TURNO): { - self update-entorno. - if (sa validate theory teoria): { + # TODO if (posicion_actual == posicion_inicial): movido = false. else movido = true. + if (sa validate theory teoria): + { print "valida". } - else { - print "Teoria no valida, salimos". + else + { + print "Teoria no valida". + plan_finished = 1. } - } iterate++. - if(iterate==101): - iterate=0. + 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. + } Stationary : CeldasObstacle (aka CeldasObstacles) { diff --git a/trunk/src/breve/robot/laberintov4.tz b/trunk/src/breve/robot/laberintov4.tz index 157623c..488ae42 100644 --- a/trunk/src/breve/robot/laberintov4.tz +++ b/trunk/src/breve/robot/laberintov4.tz @@ -1,6 +1,6 @@ @use Control. @use Stationary. -@use Celdas-2-6. +@use Celdas-2-7. @define altoPared 5. @define posYPared 25. -- 2.43.0