UNIVERSIDAD TCNICA DE AMBATOFACULTAD DE INGENIERA EN SISTEMAS,
ELECTRNICA E INDUSTRIAL PERODO ACADMICO: OCTUBRE/2014
MARZO/2015
UNIVERSIDAD TCNICA DE AMBATO
Facultad de Ingeniera en Sistemas, Electrnica e
IndustrialCarrera de Ingeniera Industrial en Procesos de
Automatizacin
Ttulo: APLICACIN DE SISTEMAS DIGITALES Y ARDUINO EN LA CREACIN
DE UN ROBOT SEGUIDOR DE LNEA
Carrera:INGENIERA INDUSTRIAL EN PROCESOS DE AUTOMATIZACIN
rea Acadmica:ELECTRONICA
Lnea de Investigacin:AUTOMATIZACION
Ciclo Acadmico y Paralelo:QUINTO INDUSTRIAL
Alumnos participantes:
Fabian BaoDiego LpezJimmy SangolquizaLeonardo VlezDiego
DomnguezDavid LemaCarlos GalarzaBladimir ChamorroOscar Pillalvaro
Guamn.
Mdulo y Docente:Ing. Santiago Collantes.Electrnica Digital.
I. INFORME DEL PROYECTO1.1 Ttulo APLICACIN DE SISTEMAS DIGITALES
Y ARDUINO EN LA CREACIN DE UN ROBOT SEGUIDOR DE LNEA
Objetivos
Crear un robot seguidor de lnea utilizando las bases
fundamentales de la electrnica digital al igual que la plataforma
de desarrollo arduino. Detallar el uso, aplicacin y relacin entre
diferentes plataformas de desarrollo electrnico con la electrnica
digital y analgica moderna.1.2 ResumenEl desarrollo de la
electrnica y de la informtica ha generado la construccin de una
gran cantidad de robots y mquinas automticas, que realizan multitud
de tareas por nosotros, proporcionndonos una mayor calidad de vida.
Estas mquinas se encargan de realizar trabajos peligrosos, pesados
o repetitivos rpidamente y con mayor precisin de lo que podemos
hacer nosotros. Mientras que las mquinas automticas solo realizan
una funcin, los robots muestran cierto grado de autonoma y
adaptacin a las condiciones del entorno en el que actan. Nuestro
proyecto consiste en la construccin de un robot capaz de seguir una
lnea con ciclo cerrado (pista-circuito), en cualquier tipo de forma
o trayectoria. Los componentes electrnicos empleados para la
construccin de este prototipo son sensores, sistemas de control,
herramientas y elementos que suministran energa. El primer paso fue
conocer el funcionamiento de cada componente que constituye al
robot, as como crear el circuito elctrico que rige la lgica del
robot, con ayuda de la investigacin y documentacin. Se cre el diseo
del robot de acuerdo a las caractersticas que requiere el circuito
elctrico. Es importante mencionar que el robot utiliza el lenguaje
de programacin en C para Arduino, ya que es el encargado de decidir
que motor se activa o se apaga en base a la orden de los sensores.
El diseo de la pista fue lo ms sencillo solo se busc un contraste
entre la lnea que el robot seguir y el fondo, se utiliz cinta de
aislar negra.Finalizamos haciendo, mencin de que los alumnos
participantes en este proyecto tienen conocimientos bsicos de
robtica y electrnica, la idea del proyecto surgi con el propsito de
adquirir mayores conocimientos en los mbitos de robtica, ciencia y
tecnologa adems de es un encuentro con diversos aspectos de la
investigacin, la lectura y la tecnologa.1.3 Palabras clave:
Arduino, Segui-linea, Sensor, Electrnica, Robtica.1.4 IntroduccinLa
robtica y la electrnica son reas importantes en la historia del
planeta ya que en todos los campos nos permite crear una cantidad
ilimitada de aplicaciones prcticas, los podemos utilizar en nuestra
va cotidiana y los damos por hecho. En el caso de los seguidores de
lnea nos permite aproximarnos a la programacin y aplicar la teora
obtenida en clase.1.5 Materiales y
MetodologaMATERIALESNOMBREIMAGENDESCRIPCIN
ComputadorDispositivo electrnico a travs del cual se desarroll
el informe tcnico
Arduino
Arduino es una plataforma de hardware libre, basada en una placa
con un microcontrolador y un entorno de desarrollo
Componentes Electrnicos:Motores DC.Sensores QRT 8RC.Ruedas.
Dispositivos que forman parte de un circuito electrnico.
Materiales Varios:Cautn.Estao.Pinzas.Utilizados soldar los
diferentes componentes y armar el proyecto.
METODOLOGAPara iniciar el diseo de los SEGUIDOR DE LINEA era
fundamental conocer las funciones que ste deba desempear y a partir
de ese punto conocer todos los dispositivos que se requeriran para
llevar a cabo dichas funciones. Una vez se tuvo conocimiento de los
dispositivos que se deberan utilizar tales como el sensor, las
ruedas, los motores, la unidad de control, los pulsadoresetc. nos
sumergimos en un gran abanico de posibilidades en cuanto a la
eleccin de dichos componentes, puesto que debamos encontrar los ms
adecuados, segn sus caractersticas, para poder desarrollar la
funcin a la que iban a ser designados de la mejor forma posible y
adecundolos todos ellos a un ensamblaje de unas dimensiones
considerables para poder, cuanto menos, reducir el coste del
proyecto pero sin restarle las funciones para las cuales iban a ser
diseados.
Me gustara destacar la dificultad del apartado de diseo ya que
se llev, junto con la parte de programacin y desarrollo de la
interface del robot, la mayor parte del tiempo designado para la
realizacin del proyecto.
Tras haber elegido la mayora de los componentes procedimos al
estudio de sus caractersticas mecnicas de ensamblaje para poder
meternos en el diseo del robot que iba a ser el encargado de
contener todos los elementos necesarios para el buen
funcionamiento.
Lo primero que debamos pensar era el material del que se iba a
fabricar puesto que pasara a ser el esqueleto del robot, adems
debamos pensar en la forma ms adecuada del mismo, en base a poder
realizar su funcin principal y que su fabricacin no fuera
excesivamente complicada y por lo tanto costosa tambin a nivel
econmico. Para ello se contemplaron infinidad de modelos ya
existentes de todas las vertientes robticas de la actualidad, con
el fin de poder llegar a inspirarse en un diseo prctico, cuidado
estticamente en la medida de nuestras posibilidades, funcional y de
un coste lo ms asequible posible. Teniendo en cuenta todos estos
requisitos optamos por la fabricacin de un chasis de cartn
prensado, ya que esta eleccin nos facilitaba muchsimo tanto la
elaboracin del mismo como la posibilidad de poder realizar pequeas
modificaciones una vez fabricado, debido principalmente, a la
maleabilidad y peso reducido que este nos ofreca.
Una vez realizados unos cuantos bocetos nos dispusimos a
establecer de forma aproximada las dimensiones de nuestro robot en
base a las necesidades de movilidad que ste debera ofrecernos y a
los componentes seleccionados para desarrollar sus funciones.
1.6 MARCO TERICO
1.6.1 Recursos utilizadosArduino.-Arduino es un entorno, libre
de licencias, que nos ha permitido realizar toda la programacin de
la placa de control de nuestro robot.Este entorno utiliza, como
lenguaje de programacin, un lenguaje similar al C que es un
lenguaje bastante conocido a nivel de programacin que nos ha
facilitado, una vez realizado el apartado de diseo, el poder
ponernos a trabajar con este entorno sin la necesidad de estudiar
demasiado tutoriales para desarrollar nuestro proyecto, puesto que
ya tenamos asumidos, a nivel conceptual, los conocimientos
necesarios para programar en dicho lenguaje.
A pesar de ello si hemos tenido que conocer el funcionamiento a
nivel de programacin de todos los sensores con los que cuentan
nuestros robots, que ha sido un trabajo un tanto complicado pero
que ha dado sus frutos y por lo tanto se ha convertido en un
trabajo satisfactorio y enriquecedor, todo ello a pesar de que en
realidad nosotros hemos realizado el diseo de una programacin mnima
de funcionamiento.
Herramientas utilizadas y estacin de trabajo:Adems de todos los
recursos informticos, tambin hemos utilizado toda una serie de
herramientas para la realizacin del ensamblaje de nuestro robot.Las
herramientas que ms hemos utilizado de este banco de trabajo son:
Soldador: para todas las tareas de soldadura de cables, pines de
conexionado y calentamiento de las fundas retractiles, designadas
para aislar y proteger los elementos soldados.
Taladro: para la realizacin de los orificios tanto en el chasis
como en los herrajes de los servos diseados por nosotros.
Destornilladores: Para el ensamblaje de todos los elementos en
el chasis y en las salidas PWM de la placa de control.
Pelacables: para facilitar la tarea de pelar los terminales de
los cables.
Tornillo de banco: Para la sujecin de piezas en su mecanizado
adems de la realizacin de dobleces en los herrajes de los
servos.
Tester: Para la comprobacin del conexionado y verificacin tanto
de la polaridad como de la continuidad de las lneas soldadas.
Reglas: Para tomar medidas a todos los elementos del robot y
posteriormente reproducirlos.
1.6.2 Descripcin de los componentes del diseo.CHASISEl chasis es
una parte importante del robot ya que debemos recordar que es el
esqueleto del mismo. Por ello debemos de tener en cuenta una serie
de caractersticas que debe cumplir a la hora de realizar la eleccin
del material con el que ser confeccionado: Debe ser lo
suficientemente robusto como para contener el peso de todos los
componentes, sin sufrir ningn esfuerzo excesivo en cuanto a fuerzas
aplicadas se refiere.
Debe ser capaz de soportar pequeos impactos producidos por
posibles colisiones en sus desplazamientos, sin sufrir
deformaciones.
Debe ser lo suficientemente maleable para facilitarnos el
proceso de colocacin de los diferentes elementos.
Debe ser lo suficientemente liviano como para poder ser
arrastrado por los componentes motrices del robot (motores, ruedas)
sin que estos sufran un esfuerzo excesivo y por lo tanto un
desgaste energtico mayor.
Debe estar realizado con un material que no requiera excesiva
dificultad para su produccin y por lo tanto no nos provoque un
coste econmico demasiado elevado.
PLACA DE CONTROLEl cerebro del robot. Esta parte es la ms
importante de nuestro robot, puesto que es la encargada de
interpretar y administrar toda la informacin recibida por los
sensores instalados y adems es capaz a posteriori de ejecutar las
rdenes para las que ha sido programada de una forma rpida y
eficiente.A la hora de su eleccin nos hemos basado en dos
condiciones que eran fundamentales para el desarrollo del robot,
una era el lenguaje de programacin que utilizara, que preferamos
que fuera un lenguaje maquina estandarizado en el mundo de la
programacin, y que por lo tanto no nos dificultara excesivamente la
creacin y el diseo de los programas en los que se iba a basar el
robot, para la correcta finalizacin de las funciones designadas.Por
otra parte deba tener el nmero suficiente de entradas y salidas,
tanto analgicas como digitales para poder comandar todos los
sensores y componentes motrices con los que cuenta el robot.Una vez
analizados detenidamente todos estos detalles y observando el
material que disponamos comenzamos a estudiar las placas
controladoras de Arduino que utilizan un entorno de programacin que
est libre de licencias, y por lo tanto es accesible y gratuito para
todo el mundo, y adems dicho entorno (ARDUINO) utiliza un lenguaje
parecido al C, para la programacin del mismo, que es un lenguaje
conocido en el mundo de la programacin y que nos ha permitido
alcanzar la totalidad de los propsitos de este proyecto.
MOTORES DC
Tras analizar el peso de todo el conjunto y determinar el
voltaje de entrada de los motores decidimos montar motores con una
reduccin de 30:1. Estos motores de corriente continua permiten una
fuerza de torsin de 0.4 Kg. /cm. DC Micro Metal de corriente
continua con reductora diseado y fabricado por Pololu para uso en
robtica. Nuestros Motores para Robtica son de alta calidad, de
dimensiones reducidas y con una reductora metlica que reduce las
revoluciones y aumenta la fuerza (troqu).
Especificaciones (Para 6V): Dimensiones: 24 x 10 x 12 mm Ratio
de la reductora: 30:1 Dimetro del eje: 3,9mm (con ranura de
bloqueo) Voltaje nominal: 6Vcc (puede funcionar entre 3 a 9Vcc)
Velocidad de giro sin carga: 440rpm Consumo sin carga: 40mA (Mx.:
360mA) Peso: 10 gramos Troqu: 0,4 kg-cm (mx.)
Aunque el motor soporte un amplio rango de voltajes, se
recomienda utilizarlo con 6V ya que con voltajes menores pierdes
bastante fuerza y con voltajes mayores puede afectar a la vida til
del motor.
SENSOR SEGUIDOR DE LNEA.El sensor seguidor de lneas es un mdulo
electrnico de Pololu que se coloca en la parte delantera e inferior
del robot.Est formado por 8 emisores y detectores de infrarrojos
separados 9,5 mm. entre s de los cuales nosotros hemos utilizado
nicamente los cuatro pertenecientes a la parte central.La funcin
principal del seguidor de lneas es la de detectar mediante los
emisores y detectores de infrarrojos si debajo del robot hay algn
tipo de lnea. Si es as el robot sigue dicha lnea intentando no
perder su rastro. Si pierde el rastro, el robot debe de intentar
volver lo antes posible a la lnea para hacer el recorrido.
ALIMENTACIN.
Una pila elctrica o batera elctrica es el formato
industrializado y comercial de la celda galvnica o voltaica.Es un
dispositivo que convierte energa qumica en energa elctrica por un
proceso qumico transitorio, tras lo cual cesa su actividad y han de
renovarse sus elementos constituyentes, puesto que sus
caractersticas resultan alteradas durante el mismo. Se trata de un
generador primario. Esta energa resulta accesible mediante dos
terminales que tiene la pila, llamados polos, electrodos o bornes.
Uno de ellos es el polo negativo o nodo y el otro es el polo
positivo o ctodo.
Driver TB6612FNGTB6612FNG: Buen driver, con l se logr una buena
velocidad. Es liviano, pequeo y tiene la funcin de freno. Cuando lo
instalamos conseguimos que la duracin de la batera pasara de 4 min
a 13 min.
1.7 DESARROLLO DEL PROYECTOPara realizar el seguidor se tom en
cuenta varios modelos que podan ser ajustables a los en s, en
nuestro caso, se buscaba realizar, pero en mucho de los casos
contaban con diferentes componentes o tena una estructura muy
compleja, por los que se decidi la siguiente:Esquema de conexin
Modelo seleccionado
Las conexiones entre mdulos se hicieron con hilos de cobre
esmaltado. Lo que reduce el peso de forma considerable.
Y se tom mucho en cuenta la disposicin de las entradas, tanto
analgicas como digitales para su conexin y para la declaracin de
variables en a programacin en arduino.
Un paso muy importante antes del ensamblaje de los sensores en
el chasis es su verificacin y funcionamiento, es decir que
verificamos que todos los sensores se encuentren censando para haci
obtener un ptimo desarrollo de seguidor, para lo cual fue de vital
importancia la importacin de su librera, la misma que es
distribuida por POLOLU en su pgina :
Librera de los sensores
Cdigo de librera de los QTR para el censado
El control PID es un paso ms avanzado y demoroso en la creacin
del seguidor, en nuestra carrera se recibi un mdulo llamado
sistemas de control, en el cual conocamos un paso ms a profundo
hacerca de este tema.En relacin a nuestro proyecto, el control PID
es provisto a la seal PWM que es controlada por el arduino por
medio de variables:
Las cuales nos indican el grado de oscilacin del seguidor, es
decir la respuesta al error en estado estable que tendr que ser
regulada por medio de las variables Kp y Kd de nuestro cdigo, los
cuales proporcionan la variacin de velocidad en nuestros motores.
(Ki es el error integral del sistema es de muy bajo valor y casi no
se tom en cuenta).
DESCRIPCIN DE LOS PARAMETROS USADOS.-
Proporcional: Es la respuesta al error que se tiene que entregar
de manera inmediata, es decir, si nos encontramos en el centro de
la lnea, los motores, tendrn en respuesta una velocidad de igual
valor, si nos alejamos del centro, uno de los motores reducir su
velocidad y el otro aumentara. Proporcional= (posicin)
-punto_consigna
Integral: La integral es la sumatoria de los errores acumulados,
tiene como propsito el disminuir y eliminar el error en estado
estacionario provocado por el modo proporcional, en otras palabras,
si el robot velocista se encuentra mucho tiempo alejado del centro
(ocurre muchas veces cuando se encuentra en curvas), la accin
integral se ira acumulando e ira disminuyendo el error hasta llegar
al punto de consigna, Integral=Integral + proporcional_pasado
Derivativo: Es la derivada del error, su funcion es mantener el
error al mnimo, corrigindolo proporcionalmente con la mismo
velocidad que se produce, de esta manera evita que el error se
incremente, en otra palabra, anticipara la accin evitando as las
oscilaciones excesivas.
Derivativo=proporcional-proporcional_pasado
CONSTANTES UTILIZADOS EN LA CODIFICACIN.-Factor (Kp) - Es un
valor constante utilizado para aumentar o reducir el impacto de
Proporcional. Si el valor es excesivo, el robot tendera responder
inestablemente, oscilando excesivamente. Si el valor es muy pequeo,
el robot responder muy lentamente, tendiendo a salirse de las
curvasFactor (Ki) - Es un valor constante utilizado para aumentar o
reducir el impacto de la Integral, El valor excesivo de este
provocara oscilaciones excesivas, Un valor demasiado bajo no
causara impacto alguno.Factor (Kd) - Es un valor constante
utilizado para aumentar o reducir el impacto de la Derivada. Un
valor excesivo provocara una sobre amortiguacin. Provocando
inestabilidad.
El PID (control proporcional, integral y derivativo) es un
mecanismo de control por realimentacin que calcula la desviacin o
error entre un valor medido y el valor que se quiere obtener (set
point, target position o punto de consigna), para aplicar una accin
correctora que ajuste el proceso.
En el caso del robot velocista, el controlador PID, (que es una
rutina basada matemticamente), procesara los datos del sensor, y lo
utiliza para controlar la direccin (velocidad de cada motor), para
de esta forma mantenerlo en curso.
Error - Llamamos a la diferencia entre la posicin objetivo y la
posicin medida del error. (Que tan lejos del punto de consigna se
encuentra el sensor, en nuestro caso el objetivo es tener los
sensores centrados).
Grafica de tiempos en estabilizar en errorPara el control de los
motores es de uso obligatorio el driver TBN6612FNG, que en si es un
puente H doble, cuya funcion principal en recibir los pulsos PWM
digitales del controlador arduino y adecuarlos hacia los motores,
que en nuestro caso son los moto reductores 30:1, para control su
velocidad tanto en lnea recta haci como su velocidad en curvas.
Diagramas de conexin.
Esto nos permitir manejar con una mayor eficiencia los motores,
y claro esta tambin ahrranos muchas lneas de cdigo. Es importante
destacar que sea cual sea el microcontrolador que usemos, este
tiene que contar con los suficientes pines de PWM.Empezaremos por
crear nuestra funcion para el manejo de un solo motor, luego
extenderemos para la cantidad de motores que queramos.
Crearemos la funcion en el que se le dar un parmetro que
contendr signo, el signo nos servir para controlar la direccin
(positivo hacia adelante, negativo hacia atrs),
Funcion de control de motores y funcion de frenado en curvas del
seguidor
Finalmente el conjunto de monta en la figura ya preestablecida y
modelada, de tal manera que los componentes esten distribuidos
simetricamente con el fin de equilibrar su peso y darle un poco de
estetica al proyecto final.
1.8 ANNALISIS E INTERPRETACIN DE RESULTADO
El conocimiento y el uso de una metodologa es una manera que
permite planear adecuadamente las actividades especializadas
implicadas en un proyecto de investigacin. El tiempo dedicado al
proyecto puede ser considerablemente reducido a travs de una buena
administracin y organizacin. Los equipos formados para este
proyecto fueron capaces de trabajar de forma conjunta y manejar
informacin especfica. El robot mvil fue diseado de acuerdo a las
especificaciones del diseo, y el sistema de control puede conducir
al robot de forma autnoma mediante los circuitos diseados para
realizar los comandos especficos. Para posteriores versiones del
proyecto se aspira y se tiene ya en mente la mejora en diseo,
control, programacin, del proyecto presentado, con el fin de
participar en las jornadas de robtica que se realizaran cada
semestre en la facultad de Ingeniera en Sistemas Electrnica
1.9 CONCLUSIONES. El controlador ATMEGA328, basado en la
plataforma Arduino, ofrece prestaciones ptimas, tanto en su
capacidad para la conexin de sensores y actuadores, como en la
facilidad de su programacin, utilizando entornos y lenguajes de
programacin de alto nivel; adems de ser la placa controladora ms
utilizada en concursos de robtica en la actualidad.
A pesar de su alto nivel de programacin encontramos distintos
fallos producidos por la no correcta interpretacin del cdigo por
parte de la placa controladora de Arduino, as como otras
imperfecciones en la precisin de los movimientos de nuestros
robots.
Es necesaria una valoracin y estudio exhaustivo en cuanto a los
elementos mecnicos se refiere, tales como coeficientes de
rozamiento de los neumticos, distancias entre ejes, tensiones. Ms
all de la funcin estructural de contener los diversos elementos de
los que consta el robot.
Una vez concluido el proyecto han quedado distintos aspectos por
perfeccionar. Estos aspectos son los siguientes:
Mejorar la precisin. Mejorar la velocidad de los robots. Mejorar
los programas para que se ejecuten en la mayor brevedad, con la
mayor sencillez posible y realicen ms movimientos.
2. REFERENCIAS BIBLIOGRFICAS http://www.Arduino.cc (Entorno
Arduino) http://www.freeduino.org/ (Entorno Arduino)
http://www.ladyada.net/learn/lcd/charlcd.html (Componentes y
robtica en general)
http://www.hispavila.com/3ds/atmega/pulsadores.html (Componentes y
robtica en general) http://www.Pololu.com (Componentes y robtica en
general)
http://www.adafruit.com/forums/viewtopic.php?f=8&t=14423 (Foro
entorno Arduino)
http://www.arduinobot.pbworks.com/w/page/10175779/Motores-DC
(Componentes Electrnicos) http://www.wikipedia.org/ (Enciclopedia
electrnica)
ANEXOS.- Cdigo de arduino.-
#include
#define NUM_SENSORS 5 //numero de sensores usados#define TIMEOUT
2500 // tiempo de espera para dar resultado en uS#define
EMITTER_PIN 11 //pin led on
///////////////pines arduino a utilizar/////////////////////
#define led1 13
///////Pines para el control de motoreductores//////////
#define pin_pwm_i 6#define motor_i 5
#define pin_pwm_d 9#define motor_d 8
#define STBY 12
//////////////////////////
#define sensores 5
////// Pines para lectura de los sesores//////////
QTRSensorsRC qtrrc((unsigned char[]) {A0, A1, A2, A3, A4
},NUM_SENSORS, TIMEOUT, EMITTER_PIN);
/////variables para almacenar valores de sensores y
posicion///////
unsigned int sensorValues[NUM_SENSORS];unsigned int
position=0;
//////// variables para el control PID /////////
int derivativo=0, proporcional=0, integral=0; //parametrosint
salida_pwm=0, proporcional_pasado=0;
//_______AQUI CAMBIEREMOS LOS PARAMETROS DE NUESTRO
ROBOT_________________
int velocidad=50; //variable para la velocidad
float Kp=2;float Kd=0.18;float Ki=0.01; //constantes
//variables para el control del sensado//
int linea=0; int flanco_color =0; // aumenta o disminuye el
valor del sensadoint en_linea=500; //valor al que considerara si el
sensor esta en linea o noint ruido= 50; //valor al cual el valor
del sensor es considerado como
ruido//________________________________________________________________________________
void setup()
{
delay(50); pinMode(motor_i, OUTPUT);//pin de direccion motor
izquierdo pinMode(motor_d, OUTPUT);//pin de direccion motor derecho
pinMode(13, OUTPUT); //led1 digitalWrite(13, HIGH); for (int i = 0;
i < 100; i++) { qtrrc.calibrate(); //funcion para calibrar
sensores } digitalWrite(13, LOW);
Serial.begin(9600); for (int i = 0; i < NUM_SENSORS; i++) {
Serial.print(qtrrc.calibratedMinimumOn[i]); Serial.print(' '); }
Serial.println();
for (int i = 0; i < NUM_SENSORS; i++) {
Serial.print(qtrrc.calibratedMaximumOn[i]); Serial.print(' '); }
Serial.println(); Serial.println(); delay(1000);}
void loop()
{ pid(linea, velocidad, Kp, Ki, Kd, flanco_color, en_linea,
ruido); Frenos_contorno(linea,700); }
/////funciones para el control del Seguidor////
void pid(int linea, int velocidad, float Kp, float Ki, float
Kd,int flanco_color, int en_linea,int ruido) { position =
qtrrc.readLine(sensorValues, QTR_EMITTERS_ON, linea,flanco_color,
en_linea, ruido ); for (unsigned char i = 0; i < NUM_SENSORS;
i++) { Serial.print(sensorValues[i]); Serial.print('\t'); }
//Serial.println(); // uncomment this line if you are using raw
values Serial.println(position); // comment this line out if you
are using raw values delay(250);
proporcional = (position) - 3500; // set point es 3500, asi
obtenemos el error integral=integral + proporcional_pasado;
//obteniendo integral derivativo = (proporcional -
proporcional_pasado); //obteniedo el derivativo if
(integral>1000) integral=1000; //limitamos la integral para no
causar problemas if (integral velocidad ) salida_pwm = velocidad;
//limitamos la salida de pwm if ( salida_pwm < -velocidad )
salida_pwm = -velocidad; if (salida_pwm < 0) {
motores(velocidad+salida_pwm, velocidad); } if (salida_pwm >0) {
motores(velocidad, velocidad-salida_pwm); }
proporcional_pasado = proporcional; }
void motores(int motor_izq, int motor_der)
{ digitalWrite(STBY, HIGH); if ( motor_izq >= 0 ) //motor
izquierdo { digitalWrite(motor_i,HIGH); // con high avanza
analogWrite(pin_pwm_i,255-motor_izq); //se controla de manera
//inversa para mayor control
} else { digitalWrite(motor_i,LOW); //con low retrocede
motor_izq = motor_izq*(-1); //cambio de signo
analogWrite(pin_pwm_i,motor_izq); }
if ( motor_der >= 0 ) //motor derecho {
digitalWrite(motor_d,HIGH); analogWrite(pin_pwm_d,255-motor_der); }
else { digitalWrite(motor_d,LOW); motor_der= motor_der*(-1);
analogWrite(pin_pwm_d,motor_der); }
}
void Frenos_contorno(int tipo,int flanco_comparacion){
digitalWrite(STBY, HIGH); if(tipo==0) { if
(positionflanco_comparacion ||
sensorValues[1]>flanco_comparacion ) { break; } } }