, por lo !ue el consumo se muestra a continuaci+n:
Página 24
Figura (1.14) 5ensor 8TR9 es+uem'tico
Características: Fimensiones 6$9H2 A$H2 A$56H2 Eensi+n de funcionamiento 7$7H) Corriente de alimentaci+n 5AA m> 3ormato de salida @ tensiones análogas 8ango de volta*e de salida A) a la tensi+n suministrada (ptima distancia de detecci+n 7mm Máima distancia de detecci+n recomendada ? mm Peso si pines de ca"ecera 7$A9g • • • • • • • •
Interfaz con las salidas KE8@> 0a# varias maneras !ue se pueden interactuar con las salidas KE8@>
•
%ste .ltimo m&todo funcionara si es capaz de o"tener una alta reflei+n de la superficie de "lanco =3igura 5$57, pero pro"a"lemente fallara si tiene un perfil de se'al de menor reflei+n =3igura 5$5$ Página 25
Figura (1.1!) 5ensor 8TR9 salida de distancia de una %ase %lanca
Figura (1.1#) 5ensor 8TR9 salida de distancia de una %ase negra
Página 26
%l fa"ricante proporciona una li"rería >)8 con funciones !ue hacen !ue sea fácil el uso de este sensor con el controlador B76@P$
Página 27
Prototi$o 0ontado
3igura =5$59 %s!uema de vehículo
Página 28
Figura (1.2) :e;6culo terminado
Página 29
C&34"u#o 1t a$a de co nt ro l
In"rodu11%n
%n este capítulo se verá las l+gicas de control aplicadas a este pro#ecto las cuales serán: • • •
PIF -+gica difusa >lgoritmo de Eremau
Cada l+gica será evaluada seg.n las respuestas re!ueridas a la situaci+n, ca"e recordar !ue para la competencia se escogerá la l+gica !ue pueda mantener el error A en la ma#or velocidad posi"le, en el paso siguiente se estudiara el control PIF de forma rápida, por el simple hecho de !ue el auto tiene !ue estar funcionando # ser gra"ado para una evaluaci+n de selecci+n, por ende mientras más rápido se termine más rápido se podrá estudiar el algoritmo de Eremau para la resoluci+n del la"erinto$ %l control por l+gica difusa se va a comparar en dos aspectos, primero se estudiara de forma visual en pista para ver comportamiento en tiempo real del auto, # segundo se verá las curvas de respuesta para una sintonizaci+n más fina de las varia"les lingQísticas para poder llevar a el error igual a A lo más rápido posi"le$
Página 30
&ontrol P2D
^Ku& es el control PIF_ 1%l control PIF es un mecanismo de control !ue a trav&s de un lazo de retroalimentaci+n permite regular la velocidad, temperatura, presi+n # flu*o entre otras varia"les de un proceso en general$ %l controlador PIF calcula atravez de la diferencia entre nuestra varia"le real contra la varia"le deseada un valor !ue llevara la varia"le real al valor o"tenido, es decir elimina el error2 Control PIF a utilizar Por decisi+n del e!uipo, ha"lando de la programacion decidimos no usar li"rerias !ue inclu#eran un control directo de PIF, por lo !ue lo agregamos de una forma matematica para !ue se aprecie paso a paso el funcionamiento de este # su aplicaci+n$
Figura (2.) la
-o principal en el programa es acotar el funcionamiento de este$ %s decir limitar la velocidad máima !ue en nuestro programa esta con los valores referenciales de la li"rería de los motores !ue les crea una escala de 6HH a 6HH, !ue es proporcional a una escala de volta*e !ue va desde los Lv a Lv$ -os valores negativos son a causa del puente 0 interno !ue posee nuestro controlador, # la forma de llamar la funci+n de est&, es utilizando valores negativos para o"tener un giro en direcci+n contraria a la esta"lecida por la polaridad entregada al motor Página 31
Figura (2.1) varia%les de control -=.
>plicaci+n de PIF -a integraci+n del PIF como fue mencionado$ %n esta oportunidad fue como es su epresi+n matemática correspondiente
Figura (2.2) l6neas de programación -=
/ la curva de respuesta para nuestro control en velocidad "a*a =75,7L este valor para poder o"tener una respuesta más clara de estas se'ales$
Página 32
Figura (2.2) respuesta del control -= con velocidad al 31>3*?
%sta respuesta nos entrega el comportamiento oscilatorio de nuestro sistema # !ue tam"i&n se ve refle*ado en forma visual, en la curva se puede apreciar de !ue el error toma el valor de A muchas veces, al principio se puede ver !ue es el tramo de la recta por!ue es el tramo del error menos oscilatorio # las desviaciones más grandes son las curvas, mientras más grande es el error más cerrada es la curva$
Página 33
&ontrol 34556
-as computadoras mane*an datos precisos !ue se reducen a cadenas de unos =5 # ceros =A # proposiciones !ue son ciertas # falsas$ %l cere"ro humano puede razonar con informaci+n !ue involucra incertidum"re o *uicios de valor como: 1el auto va lento2 o 1la velocidad es rápida2$ >demás, las personas tienen un sentido com.n !ue les permite razonar en un mundo donde las cosas son parcialmente ciertas$ -a l+gica difusa es una rama de la I> !ue le permite a una computadora analizar informaci+n del mundo real en una escala entre lo falso # verdadero$ -os matemáticos dedicados a la l+gica en la d&cada de 596A definieron un concepto clave: todo es cuestión de grado $ -a l+gica difusa manipula conceptos vagos como 1lento2 o 1rápido2 # permite a los ingenieros construir televisores, acondicionadores de aire, lavadores # otros dispositivos !ue *uzgan informaci+n difícil de definir$ -os sistemas difusos son una alternativa a las nociones de pertenencia # l+gica !ue se iniciaron en la Grecia antigua$ %l lengua*e natural mane*a conceptos no precisos como 1hace frío2 o 1el precio es alto2$ Cuando se traduce el lengua*e humano al conteto de la l+gica clásica se pierde la ri!ueza del significado, p&rdida !ue puede ser importante si estamos dise'ando un sistema eperto$ 4uponiendo !ue se dise'a un sistema eperto en reproducir la capacidad de diagn+stico de un m&dico, el ingeniero sa"e !ue el m&dico se "asa en medidas eactas, pero el diagn+stico # la receta de las medicinas están llenos de razonamiento difuso$ Cuando los matemáticos carecen de algoritmos !ue dictan c+mo un sistema de"e responder a ciertas entradas, la l+gica difusa puede controlar o descri"ir el sistema usando reglas de sentido com.n !ue se refieren a cantidades indefinidas$ -os sistemas difusos frecuentemente tienen reglas tomadas de epertos, pero cuando no ha# eperto los sistemas difusos adaptivos aprenden las reglas o"servando c+mo la gente manipula sistemas reales$ Para poder lograr el o"*etivo de tener la ma#or velocidad se esta"lecerán las varia"les de: Ferecha Centro Iz!uierda • • •
%sto es para sa"er en !u& posici+n se encuentra el vehículo # la compensaci+n de los motores se esta"lecerá de la siguiente forma: • • •
-ento Medio 8ápido Página 34
%n primera instancia se tra"a*ara directamente con la posici+n del vehículo, es decir con el error, para esto de los datos anteriores se o"tiene el rango del error, para lograr sus definiciones$
Figura (2.4) @s+uema del control difuso por Aatla%.
Fonde F representa la distancia, MI motor iz!uierdo # MF motor derecho, la idea principal es detectar la posici+n de los sensores so"re la línea # por medio del diferencial de velocidad de los motores para cam"iar la posici+n a medida !ue avanza$
Página 35
Figura (2.!) :aria%le de entrada del control difuso por Aatla%.
%n la figura de arri"a se muestra las varia"les lingQísticas, conde se muestra resaltada la varia"le de C !ue representa centro con un rango de U9AA HA HA 9AAV esto se de"e a !ue cuando el auto está en el centro no tiene un valor eacto por la posici+n de los sensores en la placa$ -as demás varia"les solo son para poder o"tener más reglas de control así tener una ma#or resoluci+n ante el actuar de los motores$
Figura (2.#) varia%les de salida del control difuso por Aatla%.
Página 36
%n esta figura se muestra las varia"les lingQísticas de los motores am"as salidas son iguales por ende las gráficas son las mismas$ Para estas varia"les se esta"lecen las siguientes reglas:
Figura (2.#) Reglas del control difuso por Aatla%
%n estas reglas se define principalmente si el errores A am"os motores van rápido, si eiste alguna diferencia #a sea para la iz!uierda o derecha los motores de forma independiente varia su velocidad seg.n corresponda$ %n el c+digo las funciones de mem"recía se esta"lecen con las funciones de la li"rería de control difuso de >rduino$
Figura (2.*) =nclusión de las li%rer6as
Como toda li"rería estas tienen !ue ser incluidas en el programa, en este caso las li"rerías está dividida para me*or funcionamiento$ Página 37
Figura (2.) eclaración de las funciones de mem%rec6a
%n la figura 6$@ se puede apreciar los valores incluidos desde Matla" # modificados a trav&s de la prue"a # error en las funciones de mem"recía de las salidas están con varia"les declaradas con anterioridad #a !ue am"os motores tienen !ue tener las mismas varia"les, en la configuraci+n de salida la funci+n de mem"recía de --I # --F está considerada para el drift del vehículo en el caso de !ue la curva sea mu# cerrada$
Figura (2.) e0emplo de declaración de las reglas
Página 38
%n la figura 6$9 se muestra como se configura una regla con las tres li"rerías 3uzz#8ule>ntecedent=` 3uzz#8uleConse!uent=` 3uzz#8ule=` 3uzz#8ule>ntecedent=` con esta funci+n se declara la condici+n de I3, declarada con anterioridad en Matla" =figura 6$?, donde la primera regla es : I3 FI4E>DCI> %4 IK
Figura (2.1) regla difusa declarada
Figura (2.11) Fusificación " desfusificación en código
Página 39
%n la figura 6$55 se muestra la funci+n fuzz#fi=` en la cual toma la se'al declarada con anterioridad setInput=5, error` # fusifica los valores o"tenidos, para !ue la li"rería fuzz#$c nos entregue los valores #a desfusificados por medio de la funci+n defuzzifi=`$
Por medio de la funci+n 4erial$print=` se o"tendrán los valores del error en tiempo real$ Para esta configuraci+n la respuesta fue la siguiente:
Figura (2.11) @rror de entrada difuso (distancia)
%n la respuesta anterior se muestra la respuesta oscilatoria de nuestro sistema volvi&ndolo críticamente inesta"le, en los tiempos entre HAA # 5HAA =ms se pueden ver siclos un poco más esta"les, esto se de"e por!ue el auto en líneas rectas trata de esta"ilizar su posici+n, ca"e se'alar !ue la pista ocupada para realizar las prue"as son curvas mu# cerradas # continuas$ Con esta respuesta nos damos cuenta de !ue el sistema es más oscilatorio con esa configuraci+n # la soluci+n para este pro"lema es aplicar más reglas # para aumentar las reglas necesitamos otra entrada al sistema, para esto se va a derivar el error o"teniendo tres varia"les lingQísticas$ • • •
Felta error positivo Felta error negativo Felta error cero
%stos deltas se calcularan en c+digo c o"teniendo los diferenciales del error, una vez #a tra"a*ado con el error mismo, #a !ue cada instrucci+n en c+digo c se demora A$AH us tiempo suficiente para calcular el diferencial del error actual con el anterior$ Página 40
Figura (2.12) /ontrol difuso con derivada del error
Por un tema de límite de la -i"rería difusa solo se pueden crear 9 reglas por ende las varia"les lingQísticas son:
Figura (2.13) :aria%les lingB6sticas para control difuso con dos entradas
Página 41
%n esta configuraci+n solo se discrimina si el ro"ot se encuentra ala derecha, iz!uierda o centro$
Figura (2.14) :aria%les lingB6sticas de la derivada del error
%n esta figura se puede apreciar las 7 varia"les lingQísticas !ue representan la tendencia de la curva$ P(4 representa la tendencia de la curva ascendente$ D%G representa la tendencia de la curva decreciente$ C%8( representa la tendencia de la curva a mantenerse$ / las funciones de mem"recía para la salida de los motores se mantendrán para no cam"iar los valores difusos de estos$
Página 42
Figura (2.1!) Reglas esta%lecidas para control difuso con 2 entradas
%n la figura 6$5H se logra apreciar las 9 reglas definidas por Matla" a continuaci+n el respectivo 4urface de estas condiciones$
Figura (2.1#) 5urface para motor i<+uierdo
Página 43
Figura (2.1*) 5urface para motor derec;o
%n c+digo c las funciones se ocupan de la misma forma con la diferencia !ue las condiciones cam"ian por ende en el programa se !ueda de la siguiente forma en el caso de las reglas$
Figura (2.1) reglas en código c para control difuso de dos entradas
-a forma de fusificar los valores de entrada se realizan de la misma forma !ue en el caso anterior, al igual !ue la desfusificaci+n$ -a respuesta para este sistema es al principio igual de oscilatoria pero con esta"ilizaci+n al final de la vuelta #a !ue es una recta$
Página 44
Figura (2.1) Respuesta del error con control difuso delta error
Con esta respuesta se ve !ue en las curvas el auto se comporta de forma más agresiva esto se de"e a la varia"le de freno =38 !ue cada motor tiene al final cuando el vehículo entra a la recta se ve la clara esta"ilizaci+n en la pista$
Página 45
%lgoritmo T'10%47
Charles Eremau fue un ingeniero franc&s del siglo NIN$ Wl fue el !ue invent+ este algoritmo para escapar de un la"erinto$ >ctualmente es el más eficiente, # no( &(egur& #& e(1&3&"or& de 1u&#;uer "3o de #&)ern"o (n !3or"&r (u
%l algoritmo es el siguiente: Fe alguna manera de)e!o( de r !&r1&ndo e# 1&!no ;ue e("&!o( =&1endo $ Cuando llegamos a un cruce da igual hacia donde sigamos, siempre !ue no ha#amos pasado antes por ahí$ / tam"i&n es importante !ue si llegamos a un 3un"o (n (&#d& , volvamos al cruce anterior$ 4i en dicho cruce #a hemos hecho todos los posi"les caminos, retrocedemos al cruce anterior a &ste$ / así sucesivamente$ %n resumen, este algoritmo es una
%ste m&todo es mu# sencillito, # (e u"#*& de(de =&1e 1en"o( de &@o( 3&r& (&#r de #&)ern"o( 1&"&1u!)&( o g&#er4&( (u)"erráne&( en las !ue es fácil perder la noci+n de espacio$ %l algoritmo es el siguiente: Cuando decidimos !ue !ueremos salir, 3one!o( nue("r& !&no dere1=& =tam"i&n es análogo para la iz!uierda &3o?&d& en #& 3&red, ? &nd&!o( 1on #& !&no &(4 =&("& ;ue ##egue!o( & #& (&#d& $ %s importante !ue nunca !uitemos la mano de la pared$ A(4 no( &(egur&!o( ;ue 3&(&!o( 3or "od&( #&( 3&rede(, # por tanto encontraremos la puerta !ue estará en una pared$ 4imple, ^verdad_ Pero este algoritmo tiene un par de &(3e1"o( neg&"'o($ %l primero es !ue, al igual !ue el algoritmo anterior, #& 'e#o1d&d de e(1&3&"or& de3enderá de #& (uer"e 3or #& 3o(1%n en #& ;ue e!3e1e!o($ -a otra pega, más importante todavía, es !ue 3ueden e9("r !ode#o( de #&)ern"o en e# ;ue no (e& e
Página 46
Página 47
%lgoritmo $or '18L% D1 L% 0%9O D1'1&%
^Fe !u& trata_ %n contra de la opini+n predominante, salir de un la"erinto es etremadamente sencillo` instrucciones: 5$ >po#ar la mano derecha en la pared de la derecha$ 6$ >vanzar manteniendo siempre la mano en contacto con la pared$ Más tarde o más temprano se encuentra la salida$ Pues los la"erintos son espacios c+ncavos, limitados, cerrados` los corredores, como el río Meandro, !ue parece unas veces avanzar, otras retroceder, seg.n informa (vidio, no son sino complicaciones !ue no cam"ian el carácter cerrado del espacio la"eríntico$ Fe manera !ue, recorriendo toda su pared interior, inevita"lemente se llegará a la salida$
Página 48
Figura (2.2) 9lgoritmo de resolución de la%erinto parte =
Página 49
Figura (2.21) 9lgoritmo de resolución de la%erinto parte ==
%l diagrama de flu*o muestra la forma correcta para la lectura # toma de decisiones para llegar al final del la"erinto, al poner en práctica el programa para tomar decisiones !ueda de la siguiente manera$
Página 50
Figura (2.22) algoritmo de resolución de la%erinto en código /CC
Posterior a la toma de decisiones ha# algo !ue es de "asta importancia$ Kue se genera al agregar la siguiente condici+n`
Figura (2.23) condición de final de la%erinto
Fonde 1v2 es un valor previamente medido del color verde, !ue al marcar todos en verde genera el recorte del recorrido para volver por el tramo corto del la"erinto$
Página 51
Figura (2.24) función de arreglo
> lo largo del recorrido cada curva fue remplazada por el ángulo de giro correspondiente es decir si do"lo a la derecha se marca como 9AY -a simplificaci+n de la ruta de regreso toma como referencia la suma de los grados para o"tener una respuesta de acci+n
Página 52
Figura (2.2!) función arreglo completo
4i lo e*emplificamos de una manera más gráfica, Fespu&s de girar 9A Y a la derecha, 5@A en la vuelta, # 9A Y de nuevo a la derecha =9AR5@AR9A7?A , el efecto es !ue el ro"ot se dirige en la direcci+n original de nuevo$ -a ruta puede ser simplificada con giro de A Y: un .nico 4Z !ue significa
Página 53
!ue
siga
su
camino
derecho$
Figura (2.2#) representación del arreglo
C&34"u#o 1; al ua ci ón 1c on óm ic a
In
%ste pro#ecto a causa de !ue fue dise'ado a modo de 1estudio2 o 1competencia2, la pro#ecci+n de venta no es aplica"le de manera directa, a menos !ue se piense como algo !ue no es$ %s decir un pro#ecto pensado para el mercado$ >un así, es destaca"le !ue el programa del 1)elociraptor2 si es aplica"le a distintos campos comerciales$ > grandes rasgos esto se puede aplicar en sectores !ue usen vehículos para tramos definidos, con la finalidad de me*orar tiempos de e*ecuci+n en transporte de materias primas$ 2#u$o de 1&$&
Ea"la 7$5 ta"la de costos )ER*+,+ME- 1
*"gresos
Página 54
. 180/000
*"vers!$" *"!!al F!"a"!am!e"to otal *"gresos Egresos H!"#!$ )laa o"etores
a "t
ME- 1
1 15
R#edas
2
-e"sor
1
Ba'y +ra"g#ta"
1
)rogramador AR
1
Batera *)+
2
argador Batera
1
Motor
2
-oporte Motor
2
E"v!$ a h!le !!HH
1
Ma"o de +'ra 7hrs otal Egresos +A )R+:E+
20 0
. . 5/000 5/000 . . 500 /500 . 8/000 . 16/000 . . /000 /000 . 15/000 . 15/000 . 15/000 . 15/000 . /000 . 14/000 . 30/000 . 30/000 . 10/000 . 20/000 . 5/000 . 10/000 . 30/000 . 30/000 valor F . 25/600 . 5/120/000 . 5/289/500 . 5/469/500
C&34"u#o &onclusión y %nálisis
En 1o!3e"en1&
Página 55
%n la competencia principal 1competencia de ro"+tica 6A5H de la universidad 3ederico 4anta María2 se logr+ una gran participaci+n en los tres días #a !ue el )elociraptor se mantuvo en primer lugar, esto se de"e a las me*oras de cali"raci+n !ue se hiso en el control$
Figura (4.) ta%la de tiempos generales
Figura (4.1) ta%la de factor de competencia
Página 56
Figura (4.2) ta%la final de posiciones
Página 57
Aná#((
>l apreciar el pro#ecto a nivel de estudio, fuera de la competencia, ca"e destacar su amplia capacidad para mostrar de una manera física la diferencia entre cada tipo de programaci+n disponi"le para aplicar a los ro"ot seguidores de línea$ 4i se aplica para la pista un control (D(33, es funcional, pero lleva consigo limitaciones de velocidad considera"les, a causa de un muestreo no constante !ue espera salir de la línea para corregir el error$ >l aplicar un control PIF, me*ora considera"lemente # es visi"le a primera instancia, #a !ue su velocidad puede aumentar lo suficiente sin perder esta"ilidad$ >un así, luego de una cierta cantidad de vueltas =varia"les seg.n la luz solar, llega a un punto de esta"ilidad crítica, perdiendo de forma "rusca el recorrido$ > diferencia de am"os controles anteriores el control 3</ se hace notar, por su velocidad de reacci+n$ Curvas veloces # precisas$
Página 58
Con1#u(%n
Fe una manera eclu#ente el hecho de tra"a*ar con @ sensores =anteriormente se tra"a*+ con , es fundamental tener una lectura correcta de la pista para tener una medida proporcional de esta, una me*ora considera"le en cuanto a la respuesta en curvas # la esta"ilidad en pista a ma#or velocidad !ue la de prue"a$ %s decir, dentro de un rango de A a 6HH =escala !ue refle*a el volta*e aplicado al motor de Av a Lv, pasamos de tra"a*ar a ?A a una velocidad de 5LA como límite en la esta"ilidad$ %l "a*o peso del prototipo de competencia en instancias genero poca adherencia a la pista, tema !ue fue solucionado me*orando la posici+n de los materiales generando un punto de esta"ilidad certera # funcional$ %l peso final fueron 5Lgr$ %n las curvas cuando son mu# cerradas, el control PIF no es mu# eficiente #a !ue al superar la velocidad de ?6,7 el ro"ot se salía de la pista al contrario del control difuso #a !ue se logr+ llegar a la velocidad LA, o"teníamos un seguimiento en las curvas mu# "ueno pero en líneas rectas se demora"a 6,5 segundos más en esta"ilizarse !ue el control PIF, lo cual se o"serv+, !ue el control PIF el diferencial de velocidad de los motores es menor respecto al control difuso propuesto$
Página 59
C&34"u#o 0 'e fe re nc ia s
Págn&( e)
^Ku& es el control PIF_ ;ttpsDDfranElinelinEmx.ordpress.comD213DD!D+ueeselcontrolpidD
Compra de materiales ;ttpsDD.pololu.comD
>lumnos del rea %lectricidad # %lectr+nica de ID>C>P 4antiago 4ur destacan con innovador pro#ecto en distintas competencias$ ;ttpDD.inacap.clDtportalvpDportalDnoticiasDnoticiasportalDalumnosdelareaelectricidad" electronicadeinacapsantiagosurdestacanconinnovadorpro"ectoendistintascompetencias
>lumnos de ID>C>P 4antiago 4ur logran una destacada participaci+n en la 1NIII Competencia de 8o"+tica2 ;ttpDD.inacap.clDtportalvpDsantiagosurDnoticiasDnoticiasstgosurDalumnosdeinacapsantiago surlogranunadestacadaparticipacionenlaxiiicompetenciadero%otica
Competencia de ro"+tica 6A5H http:$competenciaro"otica$cl6A5H Punta*es de la competencia https:docs$google$comspreadsheetsd5Kz@c<>vAB"!*<9fKr9 4BI4-)LKMcmm8?
Página 60
Página 61
%n e< o
Segudor de L4ne& 1on PD
Para comenzar el programa, es imprescindi"le agregar las li"rerías a ocupar %n este caso las llamamos de esta manera include KE84ensors$hj -i"rería de los sensores análogos include (rangutanMotors$hj -i"rería interna del Puente en 0 define D
%l máimo de velocidad a alcanzar )alor proporcional )alor integral )elocidad de cali"raci+n
Página 62
>ntes de comenzar el programa necesitamos declarar el )oid setup de manera adecuada para definir pines de tra"a*o, velocidad de transmisi+n de datos seriales # cali"raci+n void setup= 4erial$"egin =9?AA` int start 5A` int val A` pinMode=9, (
Comienza el ciclo del 4etup )elocidad de comunicaci+n serial Contador para cali"raci+n pin 9 # @ Feclarado como salida pin 5A declarado Como entrada discreta
val digital8ead=start` hile =val 0IG0 digitalrite=9, 0IG0` digitalrite=@, 0IG0` val digital8ead=start` ` if =val -( digitalrite=9, -(` digitalrite=@, -(` (rangutanMotors::set4peeds=A, A` Motores detenidos Instrucciones para %mpezar a hacer la Cali"raci+n de 4ensores dela#=5HAA` digitalrite=9, 0IG0` digitalrite=@, 0IG0` %nciende el leds para indicar !ue se está cali"rando$ for =int counter A` counter 65` counterRR if =counter ? counter j 5H (rangutan Motors::set4peeds=velcali"rate, velcali"rate` else (rangutanMotors::set4peeds=velcali"rate, velcali"rate` !tra$cali"rate=` dela#=6A` digitalrite=9, -(` digitalrite=@, -(`
>paga el led para indicar !ue se termin+ la cali"raci+n$ Página 63
(rangutanMotors::set4peeds=A, A` dela#=6AA` digitalrite=9, 0IG0` digitalrite=@, 0IG0` dela#=6AA` digitalrite=9, -(` Parpadeo para indicar !ue el ro"ot está listo$ digitalrite=@, -(` dela#=6AA` Parpadeo para indicar !ue el ro"ot está listo$ digitalrite=9, 0IG0` digitalrite=@, 0IG0` Parpadeo para indicar !ue el ro"ot está listo$ dela#=6AA` digitalrite=9, -(` Parpadeo para indicar !ue el ro"ot está listo$ digitalrite=@, -(` dela#=6AA` 3in Cali"raci+n 4ensores pinMode=start,IDP
Página 64
de
Para comenzar a tra"a*ar con un control PIF necesitamos o"tener una derivada del error$ unsigned int lastkerror A` long integral A` void loop= int position !tra$read-ine=sensor)alues` leemos posici+n de la línea en la varia"le position 4erial$println=position` 8eferencia donde seguirá la línea, mitad sensores$ int error position 6HAA` Cálculos PF int derivative error lastkerror` integral R error` lastkerror error` int poerkdifference error T )Propocional R integral )Integral R derivative T )Ferivarivo` if =poerkdifference j maimum poerkdifference maimum` if =poerkdifference maimum poerkdifference maimum` if =poerkdifference A (rangutanMotors::set4peeds=maimum, maimum R poerkdifference` digitalrite=9, -(` digitalrite=@, 0IG0` else (rangutanMotors::set4peeds=maimum poerkdifference, maimum` digitalrite=9, 0IG0` digitalrite=@, -(`
Página 65
L%g1& d
include KE84ensors$hj include (rangutanMotors$hj include 3uzz#$hj include 3uzz#Composition$hj include 3uzz#Input$hj include 3uzz#I($hj include 3uzz#(utput$hj include 3uzz#8ule$hj include 3uzz#8ule>ntecedent$hj include 3uzz#8uleConse!uent$hj include 3uzz#4et$hj TTTTTTTTT Lmo IDE%DE( F% -(GIC> FI3<4> TTTTTTTTTTTTTTTT TTTTTTTTT %NIE(bbbbb TTTTTTTTTTTTTTTTT TP>8>M%EI>CI(D F% 4%D4(8 F% -ID%>T define D
int fLA` int gLH` int hL@` int i@A` Tiniciacion del 3</T 3uzz#T fuzz# ne 3uzz#=` inicializando un o"*eto en "i"lioteca entrada distancia 3uzz#4etT FFF ne 3uzz#4et=6HAA, 6HAA, 6AAA, 5AAA`varia"le linguistica iz!uierdo 3uzz#4etT dd ne 3uzz#4et=5HAA, 5AAAR6AA, 5AAAR6AA, HAAR6AA` 3uzz#4etT F%8 ne 3uzz#4et=5AAAR6AA, HAAR6AA, HAAR6AA, A` 3uzz#4etT C%D ne 3uzz#4et=HA, A, A, HA`varia"le linguistica centro 3uzz#4etT I ne 3uzz#4et=A, HAA6AA, HAA6AA, 5AAA6AA` 3uzz#4etT ii ne 3uzz#4et=HAA6AA, 5AAA6AA, 5AAA6AA, 5HAA` 3uzz#4etT III ne 3uzz#4et=5AAA, 6AAA, 6HAA, 6HAA`varia"le linguistica derecho 4>-IF> M(E(8 IK
void setup= 4erial$"egin=55H6AA` int inPin 5A` int val A` pinMode=9, (
val digital8ead=inPin` hile =val 0IG0 digitalrite=9, 0IG0`-%F F% E%4E%( digitalrite=@, 0IG0`-%F F% E%4E%( val digital8ead=inPin` ` if =val -( digitalrite=9, -(`-%F F% E%4E%( digitalrite=@, -(` -%F F% E%4E%( (rangutanMotors::set4peeds=A,A` Motores detenidos Instrucciones para %mpezar 4ensores
a
hacer
la
Cali"raci+n
dela#=5AAA` digitalrite=9, 0IG0`-%F F% E%4E%( digitalrite=@, 0IG0` %nciende el leds para indicar !ue se está cali"rando$ for =int counterA` counter65` counterRR if =counter ? counter j 5H (rangutanMotors::set4peeds=velcali"rate, velcali"rate` else (rangutanMotors::set4peeds=velcali"rate, velcali"rate` !tra$cali"rate=` dela#=6A` Figitalrite=9, -(` >paga el led para indicar !ue se termin+ la cali"raci+n$ digitalrite=@, -(` -%F F% E%4E%( (rangutanMotors::set4peeds=A, A` dela#=6AA` digitalrite=9, 0IG0` -%F F% E%4E%( digitalrite=@, 0IG0`-%F F% E%4E%( dela#=6AA` digitalrite=9, -(` Parpadeo para indicar !ue el ro"ot está listo$ digitalrite=@, -(`-%F F% E%4E%( dela#=6AA` Parpadeo para indicar !ue el ro"ot está listo$ digitalrite=9, 0IG0` -%F F% E%4E%( Página 68
de
digitalrite=@, 0IG0` Parpadeo para indicar !ue el ro"ot está listo$ dela#=6AA` digitalrite=9, -(` Parpadeo para indicar !ue el ro"ot está listo$ digitalrite=@, -(`-%F F% E%4E%( dela#=6AA` 3in Cali"raci+n de 4ensores dela#=5AAA` 8etardo N segundos antes de %mpezar a andar T IDICI>-I>CI(D F% )>8I>B-%4 / 8%G->4 F%- 3</ T creando las varia"les lingQísticas 3uzz#InputT distancia ne 3uzz#Input=5` creando el sete del fuzz# en relaci+n a la distancia distanciajadd3uzz#4et=I`agregar varia"le linguistica a distancia distanciajadd3uzz#4et=FFF` distanciajadd3uzz#4et=C%D`agregar varia"le linguistica a distancia distanciajadd3uzz#4et=III` distanciajadd3uzz#4et=F%8`agregar varia"le linguistica a distancia distanciajadd3uzz#4et=ii` distanciajadd3uzz#4et=dd` fuzz#jadd3uzz#Input=distancia` TDo tenemos segunda varia"le de entrada aun!ue podemos incluir un >celer+metro para tener me*or sensi"ilidad en la pista T T Creaci+n de la varia"le de salida T 3uzz#(utputT M(E(8I ne 3uzz#(utput=5` M(E(8Ijadd3uzz#4et=--I` M(E(8Ijadd3uzz#4et=-I` M(E(8Ijadd3uzz#4et=MI` M(E(8Ijadd3uzz#4et=8I` M(E(8Ijadd3uzz#4et=88I` fuzz#jadd3uzz#(utput=M(E(8I` T Creaci+n de la segunda varia"le de salida T Página 69
3uzz#(utputT M(E(8F ne 3uzz#(utput=6` M(E(8Fjadd3uzz#4et=--F` M(E(8Fjadd3uzz#4et=-F` M(E(8Fjadd3uzz#4et=MF` M(E(8Fjadd3uzz#4et=8F` M(E(8Fjadd3uzz#4et=88F` fuzz#jadd3uzz#(utput=M(E(8F` T Creaci+n de las reglas C>F> %DE8>F> EI%D%
Página 70
ne
3uzz#8ule=6,
ifdistanciaC%D,
([ AAAAA7 3uzz#8ule>ntecedentT ifdistanciaF%8 ne 3uzz#8ule>ntecedent=` ifdistanciaF%8j*oin4ingle=F%8` 3uzz#8uleConse!uentT thenM(E(8I8I>ndM(E(8F-F ne 3uzz#8uleConse!uent=` thenM(E(8I8I>ndM(E(8F-Fjadd(utput=8I` thenM(E(8I8I>ndM(E(8F-Fjadd(utput=-F` 3uzz#8uleT fuzz#8uleA7 ne 3uzz#8ule=7, ifdistanciaF%8, thenM(E(8I8I>ndM(E(8F-F` fuzz#jadd3uzz#8ule=fuzz#8uleA7` ([ AAAAA 3uzz#8ule>ntecedentT ifdistanciaFFF ne 3uzz#8ule>ntecedent=` ifdistanciaFFFj*oin4ingle=FFF` 3uzz#8uleConse!uentT thenM(E(8IMI>ndM(E(8F--F ne 3uzz#8uleConse!uent=` thenM(E(8IMI>ndM(E(8F--Fjadd(utput=MI` thenM(E(8IMI>ndM(E(8F--Fjadd(utput=--F` 3uzz#8uleT fuzz#8uleA thenM(E(8IMI>ndM(E(8F--F` fuzz#jadd3uzz#8ule=fuzz#8uleA`
ne
3uzz#8ule=,
ifdistanciaFFF,
([ AAAAAH 3uzz#8ule>ntecedentT ifdistanciaIII ne 3uzz#8ule>ntecedent=` ifdistanciaIIIj*oin4ingle=III` 3uzz#8uleConse!uentT thenM(E(8I--I>ndM(E(8FMF ne 3uzz#8uleConse!uent=` thenM(E(8I--I>ndM(E(8FMFjadd(utput=--I` thenM(E(8I--I>ndM(E(8FMFjadd(utput=MF` 3uzz#8uleT fuzz#8uleAH ne 3uzz#8ule=H, ifdistanciaIII, thenM(E(8I--I>ndM(E(8FMF` Pass"and o >ntecedent e o Conse!uent da epression fuzz#jadd3uzz#8ule=fuzz#8uleAH` >ficionado o 3uzz#8ule au o"*ector 3uzz# ([ AAAAA? Página 71
3uzz#8ule>ntecedentT ifdistanciaii ne 3uzz#8ule>ntecedent=` ifdistanciaiij*oin4ingle=ii` 3uzz#8uleConse!uentT thenM(E(8IMI>ndM(E(8F8F ne 3uzz#8uleConse!uent=` thenM(E(8IMI>ndM(E(8F8Fjadd(utput=MI` thenM(E(8IMI>ndM(E(8F8Fjadd(utput=8F` 3uzz#8uleT fuzz#8uleA? ne 3uzz#8ule=?, ifdistanciaii, thenM(E(8IMI>ndM(E(8F8F` Pass"and o >ntecedent e o Conse!uent da epression fuzz#jadd3uzz#8ule=fuzz#8uleA?` >ficionado o 3uzz#8ule au o"*ector 3uzz# ([ AAAAAL 3uzz#8ule>ntecedentT ifdistanciadd ne 3uzz#8ule>ntecedent=` ifdistanciaddj*oin4ingle=dd` 3uzz#8uleConse!uentT thenM(E(8I8I>ndM(E(8FMF ne 3uzz#8uleConse!uent=` thenM(E(8I8I>ndM(E(8FMFjadd(utput=8I` thenM(E(8I8I>ndM(E(8FMFjadd(utput=MF` 3uzz#8uleT fuzz#8uleAL ne 3uzz#8ule=L, ifdistanciadd, thenM(E(8I8I>ndM(E(8FMF` Pass"and o >ntecedent e o Conse!uent da epression fuzz#jadd3uzz#8ule=fuzz#8uleAL` >ficionado o 3uzz#8ule au o"*ector 3uzz# ([ void loop= T 8%)I4>8 -> )>8I>B-% F% -(4 4%D4(8%4 >
4erial$println=error` dis position` fuzz#jsetInput=5, error` fuzz#jfuzzif#=` T 4erial$print= iz!uierdo: ` 4erial$print=IjgetPertinence=` 4erial$print= centro: ` 4erial$print=C%DjgetPertinence=` 4erial$print= derecho: ` 4erial$println=F%8jgetPertinence=` T float motorI fuzz#jdefuzzif#=5` float motorF fuzz#jdefuzzif#=6` (rangutanMotors::set4peeds=motorI, motorF` if=motorIjmotorFdigitalrite=9, 0IG0`digitalrite=@, -(` else digitalrite=@, 0IG0`digitalrite=9, -(` if=motorImotorFdigitalrite=9, 0IG0`digitalrite=@, 0IG0` ` L%g1& d
include KE84ensors$hj include (rangutanMotors$hj include 3uzz#$hj include 3uzz#Composition$hj include 3uzz#Input$hj include 3uzz#I($hj include 3uzz#(utput$hj include 3uzz#8ule$hj include 3uzz#8ule>ntecedent$hj include 3uzz#8uleConse!uent$hj include 3uzz#4et$hj TTTTTTTTT Lmo IDE%DE( F% -(GIC> FI3<4> TTTTTTTTTTTTTTTT TTTTTTTTT %NIE(bbbbb TTTTTTTTTTTTTTTTT Página 73
TP>8>M%EI>CI(D F% 4%D4(8 F% -ID%>T define D
void setup= 4erial$"egin=55H6AA` int inPin 5A` int val A` pinMode=9, (
a
hacer
la
Cali"racion
dela#=5AAA` digitalrite=9, 0IG0`-%F F% E%4E%( digitalrite=@, 0IG0` %nciende el leds para indicar !ue se está cali"rando$ for =int counterA` counter65` counterRR if =counter ? counter j 5H (rangutanMotors::set4peeds=velcali"rate, velcali"rate` else (rangutanMotors::set4peeds=velcali"rate, velcali"rate` !tra$cali"rate=` dela#=6A` digitalrite=9, -(` >paga el led para indicar !ue se termin+ la cali"raci+n$ digitalrite=@, -(` -%F F% E%4E%( Página 75
de
(rangutanMotors::set4peeds=A, A` dela#=6AA` digitalrite=9, 0IG0` -%F F% E%4E%( digitalrite=@, 0IG0`-%F F% E%4E%( dela#=6AA` digitalrite=9, -(` Parpadeo para indicar !ue el ro"ot está listo$ digitalrite=@, -(`-%F F% E%4E%( dela#=6AA` Parpadeo para indicar !ue el ro"ot está listo$ digitalrite=9, 0IG0` -%F F% E%4E%( digitalrite=@, 0IG0` Parpadeo para indicar !ue el ro"ot está listo$ dela#=6AA` digitalrite=9, -(` Parpadeo para indicar !ue el ro"ot está listo$ digitalrite=@, -(`-%F F% E%4E%( 3in Cali"racion de 4ensores dela#=5AAA` 8etardo N segundos antes de %mpezar a andar T IDICI>-I>CI(D F% )>8I>B-%4 / 8%G->4 F%- 3</ T creando las varia"les lingQísticas 3uzz#InputT %88 ne 3uzz#Input=5` creando el sete del fuzz# en relaci+n a la distancia %88jadd3uzz#4et=M%D` %88jadd3uzz#4et=C%D` %88jadd3uzz#4et=M>/` fuzz#jadd3uzz#Input=%88` 3uzz#InputT F%88 ne 3uzz#Input=6` F%88jadd3uzz#4et=D%G` F%88jadd3uzz#4et=C%8(` F%88jadd3uzz#4et=P(4` fuzz#jadd3uzz#Input=F%88`
Tno tenemos segunda varia"le de entrada aun!ue podemos incluir un aceler+metro para tener me*or sensi"ilidad en la pista T T creaci+n de la varia"le de salida T 3uzz#(utputT M(E(8I ne 3uzz#(utput=5` Página 76
M(E(8Ijadd3uzz#4et=38I` M(E(8Ijadd3uzz#4et=MI` M(E(8Ijadd3uzz#4et=>CCI` fuzz#jadd3uzz#(utput=M(E(8I` T Creaci+n e la segunda varia"le de salida T 3uzz#(utputT M(E(8F ne 3uzz#(utput=6` M(E(8Fjadd3uzz#4et=38F` M(E(8Fjadd3uzz#4et=MF` M(E(8Fjadd3uzz#4et=>CCF` fuzz#jadd3uzz#(utput=M(E(8F` T creaci+n de las reglas C>F> %DE8>F> EI%D%
ne
3uzz#8ule=5,
if%88M>/>ndF%88P(4,
([ AAAAA6 3uzz#8ule>ntecedentT if%88C%D>ndF%88P(4 ne 3uzz#8ule>ntecedent=` if%88C%D>ndF%88P(4j*oinith>DF=C%D,P(4` 3uzz#8uleConse!uentT thenM(E(8I38I>ndM(E(8F>CCF ne 3uzz#8uleConse!uent=` Página 77
thenM(E(8I38I>ndM(E(8F>CCFjadd(utput=38I` thenM(E(8I38I>ndM(E(8F>CCFjadd(utput=>CCF` 3uzz#8uleT fuzz#8uleA6 thenM(E(8I38I>ndM(E(8F>CCF` fuzz#jadd3uzz#8ule=fuzz#8uleA6`
ne
3uzz#8ule=6,
if%88C%D>ndF%88P(4,
([ AAAAA7 3uzz#8ule>ntecedentT if%88M%D>ndF%88P(4 ne 3uzz#8ule>ntecedent=` if%88M%D>ndF%88P(4j*oinith>DF=M%D,P(4` T3uzz#8uleConse!uentT thenM(E(8I38I>ndM(E(8F>CCF ne 3uzz#8uleConse!uent=` thenM(E(8I38I>ndM(E(8F>CCFjadd(utput=38I` thenM(E(8I38I>ndM(E(8F>CCFjadd(utput=>CCF` T 3uzz#8uleT fuzz#8uleA7 ne 3uzz#8ule=7, if%88M%D>ndF%88P(4, thenM(E(8I38I>ndM(E(8F>CCF` fuzz#jadd3uzz#8ule=fuzz#8uleA7` ([ AAAAA 3uzz#8ule>ntecedentT if%88M%D>ndF%88C%8( ne 3uzz#8ule>ntecedent=` if%88M%D>ndF%88C%8(j*oinith>DF=M%D,C%8(` T 3uzz#8uleConse!uentT thenM(E(8I38I>ndM(E(8F>CCF ne 3uzz#8uleConse!uent=` thenM(E(8I38I>ndM(E(8F>CCFjadd(utput=38I` thenM(E(8I38I>ndM(E(8F>CCFjadd(utput=>CCF` T 3uzz#8uleT fuzz#8uleA ne 3uzz#8ule=, if%88M%D>ndF%88C%8(, thenM(E(8I38I>ndM(E(8F>CCF` fuzz#jadd3uzz#8ule=fuzz#8uleA` ([ AAAAAH 3uzz#8ule>ntecedentT if%88M%D>ndF%88D%G ne 3uzz#8ule>ntecedent=` if%88M%D>ndF%88D%Gj*oinith>DF=M%D,D%G` T 3uzz#8uleConse!uentT thenM(E(8I38I>ndM(E(8F>CCF ne 3uzz#8uleConse!uent=` thenM(E(8I38I>ndM(E(8F>CCFjadd(utput=38I` thenM(E(8I38I>ndM(E(8F>CCFjadd(utput=>CCF` T Página 78
3uzz#8uleT fuzz#8uleAH ne 3uzz#8ule=H, thenM(E(8I38I>ndM(E(8F>CCF` fuzz#jadd3uzz#8ule=fuzz#8uleAH` ([
if%88M%D>ndF%88D%G,
AAAAA? 3uzz#8ule>ntecedentT if%88C%D>ndF%88D%G ne 3uzz#8ule>ntecedent=` if%88C%D>ndF%88D%Gj*oinith>DF=C%D,D%G` T 3uzz#8uleConse!uentT thenM(E(8I>CCI>ndM(E(8F38F ne 3uzz#8uleConse!uent=` thenM(E(8I>CCI>ndM(E(8F38Fjadd(utput=>CCI` thenM(E(8I>CCI>ndM(E(8F38Fjadd(utput=38F` T 3uzz#8uleT fuzz#8uleA? ne 3uzz#8ule=?, if%88C%D>ndF%88D%G, thenM(E(8I>CCI>ndM(E(8F38F` fuzz#jadd3uzz#8ule=fuzz#8uleA?` ([ AAAAAL 3uzz#8ule>ntecedentT if%88M>/>ndF%88D%G ne 3uzz#8ule>ntecedent=` if%88M>/>ndF%88D%Gj*oinith>DF=M>/,D%G` T 3uzz#8uleConse!uentT thenM(E(8I>CCI>ndM(E(8F38F ne 3uzz#8uleConse!uent=` thenM(E(8I>CCI>ndM(E(8F38Fjadd(utput=>CCI` thenM(E(8I>CCI>ndM(E(8F38Fjadd(utput=38F` T 3uzz#8uleT fuzz#8uleAL ne 3uzz#8ule=L, if%88M>/>ndF%88D%G, thenM(E(8I>CCI>ndM(E(8F38F` fuzz#jadd3uzz#8ule=fuzz#8uleAL` ([ AAAAA@ 3uzz#8ule>ntecedentT if%88M>/>ndF%88C%8( ne 3uzz#8ule>ntecedent=` if%88M>/>ndF%88C%8(j*oinith>DF=M>/,C%8(` T 3uzz#8uleConse!uentT thenM(E(8I>CCI>ndM(E(8F38F ne 3uzz#8uleConse!uent=` thenM(E(8I>CCI>ndM(E(8F38Fjadd(utput=>CCI` thenM(E(8I>CCI>ndM(E(8F38Fjadd(utput=38F` T 3uzz#8uleT fuzz#8uleA@ ne 3uzz#8ule=@, if%88M>/>ndF%88C%8(, thenM(E(8I>CCI>ndM(E(8F38F` fuzz#jadd3uzz#8ule=fuzz#8uleA@` ([ Página 79
AAAAA9 3uzz#8ule>ntecedentT if%88C%D>ndF%88C%8( ne 3uzz#8ule>ntecedent=` if%88C%D>ndF%88C%8(j*oinith>DF=C%D,C%8(` 3uzz#8uleConse!uentT thenM(E(8IMI>ndM(E(8FMF ne 3uzz#8uleConse!uent=` thenM(E(8IMI>ndM(E(8FMFjadd(utput=MI` thenM(E(8IMI>ndM(E(8FMFjadd(utput=MF` 3uzz#8uleT fuzz#8uleA9 thenM(E(8IMI>ndM(E(8FMF` fuzz#jadd3uzz#8ule=fuzz#8uleA9`
ne
3uzz#8ule=9,
if%88C%D>ndF%88C%8(,
([ void loop= T 8%)I4>8 -> )>8I>B-% F% -(4 4%D4(8%4 >
float motorF fuzz#jdefuzzif#=5` float motorI fuzz#jdefuzzif#=6` (rangutanMotors::set4peeds=motorI, motorF` if=motorIjmotorFdigitalrite=9, 0IG0`digitalrite=@, -(` else digitalrite=@, 0IG0`digitalrite=9, -(` if=motorImotorFdigitalrite=9, 0IG0`digitalrite=@, 0IG0` `
A#gor"!o #&)ern"o
include (rangutanMotors$hj const int estandar HA, giro 5@` char dir` const int a5pin >A`int a5 A` const int a6pin >5`int a6 A` const int a7pin >6`int a7 A` const int apin >7`int a A` const int aHpin >`int aH A` const int a?pin >H`int a? A` int s5, s6, s7, s, sH, s?` int contA, cIKA, cF%8A, c)A, cG%DA, negro 7AA, listo A` Medida de lectura del negro =depende la luz del am"iente char pathU5AAV ` unsigned char pathklength A` unsigned char foundkleft, foundkstraight, foundkright` void seguidor= contA` cF%8A` cIKA` if =s5 A sH A =s6 5 s 5 (rangutanMotors::set4peeds=estandar,estandar` derecho digitalrite=9, 0IG0`digitalrite=@, 0IG0` if =s5 5 sH A =s6 5 s 5 (rangutanMotors::set4peeds=estandar,A`derecha Página 81
digitalrite=@, 0IG0`digitalrite=9, 0IG0`digitalrite=9, -(` if =s5 A sH 5 =s6 5 s 5 (rangutanMotors::set4peeds=A,estandar` iz!uierda digitalrite=@, -(`digitalrite=9, -(`digitalrite=9, 0IG0` void iz!= if=cont A (rangutanMotors::set4peeds=5A6A6A7A, (rangutanMotors::set4peeds=5A6A6A 7A, 6A5A6A7A` detener (rangutanMotors::set4peeds=giro, giro` dela#=6AAHA5A` contcontR5` cIKcIKR5` cG%DcG%DR5` void der= if=cont A (rangutanMotors::set4peeds=5A7A6A7A5A, (rangutanMotors::set4peeds=5A7A6A 7A5A, 5A7A6A7A5A`detener (rangutanMotors::set4peeds=giro, giro` dela#=5AA6A5A7A` (rangutanMotors::set4peeds=giro, giro` dela#=6AA` cF%8cF%8R5` cG%DcG%DR5` void final=(rangutanMotors::set4peeds=A,A`dela#=5AAA` digitalrite=9, 0IG0` digitalrite=@, 0IG0` dela#=6AA` digitalrite=9, -(` Parpadeo para indicar !ue el ro"ot está listo$ digitalrite=@, -(` dela#=6AA` Parpadeo para indicar !ue el ro"ot está listo$ digitalrite=9, 0IG0` digitalrite=@, 0IG0` Parpadeo para indicar !ue el ro"ot está listo$ dela#=6AA` digitalrite=9, -(` Parpadeo para indicar !ue el ro"ot está listo$ Página 82
digitalrite=@, -(` dela#=6AA` void vuelta= if=cF%8 A cIK A if=cont A (rangutanMotors::set4peeds=5A7A7AALAHA, (rangutanMotors::set4peeds=5A7A7A ALAHA, 5A7A7AALAHA`detener 5A7A7AALAHA `detener dela#=H` (rangutanMotors::set4peeds=giro, giro` dela#=5A` (rangutanMotors::set4peeds= giro5A,giro` dela#=6@A` contcontR5` c)c)R5` cG%DcG%DR5` void direccion= d sitch =dir case i: iz!=` "rea]` case d: der=` "rea]` case l: seguidor=` "rea]` case v: vuelta=` "rea]` case f: final=` "rea]` Página 83
void turn=char dirfunci+n de direcci+n sitch =dir case -:vuelta iz!uierda iz!=` "rea]` case 8: vuelta derecha der=` "rea]` case B:vuelta en 5@AY vuelta=` "rea]` case 4: no hace nada "rea]` void lectura= a5 ana analo log8 g8ea ead= d=a5 a5pi pin n`` a6 anal analog og8e 8ead ad=a =a6p 6pin in` ` a7 ana analo log8 g8ea ead= d=a7 a7pi pin n`` analog8ead=apin` aH analog8ead=aHpin` a? analog8ead=a?pin` if =a5 jnegro s5 5` elses5 A` if =a6 jnegro s6 5` elses6 A` if =a7 jnegro s7 5` elses7 A` if =a jnegro s 5` elses A` if =aH jnegro sH 5` elsesH A`
der 4%G
seguir línea if =s6 A s A = s7 5 dir l` else if = sH 5 s 5 s7 5 s6 5 s5 5 dir d`t else if = sH A s A s7 A s6 A s5 A dir v`final de línea else if = sH 5 s 5 s7 5 s6 5 s5 5 dir f`
derecha if = sH 5 s 5 s7 5 s6 5 s5 5 s? 5 dir d` else if =s6 A sH A s7 5 dir l` else if =s5 5 s6 5 s7 5 sHA s?A dir d` Página 84
a
iz!uierda if =s? 5 sH 5 s5 s6 A s5 A dir i` iz!uierda Get the angle as a num"er "eteen A and 7?A degrees$ totalkangle totalkangle 7?A` 8eplace all of those turns ith ith a single one$ sitch=totalkangle case A: pathUpathklength 7V 4` "rea]` case 9A: pathUpathklength 7V 8` "rea]` case 5@A: pathUpathklength 7V B` "rea]` case 6LA: pathUpathklength 7V -` "rea]` pathklength 6` char selectkturn=unsigned char foundkleft, unsigned char foundkstraight, unsigned char foundkright esta funci+n toma la la decisi+n de donde seguir la línea del la"erinto if =foundkleft return -` else if=foundkstraight return 4` else if=foundkright return 8` else return B` estos valores se usan en la funci+n turn void setup= Página 85
4erial$"egin=9?AA` int start 5A` int val A` pinMode=9, (