prototipo de navegaciÓn difusa para robots...

PROTOTIPO DE NAVEGACIÓN DIFUSA PARA ROBOTS MÓVILES EN ENTORNOS ESTRUCTURADOS

SERGIO ANDRÉS LÓPEZ RÍOS EIDER HARVEY MARIACA ORTIZ

CLAUDIA FAISULY OCHOA GALEANO

Guía de usuario

Asesor (es) ANDRÉS MAURICIO CÁRDENAS

GUSTAVO ACOSTA AMAYA

UNIVERSIDAD DE SAN BUENVENTURA FACULTAD DE INGENIERÍA INGENIERÍA ELECTRÓNICA

MEDELLÍN 2009

CONTENIDO

pág.

INTRODUCCIÒN 4

1. ENSAMBLE DE LA PLATAFORMA MÓVIL 8

1.1. BASE ACRÍLICA 8

1.2. MOTORES Y CAJAS REDUCTORAS (GEARBOX) 9

1.3. RUEDAS 9

1.3.1. Ballcaster 9

1.4. BATERÍA 10

2. CIRCUITO DE POTENCIA 11

3. CIRCUITO DE CONTROL 12

3.1. SENSOR ULTRASÓNICO MAX SONAR EZ1 12

3.2. SENSOR INFRARROJO GP2D12 13

4. CABLEADO 14

5. MODO DE OPERACIÓN 15

5.1. DIAGRAMA DE FLUJO 16

5.2. PLANTEAMIENTO DEL ALGORITMO DE CONTROL EN EL TOOLBOX DE MATLAB 17

5.2.1. Simulación en matlab 18

5.2.2. Caracterización de las variables lingüísticas y

conjuntos difusos 19

5.2.3. Reglas de inferencia 21

5.2.4. Vista de reglas en 3D 24

5.2.5. Comando readfis 25 5.2.6. Tabla FAM 26

4

INTRODUCCIÒN

Para llevar a cabo las tareas asignadas, un robot móvil debe poder detectar y

evitar obstáculos a su alrededor, y de manera simultánea, elaborar

representaciones que le permitan determinar su localización en el entorno de

trabajo. Estas representaciones o mapas se construyen y actualizan a través de la

exploración segura y sistemática del entorno. Existen dos estrategias de

navegación para desplazarse en el entorno: la navegación deliberativa y la

navegación reactiva. En la navegación deliberativa el robot planifica la ruta que le

permitirá desplazarse del punto de partida al punto de llegada. En la navegación

reactiva el robot responde de manera inmediata a los estímulos que recibe del

entorno. La finalidad consiste en mantener la integridad física del robot.

La literatura científica ha reportado diferentes estrategias de comportamientos reactivos para la navegación del entorno:

Modelos computacionales de entornos de trabajo

En las últimas dos décadas el mapeo de entornos ha sido un ámbito de

investigación con una marcada preferencia por las técnicas probabilísticas, lo que

se debe al hecho de que permiten modelar las incertidumbres asociadas a los

sistemas de percepción comúnmente empleados en la robótica móvil.

Dispositivos como el sonar, el laser, módulos infrarrojos, compases, brújulas y

GPS, permiten a los robots percibir y adquirir datos del entorno; sin embargo se

hace necesario además, plantear estrategias de navegación para la exploración

del medio circundante que permitan compensar ruido en las medidas y

limitaciones en los rangos de observación.

Actualmente los paradigmas dominantes en la representación de entornos de

trabajo son los mapas topológicos, los mapas métricos y versiones combinadas de

éstos. Mientras que en las representaciones métricas se busca establecer

relaciones métricas exactas entre obstáculos en las representaciones topológicas

se busca establecer conexiones entre localizaciones reconocibles y distintivas del

entorno.

Mapas Topológicos

Los mapas topológicos buscan establecer conexiones entre localizaciones

reconocibles y distintivas del entorno. Pueden ser considerados como

representaciones abstractas que describen relaciones entre diversos elementos

del entorno sin necesidad de emplear referencias absolutas.

5

Desde un punto de vista formal, las representaciones topológicas se valen de

grafos para representar el espacio físico. Las marcas, puntos o lugares

característicos se representan por nodos a los que se asocia información

perceptual como histogramas de colores, imágenes y datos métricos. Los arcos

permiten establecer una relación simple de conectividad entre nodos y pueden

indicar, por ejemplo, que tan viable puede ser desplazarse a sitios contiguos.

También se suele asignar a cada arco un peso con el que se indica el costo de

acceder a un nodo adyacente. En los mapas topológicos no es necesario incluir

información métrica adicional puesto que en cada nodo se incorporan datos

relativos a los nodos contiguos a los que puede acceder.

Entre las características más relevantes de los mapas topológicos están:

- Son representaciones compactas en las solamente se capturan aquellas características del entorno necesarias para la navegación. Por tal motivo constituyen representaciones fáciles de almacenar y comunicar.

- Puesto que el reconocimiento de características depende del punto de vista del robot, entonces se presentan con cierta frecuencia situaciones de ambigüedad (aliasing) en la identificación de marcas.

- Son representaciones que permiten razonamiento simbólico de alto nivel para el mapeo, navegación, planeamiento y comunicación; características que facilitan notablemente el diseño de interfaces de usuario.

- Los errores de desplazamiento y de orientación que se acumulan durante el desplazamiento de un nodo a otro no necesariamente son acumulados en la representación global del entorno.

- Para entornos de grandes dimensiones resultan ser modelos de difícil construcción y mantenimiento.

Mapas Métricos

Los modelos métricos buscan obtener representaciones geométricas

relativamente detalladas del entorno. Permiten describir el mundo a partir de sus

propiedades geométricas o mediante su discretización. En el primer caso se

elaboran descripciones métricas detalladas entre objetos con diferentes

propiedades geométricas. En el segundo caso la superposición de una malla o

rejilla permite discretizar el entorno en una serie de celdas uniformes que

representan, cada una, una parte del área de trabajo. A cada celda se asigna una

6

o más propiedades que indican la creencia de que esa porción del espacio se

encuentra ocupada o no.

Mapas Geométricos

Frecuentemente en la navegación de espacios interiores y exteriores es posible

capturar formas geométricas simples (primitivas) mediante los sistemas de

percepción del robot. En los mapas geométricos generalmente se establecen

distancias, dimensiones y posiciones entre primitivas geométricas que se pueden

ser consideradas como características distintivas del entorno o balizas. Una o

varias de estas características permiten representar los objetos presentes en el

área de trabajo, por ejemplo, dos segmentos de línea que se interceptan

representan una esquina. En tal sentido, una representación geométrica del

entorno no sería más que una lista de características geométricas referenciadas

con precisión en un sistema global de coordenadas.

En la elaboración de mapas geométricos, las primitivas geométricas deberán

cumplir una serie de requerimientos como son: no dar lugar a ambigüedades y ser

distinguibles, permanecer visibles a los sistemas de percepción del robot, ser

estables e variar poco con el tiempo y permitir la determinación de distancias

entre ellas.

Las primitivas geométricas de uso más generalizado son:

- Puntos: en mapas bidimensionales representan objetos dispuestos verticalmente y de espesor reducido, como por ejemplo postes o bordes de muros.

- Segmentos de línea: generalmente corresponden a fronteras de obstáculos presentes en el entorno como por ejemplo muros y puertas. En representaciones 2D se parametrizan vectorialmente e incorporan información relativa al origen local de coordenadas, la longitud del segmento y la orientación con respecto al sistema de referencia global.

- Esquinas: conformadas por dos segmentos de línea que se interceptan ortogonalmente.

- Segmentos de arco: primitivas que representan columnas con secciones

transversales de patrón circular, como depósitos circulares de basura, lámparas, etc.

- Polígonos: constituyen el contorno de los obstáculos en el plano horizontal.

7

Mapas de Celdas

Propuestos inicialmente por Moravec y Elfes, los mapas de celdas constituyen

una herramienta de modelado probabilístico para la representación de obstáculos

en entornos de trabajo. Se elaboran a partir de la discretización del entorno

mediante una rejilla de celdas rectangulares uniformes.

Con el tiempo han surgido un número relativamente elevado de variaciones que

dependen del contenido de las celdas, su forma y la manera en que se incorpora

nueva información a ellas. Se conocen las siguientes aproximaciones:

- Celdas de Ocupación (Occupancy Grid) - Celdas de Certeza (Cetainty Grid) - Mapas de Probabilidad (Probability Maps) - Histograma de Celdas (Histogramic Grids) - Celdas de Inferencia (Inference Grids)

En los mapas de celdas, cada celda representa una porción del área de trabajo a

la cual se asigna un valor con el que se indica la probabilidad de encontrarse

ocupada o no por un obstáculo. El valor se calcula a partir de medidas de rango y

estimaciones odométricas suministradas por los sistemas de percepción del robot

durante la exploración del entorno.

8

1. ENSAMBLE DE LA PLATAFORMA MÓVIL

La plataforma robótica de la empresa dynamo electronics es para terreno plano,

de bajo costo, dimensiones reducidas y alto grado de flexibilidad para el control de

desplazamientos puesto que corresponde a una configuración diferencial con una

rueda castor ball de soporte. Gracias a su configuración diferencial la plataforma

puede efectuar giros sobre su propio eje y su geometría circular le permite

moverse sin dificultad en su área de trabajo.

1.1. BASE ACRÍLICA

La plataforma de acrílico es de forma circular con un ancho de 12.9 cm y un largo de 12.9 cm, cuenta con sus respectivas perforaciones para el ensamble las cuales se realizaron por medio de corte laser.

1.2. MOTORES Y CAJAS REDUCTORAS (GEARBOX)

9

La caja de dos motores con engranaje independiente tiene dimensiones de 7.5 cm de largo, 10 cm de ancho (extremo a extremo de los ejes) y ejes de 3mm, se pueden ensamblar para obtener relación de reducción velocidad de 58:1 (212rpm aproximadamente) y de 203:1(59rpm aproximadamente); los motores requieren un voltaje de alimentación aproximado de 3-6 voltios.

1.3. RUEDAS Las ruedas tienen un diámetro de 3.6 cm y un ancho de 1.5 cm cada una, su material es en goma de caucho negro y sus rines son de plástico.

1.3.1. Ballcaster

Rueda de apoyo especial para la plataforma robótica, configuración de tres alturas diferentes 1.5, 2.5 y 3.5 cm, el soporte es de plástico negro y la rueda de apoyo es un balín metálico.

1.4. BATERÍA

10

La batería es de plomo ácido, seca recargable, entrega un voltaje de 12V y una corriente de 1.2Ah. Sus dimensiones son de 95mm de largo, 51mm de alto y 43mm de ancho.

11

2. CIRCUITO DE POTENCIA

El circuito de potencia fue conformado por dos opto acopladores (4N25) los cuales cumplen la función de protección de las salidas del microcontrolador contra corto circuito aislándolas del puente H. Un par de relés que conmutan para realizar la inversión de giro de cada uno de los motores. Cuenta además con un regulador de voltaje (LM7805) que polariza el circuito integrado puente H (L293) y a una compuerta lógica (74LS04). El puente H y la compuerta lógica tienen como función el control de la inversión de giro de los motores. Un regulador LM7805 independiente se emplea para la alimentación del sonar con el fin de evitar ruido que afecte la toma de medidas del sonar.

12

3. CIRCUITO DE CONTROL

El circuito de control está conformado por un micro controlador 16F877 de microchip y un regulador de voltaje LM7805 para su alimentación. Este circuito tiene como función recibir los datos obtenidos por el sensor ultrasónico de forma serial y el sensor infrarrojo de forma análoga. El microcontrolador se encarga de procesar los datos obtenidos por los sensores ingresándolos al sistema fuzzy para posteriormente entregar a la salida una señal PWM para el control de los motores.

3.1. SENSOR ULTRASÓNICO MAX SONAR EZ1

El sistema de percepción principal corresponde a un transductor ultrasónico (Max

sonar EZ1) para la estimación de distancias. Este se ubica en el costado derecho

de la plataforma móvil y las medidas de rango que proporciona le permiten al

controlador mantener una distancia constante a la pared. Las características del

sensor son:

- Rango de medida de 6 a 254 pulgadas o 0.1524m a 6.45 m. - Bajo consumo (2 mA). - Tres formatos de salida: ancho de pulso, voltaje analógico, señal digital

serial.

13

- Bajo peso (4.3 gr). - Medidas estables. - Medida de rango por tiempo de vuelo ToF (Time of Fly).

3.2. SENSOR INFRARROJO GP2D12

El sistema de percepción secundario es un sensor de tipo infrarrojo ubicado al

frente, con el fin de evitar colisiones frontales con objetos circundantes. El

dispositivo empleado fue el GP2D12 de Sharp, el cual entrega un voltaje no lineal

que indica la distancia respecto al objeto. Las características del sensor son:

- La salida está disponible de forma continua y su valor es actualizado cada 32 ms.

- Normalmente se conecta esta salida a la entrada de un convertidor analógico digital el cual convierte la distancia en un número que puede ser usado por el microprocesador.

- La salida también puede ser usada directamente en un circuito analógico. Hay que tener en cuenta que la salida no es lineal.

- El sensor utiliza solo una línea de salida para comunicarse con el procesador principal.

- El sensor se entrega con un conector de 3 pines. - Tensión de funcionamiento 5V. - Temperatura funcionamiento:-10 a 60ºC. - Consumo Medio: 35 mA. - Margen de medida 10cm a 80 cm.

14

4. CABLEADO

Debido a que se utilizo una tarjeta entrenadora para microcontroladores fue necesario utilizar para la conexión del circuito de control, el circuito de potencia y el sensor ultrasónico cable de tipo correa. La correa número uno conecta el puerto c del microcontrolador con dos opto acopladores 4N25 para las dos salidas PWM y adicionalmente conecta la salida del dato serial, el control de giro de ambos motores y un switche start/stop para el inicio y parada del sistema. La correa número dos conecta el sensor ultrasónico y el circuito de potencia en donde hay una conexión con el puerto serie del microcontrolador. La correa número tres conecta el sensor infrarrojo y envía un voltaje de referencia al puerto A del microcontrolador. El voltaje de referencia es comparado con el dato obtenido por el sensor infrarrojo para tener precisión el las medidas.

15

5. MODO DE OPERACIÓN

La plataforma cuenta con un switche start/stop ubicado en el circuito de potencia el cual le permite iniciar su marcha. Desde su arranque y Durante el recorrido el sistema de percepción ultrasónico e infrarrojo toman muestras de la distancia a la que se encuentra la plataforma. Estos datos son procesados por el microcontrolador 16F877 en donde son comparados con un set point permitiendo determinar la lejanía o cercanía respecto a la pared lateral derecha y la pared frontal. Una vez determinado el error de distancia el algoritmo difuso controla el ciclo de dureza del motor derecho ubicando la plataforma así: si la plataforma se encuentra lejos de la pared el pwm del motor derecho disminuye de tal forma que esta quede ubicada mas cerca del set point, si la plataforma se encuentra ubicada cerca de la pared el pwm del motor derecho aumenta para de igual forma ubicar la plataforma cerca del set point y si la plataforma se encuentra en el set point esta debe conservar una línea recta hasta que encuentre una perturbación del entorno en el cual se encuentra operando.

16

5.1. DIAGRAMA DE FLUJO

PWM = 85

PWM = 80

DISTANCIA ES LEJOS

+

+

SI SETPOINT > MEDICION ENT

ERROR ES POSITIVO


ERROR ES POSITIVO


ERROR ES POSITIVO

MEDICION ENT ERROR

ES POSITIVO

2

PWM = 90

2 DISTANCIA ES ALGO LEJOS

PWM = 100

SETPOINT < MEDICION

ERROR ES NEGATIVO

SETPOINT = MEDICION

ERROR ES CERO

1

COMPARAR EL SETPOINT CON LA

MEDICION ACTUAL

INICIO

LEER DISTANCIA DEL SENSOR

POR PUERTO SERIAL

CONVERTIR EL DATO RECIBIDO DE

ASCII A DECIMAL

1 SETPOINT > MEDICION

ERROR ES POSITIVO

PWM = 102

PWM = 105

DISTANCIA ES CERCA

+

+


ERROR ES POSITIVO


ERROR ES POSITIVO


ERROR ES POSITIVO

MEDICION ENT ERROR

ES POSITIVO

PWM = 110

DISTANCIA ES MUY CERCA

+

+


ERROR ES POSITIVO


ERROR ES POSITIVO


ERROR ES POSITIVO

MEDICION ENT ERROR

ES POSITIVO

SI

SI SI

SI SI

SI

SI

NO

NO

NO

NO

NO

NO

17

5.2. PLANTEAMIENTO DEL ALGORITMO DE CONTROL EN EL TOOLBOX DE MATLAB

Para iniciar el desarrollo de la simulación es necesario el planteamiento inicial de

las variables lingüísticas tanto de entrada como de salida y la respectiva

descripción de las etiquetas lingüísticas utilizadas que están definidas mediante la

siguiente tupla.

x=Distancia Lateral Derecha.

T(x)={ DLMC, DLCE, DLAC, DLNO, DLAL, DLLE,DLML}

U: Universo del discurso para la variable lingüística x. U=[6,40] (distancia en

pulgadas).

G: Regla sintáctica de acuerdo al conjunto de términos definidos.

M: Regla semántica: DLMC (Distancia Lateral Muy Cerca), DLCE (Distancia

Lateral Cerca), DLAC (Distancia Lateral Algo Cerca), DLNO (Distancia Lateral

Normal), DLAL (Distancia Lateral Algo Lejos), DLLE (Distancia Lateral Lejos),

DLML (Distancia Lateral Muy Lejos).

y=Distancia Frontal.

T(y)={DFCE, DFNO, DFLE}

U: Universo del discurso para la variable lingüística x. U=[10,50] (distancia en

centímetros).


M: Regla semántica: DFCE (Distancia Frontal Cercana), DFNO (Distancia Frontal

Normal), DFLE (Distancia Frontal Lejana).

z=PWM motor derecho.

T(y)={PWMML, PWMAL, PWML, PWMN, PWMAH, PWMH, PWMMH}

U: Universo del discurso para la variable lingüística x. U=[70,120] (ciclo de dureza

en porcentaje).


M: Regla semántica: PWMML (PWM Muy Bajo), PWMAL (PWM Algo Bajo), PWML

(PWM Bajo), PWMN (PWM Normal), PWMAH (PWM Algo Alto), PWMH (PWM

{x, T(x), U, G, M}

Variable lingüística, p. ej. x=distancia lateral derecha

Términos o valores lingüísticos, p. ej. T(x)={DLMC, DLCE, DLAC, DLNO, DLAL, DLLE,DLML }

Universo del discurso, p. ej. 6 a 40 distancia en

pulgadas que se expresa como: U=[6,40]

Regla sintáctica

Regla semántica

18

Alto), PWMMH (PWM Muy Alto).

5.2.1. Simulación en matlab a. Para realizar la simulación del algoritmo de control en matlab primero se abre la ventana de trabajo de matlab (workspace) y se llama el comando fuzzy.

b. Esta función abre automáticamente una ventana llamada FIS editor, la cual presenta un bloque que identifica una variable de entrada, un bloque que representa el control; puede ser de tipo mandani o sugeno dependiendo de las características del controlador y otro bloque que identifica una variable de salida.

c. Si el control requiere más de una variable de entrada o salida en la ventana FIS editor se selecciona la opción edit, luego add variable y se selecciona la variable requerida (input ó output).

19

5.2.2. Caracterización de las variables lingüísticas y conjuntos difusos En el caso de navegación difusa para robots móviles en entornos estructurados fueron definidas dos variables lingüísticas de entrada, un controlador tipo mandani y una variable de salida.

Se da doble clic sobre la variable que se requiere editar, sale una ventana llamada membership function editor en la cual se puede cambiar el rango que define el universo del discurso y los parámetros que caracterizan los conjuntos difusos dentro de esa variable, al igual permite modificar el tipo de función a utilizar (triangular, trapezoidal, sinusoidal, etc.).

20

5.2.3. Reglas de inferencia

Una vez definidas las variables de entrada – salida y los conjuntos difusos se

procede a crear las reglas que definirán el funcionamiento correcto del sistema.

En la ventana FIS editor se da un clic en edit y luego clic en rules se abrirá

una ventana llamada rule editor.

21

En la ventana rule editor se encuentran unas entidades que corresponden a

las operaciones que debe realizar la lógica difusa con el formato IF-THEN.

Para este caso se encuentran dos entidades que representan las variables

de entrada y una entidad que representa la variable de salida. Se da clic en

la condición que se desea operar en la primera entidad con una condición

de la segunda entidad y se selecciona la condición que el controlador debe

efectuar en la entidad salida, por último se da clic en add rule para generar

la regla; de esta manera se genera cada una de las reglas que permitirán

desarrollar el algoritmo de control.

22

Una vez generadas las reglas se puede visualizar de forma grafica su

comportamiento. En la ventana FIS editor se selecciona la opción view y luego la

opción rules para desplegar una ventana llamada rule viewer.

23

5.2.4. Vista de reglas en 3D

Para obtener una visión en 3D de las regla se selecciona en la ventana de FIS

editor la opción view, luego la opción surface y se despliega una vantana llamada

surface viewer.

24

5.2.5. Comando readfis

Con el comando readfis se encuentra la documentación general del sistema de

control:

readfis ans = name: 'control final1' type: 'mamdani' andMethod: 'min' orMethod: 'max' defuzzMethod: 'centroid' impMethod: 'min' aggMethod: 'max' input: [1x2 struct] output: [1x1 struct] rule: [1x21 struct]

25

5.2.6. Tabla FAM

De acuerdo a las reglas antes planteadas se crea entonces la siguiente tabla

llamada BIOFAM (Binary Input-Output Fuzzy Asociative Memory).

y

x

DLMC DLCE DLAC DLNO DLAL DLLE DLML

DFCE PWMMH PWMMH PWMMH PWMMH PWMMH PWMMH PWMMH

DFNO PWMMH PWMH PWMAH PWMH PWMAL PWML PWMML

DFLE PWMMH PWMH PWMAH PWMN PWMAL PWML PWMML

26

ALGORITMO FUZZY

program SONAR_FUZZY3

Dim TX, CONTADOR as word

A, B, i, J, K, COLUMNA,TOTAL,DATO, VALOR1, VALOR2, DATO_DEC,DATO_UNI,

DATO_TOTAL, TEST,

MEDICION, SETPOINT,EROR as byte

VALOR as WORD[4]

VALOR_TX as String[8]

TESTEO AS CHAR

TESTER AS STRING[5]

main:

OPTION_REG = $84

ADCON1 = $88

TRISA = $FF

TRISC = %11110000

PWM1_Init(1000)

PWM2_Init(1000)

USART_init(9600)

clearbit(portc,1)

clearbit(portc,2)

clearbit(portc,3)

clearbit(portc,0)

'SETBIT(PORTC,3)

TMR0 = 96

'INTCON = $A0

27

CONTADOR = 0

inicio:

COLUMNA = 0

DATO_TOTAL = 0

DATO_UNI = 0

DATO_DEC = 0

VALOR1 = 0

VALOR2 = 0

J = 0

K = 1

SETPOINT = 20

EROR = 0

MEDICION = 0

B = 0

FOR i = 0 TO 3

VALOR[i] = 0

NEXT i

WHILE TRUE

GOSUB DATO_SERIAL

IF PORTC.4 = 1 THEN

PWM1_STOP

PWM2_STOP

CLEARBIT(PORTC,0)

CLEARBIT(PORTC,3)

'DELAY_MS(800)

28

PWM1_START

PWM2_START

PWM1_CHANGE_DUTY(100)


GOTO CICLO_2

END IF

WEND

CICLO_2:

WHILE TRUE

GOSUB DATO_SERIAL

GOSUB COMPARACION2

IF PORTC.4 = 0 THEN

PWM1_STOP

PWM2_STOP

goto inicio

END IF

WEND

DATO_SERIAL:

A = 0

WHILE A < 4

if (USART_Data_Ready = 1) then

TX = USART_Read

IF TX=0x0D THEN

29

J=0

'VALOR_TX[4]=0

VALOR_TX[6]=0

VALOR_TX[7]=0

VALOR_TX[8]=0

lcd_out(2,1,VALOR_TX)

LCD_OUT(1, 10, TESTER)

else

IF TX <> "R" THEN

A = A + 1

VALOR_TX[J]=TX

J = J + 1

END IF

END IF

end if

TESTEO = VALOR_TX[1]

TEST = TESTEO - 48

DATO_DEC = TEST*10

TESTEO = VALOR_TX[2]

DATO_UNI = TESTEO - 48

DATO_TOTAL = DATO_UNI + DATO_DEC

MEDICION = DATO_TOTAL

BYTETOSTR(DATO_TOTAL, TESTER)

WEND

30

RETURN

COMPARACION2:

EROR = 0

IF MEDICION = SETPOINT THEN


ELSE

IF MEDICION > SETPOINT THEN

EROR = MEDICION - SETPOINT

IF (EROR > 0) AND (EROR <= 4) THEN


DELAY_MS(180)


DELAY_MS(280)

ELSE



DELAY_MS(200)


DELAY_MS(300)

31

ELSE



DELAY_MS(400)


DELAY_MS(800)

END IF

END IF

END IF

END IF

ELSE

IF MEDICION < SETPOINT THEN

EROR = SETPOINT - MEDICION



ELSE



ELSE

32



END IF

END IF

END IF

END IF

END IF

END IF

RETURN

end.

prototipo de navegaciÓn difusa para robots...

Documents