Murphy - Fase lV
Introducción a la Ingeniería Electrónica - 710100M
Facultad de Ingeniería
Universidad del Valle
Integrantes
Sergio Becerra Sánchez - 2125142
Alejandro Muñoz Gutiérrez - 2123843
Robot seguidor de línea (Murphy) Fase IV
Driver TB6612FNG: El driver para motores TB6612FNG es un controlador que posee dos puentes H que nos permite
manejar dos motores de corriente continua con una corriente constante de 1.2A (3.2A pico), variando la velocidad y
el sentido de giro. Posee dos señales de entrada para cada motor (IN1 e IN2) que son usadas para controlar el sentido
de giro. Las dos salidas de motores (OUT1 y OUT2) Pueden ser controladas de manera separada y la velocidad de
los motores es controlada mediante una señal PWM con una frecuencia de hasta 100kHz, dicha señal PWM es calculada
mediante un controlador PID que será explicado luego. El pin STNDBY permite activar o desactivar el driver
colocando al motor en modo de standby. El driver posee diodos internos de protección.
Características:
Chip: TB6612FNG (Toshiba)
Canales: 2 (soporta 2 motores DC o 1 motor PAP)
Voltaje de Potencia (VM): 5V - 15V
Voltaje Operación (VCC): 2.7V - 5.5V
Consumo de corriente 2.2mA
Dimensiones: 20.3mm x 20.3m
Consideramos que este driver es la mejor opción para este proyecto pues, es debido a su pequeño tamaño y a sus capacidades para controlar la potencia los motores (tanto en el control de las velocidades como en el sentido de giro de los motores), que lo consideramos fácil de implementar y programar. Además de poseer diodos internos de protección que previenen contra la potencia inversa en la alimentación.
Vista Lateral del robot
Vista Superior del robot
Vista diagonal del robot
Funcionamiento del Programa:
Para entender el funcionamiento del programa que controla los motores, voy a nombrar las variables más importantes.
VBase: Es el voltaje base que poseen los motores cuando se encuentran en línea recta
STBY: Es el pin del standby, necesario para dar inicio al driver
Cambio: Variable en la que se almacena el PID
Error: Es el parámetro proporcional del PID, este indica que tan desviado está el robot en el momento presente.
D: Es el parámetro derivativo del PID, este se usa para estimar el error futuro que va a tener el robot
I: Es el parámetro integral del PID, este calcula el error anterior que tuvo el robot
EAnterior: Variable en la que se almacena el error anterior, esta variable es necesaria para calcular
pwma: Esta variable almacena el cambio para el motorA
pwmb: Esta variable almacena el cambio para el motorB
position: Es una aproximación de la posición del robot sobre la línea, este valor va entre 0 y 7000, siendo 0 un extremo y 7000 el otro extremo del sensor.
Una vez explicadas las variables, explicaremos el funcionamiento del PID. Al momento en el que se pulsa el botón,
se envía un pulso alto a través del pin de STNDBY del motor para activar el driver y dar funcionamiento
de los motores, los sensores iniciaran a leer la pista y estimar la posición en la que se encuentra el robot, aquí entra la
variable position que es en la que se almacena la posición actual del robot. Con dicha variable, se estima el Error
del robot sobre la pista, para ello se resta la position con 3500, dónde 3500 es el set point y es igual a que el robot
se encontrará bien posicionado.
Error = position - 3500
Luego se empiezan a medir los parámetros derivativo e integral (D e I), los cuales son medidos dependiendo del Error y del EAnterior.
D = Error - EAnterior
I = Error + EAnterior
Una vez son medidos los tres parámetros, (Error (que es el proporcional), D e I), se calcula el PID y dicho valor se almacena en la variable Cambio de la siguiente manera:
Cambio = (Error * 0.063) + (D * 0.01) + (I * 0.03)
Las constantes se calculan al momento de probar el robot sobre la pista. La variable Cambio se usa para cambiar
la velocidad de los motores y con ello, intentar que el robot encuentre su posición ideal la cual es estar sobre la línea negra
. Para calcular la velocidad de los motores se utiliza la siguiente ecuación:
pmwa = Vbase - Cambio
pmwb = Vbase - Cambio
Donde pwma es la variable para el motor A y pwmb es la variable para el motor B. Para evitar problemas
con el código de arduino se ha añadido la siguiente condición.
if (pwma > 255){pwma = 255; }
if (pwma > 255){pwma = 255; }
Esto se hace debido a que es posible que el cambio de velocidad exceda 255, que es el máximo valor de salida análoga
dada por el arduino.
Para finalizar, se envían los valores de pwm a sus respectivos motores y se establece el sentido de los motores hacia
adelante.
analogWrite(MotorA, pwma);
analogWrite(MotorB, pwmb);
digitalWrite(MotorA1, LOW);
digitalWrite(MotorA2, HIGH);
digitalWrite(MotorB1, LOW);
digitalWrite(MotorB2, HIGH);
Se almacena el error anterior en la variable EAnterior.
EAnterior = Error
De todas formas, también se explica el funcionamiento dentro del código.
Código de programación:
https://create.arduino.cc/editor/alemg42/1b7a061f-1a30-4af4-9e7a-90bd74c719c6/preview
Video:
Referencias
Comentarios
Publicar un comentario