Robot Evasor de Obstáculos

Robótica - Entrega II
Diego Chila1 , Laura Sarmiento2 y Helberth Calderón3

Abstract— The design of a robot that evades obstacles con- también se hace uso del sensor de proximidad FC-51, el
sists of several controllers. In order to achieve an adequate cual nos servirá para evitar abismos. Todo esto unido en un
algorithm, one must first have well-characterized the engines chasis de acrı́lico lo que da un peso total de 472 gramos.
and get a precise speed control that ensures the same speed
of the engines. Once this is achieved and with the help of
linear algebra, design and implement controllers in order to A. Regulador
reach the desired point and avoid obstacles. In this report, the
characterization of the motors and the design process of each Para la alimentación del PIC y del dispositivo bluetooth
controller will be detailed. es necesario tener una fuente DC de 5V, para esto se hace
uso del regulador LM7805 con la configuración dada en la
I. INTRODUCCI ÓN Fig 1.
El uso de los sensores es de enorme ayuda para un
robot al dar información sobre su entorno y de esta forma
llevar a cabo su algoritmo de la mejor manera. Actualmente,
existen automóviles que tienen la capacidad de manejarse
por sı́ solos gracias al uso de sensores que lo previenen
de inminentes choques. El objetivo en el presente proyecto
es mostrar los pasos y diseños para lograr implementar un
algoritmo similar al de estos automóviles, donde el prototipo
pueda dirigirse de un punto A hacia un punto B en el
camino más corto posible evitando obstáculos y abismos.
Simultáneamente a esto, poder transmitir los datos de la
posición del robot y graficar su trayectoria en Matlab.
Fig. 1. Circuito del regulador LM7805 a 5V
II. DESCRIPCI ÓN DEL SISTEMA
El prototipo se compone de dos motores, cada uno de-
bidamente acoplado a una rueda de 6.47cm de diámetro, B. Puente H
separadas entre sı́ 11.36cm y acoplados cada uno a un Como etapa de potencia para los motores se implementó
Encoder, ambos Encoder tienen adaptado una caja con la el dispositivo L298N como se mencionó anteriormente, su
cual se protegen de posibles interferencias de luz. Tanto puesta en funcionamiento se muestra en la Fig 2.
el microcontrolador PIC18F4550 como el modulo bluetooth
HC-05 junto con todo el circuito de acondicionamiento, el
cual incluye un regulador de 5 voltios LM 7805, estará
implementado en una PCB hecha a la medida la cual aliviana
el vehı́culo y distribuye mejor el espacio. En cuanto a la
potencia, se utiliza el modulo puente H L298N. Como fuente
de alimentación para el prototipo se hace uso de dos baterı́as
18650, cada una de 3.7 voltios y 4800 mAh, las cuales
suponen una entrega de energı́a más que suficiente para
el vehı́culo. Debido a que se hace necesario saber a qué
distancia está el vehı́culo del obstáculo se utilizan 3 sensores
SHARP con rango de 10 a 80 centı́metros. Por último,

*Informe proyecto II - Robótica, Universidad Distrital Francisco José de

Caldas.
1 D. Chila, estudiante de ingenierı́a electrónica, Universidad Distrital
Francisco José de Caldas. Código 20142005033
2 L. Sarmiento, estudiante de ingenierı́a electrónica, Universidad Distrital
Francisco José de Caldas. Código 20151005034 Fig. 2. Circuito del puente H L298N
3 H. Calderón, estudiante de ingenierı́a electrónica, Universidad Distrital
Francisco José de Caldas. Código 20141005008
C. Bluetooth velocidadD=250000/(tiempoD);
%limite de frecuencia
Para realizar la transmisión de datos que permite observar if (velocidadD>10000) {
la posición en tiempo real del carrito se hace uso del velocidadD=0; }
dispositivo bluetooth HC-05 que envı́a datos a un receptor %promedio
velocidadPromD=(velocidadD+velocidadD1+velocidadD2
inalámbricamente, su puesta en marcha es muy sencilla pues +velocidadD3)/4;
solo necesita un envı́o de datos serial desde el PIC hacia el %limite del promedio
pin receptor del bluetooth por medio de un divisor de voltaje if (velocidadPromD>65)
{
a 3.3v que se hace con resistencias de 2kΩ y 1kΩ. velocidadPromD=65;
}
III. IDENTIFICACI ÓN DEL SISTEMA %IZQUIERDO
A. Función de transferencia de los motores %actualizar valores anteriores
velocidadI3=velocidadI2;
Para el caso en cuestión se requiere analizar los dos velocidadI2=velocidadI1;
motores para obtener la función de transferencia de este en velocidadI1=velocidadI;
%convertir timer a valor de frecuencia
velocidad dado como: velocidadI=250000/(tiempoI);
K %limite de frecuencia
G(s) = (1) if (velocidadI>10000) {
(s + a)(s + b) velocidadI=0;}
Para lograr lo mencionado anteriormente se habı́a implemen- %promedio
velocidadPromI=(velocidadI+velocidadI1+velocidadI2
tado en primer lugar un conversor de frecuencia a voltaje con +velocidadI3)/4;
el objetivo de tomar la lectura de los encoders y realizar el %limite de promedio
cambio a voltaje, esto con el fin de realizar una caracteri- if (velocidadPromI>65)
{
zación por respuesta al impulso; lastimosamente este método velocidadPromI=65;
no fue muy preciso por lo cual se optó posteriormente por }
un método que tuviera en cuenta la respuesta a impulso en }
diferentes rangos y contemplando el verdadero cero de cada
Lo que se procede a realizar es obtener en primera
motor y su valor máximo, este método es el de mı́nimos
instancia los valores lı́mite de cada motor que hacer que
cuadrados.
la rueda se mueva levemente, en el caso del motor derecho
De forma previa se busca obtener primero una señal más
se obtuvo un valor de 320 que corresponde al 31.28% de
limpia que se ajuste al valor verdadero que se evidencia
ciclo útil para un valor de 1023 en el 100%, en el caso del
en cada motor, es decir, para una lectura errónea como se
motor izquierdo el valor fue de 300, es decir 29,32%. Ya
evidenciaba en el proyecto anterior, en donde el encoder
conociendo los datos frontera se procede a enviar una serie
proporcionaba lecturas con un error mayor al 100% a pesar
de datos diferentes, los cuales harán un barrido de ciclo útil
de que no fuese muy frecuente, se realiza un promedio con
del motor, para es te caso se escogen diferentes valores que
el fin de que estos valores no sean tan erróneos, para esto
se repetirán 3 veces para cada motor, la medida de salida
se debe obtener el valor de tiempo de un periodo de los
de cada motor con sus respectivos escalones de entrada se
encoders, el código es el siguiente:
muestran en las figuras 3 y 4.

#int_ext2
void int_llantaD() {
tiempoD=get_timer0();
set_timer0(0);
delay_us(300);
}

#int_ext1
void int_llantaI(){
tiempoI=get_timer3();
set_timer3(0);
delay_us(300);
}

Luego de obtener el valor del timer se procede a hacer el

promedio para evitar un poco ruido en la señal: Fig. 3. Respuesta del motor derecho ante diferentes niveles de señal de
entrada

void velocidades() Las velocidades obtenidas experimentalmente se guardan

{
%DERECHO en un vector columna que posteriormente se utilizará en la
%actualizar valores anteriores función ARX de matlab, esta herramienta da como resultado
velocidadD3=velocidadD2; dos polinomios A(Z) y B(Z) que describen la función de
velocidadD2=velocidadD1;
velocidadD1=velocidadD; transferencia discreta de cada motor, los resultados de cada
%convertir timer a valor de frecuencia motor son:
 Fig. 7. Respuesta del motor izquierdo según la función de transferencia
Fig. 4. Respuesta del motor izquierdo ante diferentes niveles de señal de
entrada

Bd (z) 0.001845z −1 + 0.002398z −2

Gd (z) = = (2)
Ad (z) 1 − 1.319z −1 + 0.3913z −2

Bi (z) 0.002494z −1 + 0.003208z −2

Gi (z) = = (3)
Ai (z) 1 − 1.192z −1 + 0.2883z −2
Ya con las funciones de trasferencia obtenidas en el primer
intento de caracterización para cada motor se procede a
comprobar su validez llevando a cabo un segundo intento y
verificando que el resultado de las funciones de transferencia Fig. 8. Respuesta del motor izquierdo ante diferentes niveles de señal de
entrada
(figura 5 y 7) esté acorde a los datos obtenidos de los
encoders(figura 6 y 8).
B. Función equivalente de los sensores
Para obtener la distancia a la que se encuentra un obstáculo
se hace uso del sensor de distancia SHARP, este sensor pro-
porciona voltajes de 0V a 5V aproximadamente en donde el
valor de voltaje es alto para obstáculos cercanos y disminuye
a medida que se encuentra más lejano. Para poder obtener el
valor preciso de distancia se toman los valores de voltaje que
se pueden observar en la Fig 9 para determinadas distancias,
luego se emula a través de una función de potencia debido a
que la curva es exponencial, el valor de distancia graficando
Fig. 5. Respuesta del motor derecho según la función de transferencia
en el eje x el voltaje de salida y en el eje y la distancia, ası́
se obtiene una función de la forma y = mxa que será la
que se pondrá en el PIC tomando la lectura del sensor por
medio del ADC.

Fig. 6. Respuesta del motor derecho ante diferentes niveles de señal de

entrada

Comparando entonces los valores obtenidos se comprueba

la similitud de las señales que según la herramienta ARX es
de 93.83% para el derecho y 94.12% para el izquierdo. Fig. 9. Curva representativa del sensor de distancia
 Como se puede obsevar a partir de la Fig 9 el exponente El proceso realizado fue implementar el código anterior
que se le asigna al valor de voltaje es muy cercano a uno, inicialmente para un motor y visualizar en matlab el tiempo
como la función para realizar la exponencial en el PIC se de establecimiento y overshoot de la respuesta del sistema
tiene que realizar 3 veces (Una para cada sensor SHARP) se con controlador, y posteriormente aplicar variaciones en el
ocupa aproximadamente 20% más de la ROM del PIC, es código con la función de transferencia del otro motor que
por esto que observando que se presenta un error mı́nimo al generaran una respuesta bastante similar a la del primer
cambiar el valor de 1.091 a 1 se opta por no implementar la motor, las respuestas al escalón del sistema con controladores
función POW. derecho e izquierdo se presentan en la figura 11, los contro-
ladores obtenidos fueron:
IV. DISE ÑO DEL SISTEMA DE CONTROL
En el caso de este proyecto se requieren dos controladores, (z − 0.8446)(z − 0.616)
Cd (z) = 64.894 (4)
el primero es el controlador de velocidad que hace que los z(z − 1)
motores del carrito giren de acuerdo a lo deseado, el segundo
(z − 0.8336)(z − 0.3358)
controlador es de ángulo que funciona como medio para que Ci (z) = 29.567 (5)
el carrito vaya al punto deseado a partir de las coordenada z(z − 1)
X y Y deseadas, el diagrama de bloques general se muestra
Ahora se requiere obtener la forma de implementación
en la Fig 10.
en el código para ejecutar experimentalmente esta respuesta,
A. Control de velocidad para esto se expande la ecuación de cada motor:
Para obtener un diseño de controlador en el cual las 64.894 − 94.78z −1 + 33.76z −2
respuestas de ambos motores sean muy similares, se asigna Cd (z) = (6)
1 − z −1
el valor de cada función de trasferencia en discreto en un
programa de Matlab, este realiza un control PID a partir del 29.567 − 34.57z −1 + 8.276z −2
sistema y de los valores deseados de tiempo de establec- Ci (z) = (7)
1 − z −1
imiento y overshoot, el código se muestra a continuación:
Teniendo en cuenta que:

close all U (z)

clear all
C(z) = (8)
e(z)
clc
%Se asigna el tiempo de muestreo El valor para la salida de cada controlador es:
T=0.05;
%Se asigna la funcion de transferencia del sistema
plantaD=tf([0.002494 0.003208],[1 -1.192 ... Ud (z) = Ud (z)z −1 + 64.894e(z)
0.2883],0.05); (9)
%Tiempo de establecimiento deseado − 94.78e(z)z −1 + 33.76e(z)z −2
Ts = 1.117;
%Overshoot deseado
zita=0.92; Ui (z) = Ui (z)z −1 + 29.567e(z)
%Obtener la frecuencia natural del sistema (10)
wn=(4/(Ts*zita));
− 34.57e(z)z −1 + 8.276e(z)z −2
%Obtener la respuesta deseada Realizando la transformada inversa Z se obtiene lo sigu-
respuestaDes=tf([wnˆ2],[1 2*wn*zita +wnˆ2]);
%Polos deseados iente:
PolDes=exp(T*(-zita*wn + j*wn*sqrt(1-zitaˆ2)));
%Obtenemos el angulo de la planta en el polo ...
deseado en grados Ud (k) = Ud (k − 1) + 64.894e(k)
AngPolDes=angle(evalfr(plantaD,PolDes))*180/pi; (11)
− 94.78e(k − 1) + 33.76e(k − 2)
%Angulo que debe aportar el controlador
AngC=wrapTo180(180-AngPolDes);
%Angulo que aporta b
Ui (k) = Ui (k − 1) + 29.567e(k)
(12)
Angb=AngC+(180/pi*angle(PolDes)) − 34.57e(k − 1) + 8.276e(k − 2)
+(180/pi*angle(PolDes-1))-90;
%obtener b El código a poner en práctica es:
if Angb<90
b=(real(PolDes)-((imag(PolDes))
/(tan(Angb*pi/180)))); void controladorIzq()
else {
b=((imag(PolDes))/(tan(Angb*pi/180))) %actualizar valores anteriores
+real(PolDes); e_k2I=e_k1I;
end e_k1I=e_kI;
%obtener a %error= entrada (wif) - salida (velocidadPromI)
a=real(PolDes); e_kI=wif-velocidadPromI;
%Hallamos la ganancia necesaria %actualizar valores anteriores
K=1/abs(evalfr(plantaD,PolDes)*(PolDes-a)*(PolDes-b) u_k1I=u_kI;
/((PolDes)*(PolDes-1))); %controlador izquierdo
%obtener la funcion del controlador u_kI=(29.567*e_kI)-(34.5762*e_k1I)
control=zpk([a b],[0 1],K,T); +(8.276*e_k2I)+u_k1I;
 Fig. 10. Diagrama de bloques total del sistema de control

Fig. 11. Respuesta al escalón para el motor derecho e izquierdo con controlador

%limites de la s e a l de salida }
if (u_kI>61) }
{
u_kI=61; void controladorDer()
} {
if (u_kI<0.16) %actualizar valores anteriores
{ e_k2D=e_k1D;
u_kI=0.16; e_k1D=e_kD;
} %error = entrada (wdf) - salida (velocidadPromD)
%ecuacion para obtener registro del pwm e_kD=wdf-velocidadPromD;
duty2=(16.814*u_kI)-2.6512; %actualizar valores anteriores
%limites del pwm u_k1D=u_kD;
if (duty2>1023) %controlador derecho
{ u_kD=(64.894*e_kD)-(94.7841764*e_k1D)
duty2=1023; +(33.762635*e_k2D)+u_k1D;
} %limites de la s e a l de salida
else if (duty2<300) if (u_kD>60)
{ {
duty2=300; u_kD=60;
 } •r es el valor del radio, es decir, 0.03235 metros
if (u_kD<0.34) •l es la distancia entre la rueda y la mitad del vehı́culo,
{
u_kD=0.34; es decir 0.0568cm
} • VL es la velocidad lineal que se desea.
%ecuacion para obtener registro del pwm • w[k] es la velocidad angular que proporciona la salida
duty1=(17.146*u_kD)-5.7805;
%limites del pwm del PID
if (duty1>1023) Entonces se puede obtener el valor de velocidad angular
{
duty1=1023; de cada motor como:
}     
if (duty1<335) wd 30.912 1.7558 VL
=
{ wi 30.912 −1.7558 w[k]
duty1=335;
} Estos valores son enviados al control de velocidad pero
} como frecuencia pues se está controlando con los valores de
frecuencia que ve cada encoder, y a partir de estos valores
B. Control de ángulo se obtiene con la matriz A el valor de velocidad angular y
Para llegar al punto deseado se debe hace uso de un velocidad lineal real del carro:
controlador PID que recibe las coordenadas X y Y deseadas y
por medio de la función trigonométrica de la tangente inversa
  r r
    
VLr wdr 0.016175 0.016176 wdr
se obtiene el ángulo que lleva el carro a dicho punto, para = 2r 2
−r =
w[k]r 2l 2l wir 0.28477 −0.28477 wir
implementar un control PID se desea lo siguiente:
A partir de estos valores se debe realizar un integrador para
Ω(s) ki obtener el ángulo real del carrito, esto a partir de θ̇ = w,
= kp + + kd s (13)
θe (s) s para esto se realizan sumas sucesivas que emulen el valor
1−z −1
de la integral, es decir que los valores ángulo real y de las
Pasando el PID a discreto tomando s = T y expan- coordenadas actuales del carrito se pueden obtener como:
diendo la ecuación se obtiene lo siguiente:
θr [k] = θr [k − 1] + (wkr [k]T ); (20)
kp T +ki T 2 +kd kp T +kd kd
Ω(s) z2( T )− z( T ) + T
= (14)
θe (s) z)z − 1) xr [k] = xr [k − 1] + (T vdr [k]cos(θr [k])); (21)
Para tener una expresión más simple se toma lo siguiente:

kp T + ki T 2 + kd yr [k] = yr [k − 1] + (T vdr [k]sen(θr [k])); (22)

a= (15)
T El código implementado es el siguiente:
kp T + kd
b= (16) void contrAng()
T
{
%actualizar valores
kd etheta_2=etheta_1;
c= (17)
T etheta_1=etheta;
%punto suma
La ecuación de transferencia del control de ángulo es etheta=theta_g-theta_r;
entonces: %limitar valores de -pi a pi
Ω(s) z 2 a − zb + c if (etheta>(2*pi))
= (18) {
θe (s) z2 − z
etheta=etheta-(2*pi);
z −2 }
Multiplicando por z −2 y aplicando transformada Z inversa, if (etheta>pi)
el valor de w es: {
etheta=etheta-(2*pi);
w[k] = w[k − 1] + aθe [k] − bθe [k − 1] + cθe [k − 2] (19) }
if (etheta<(-2*pi))
Con esto ya se obtiene el valor de velocidad angular de- {
seado según el PID, ahora para obtener el valor de velocidad etheta=etheta+(2*pi);
}
angular del motor derecho e izquierdo se hace uso de la if (etheta<(-pi))
matriz A inversa: {
etheta=etheta+(2*pi);
  1 l
  }
wd r r VL
= 1 −l wk_1=wk;
wi r r
w[k] %control PID de velocidad angular
wk=(wk_1)+(a*etheta)-(b*etheta_1)+(c*etheta_2);
En donde: %valores de wd y wi con limites
wd=(30.912*vd)+(1.7558*wk);
if (wd<0) if ((abs(y_r)<0.2)&&(abs(y_r)>(-0.2))&&
{ (abs(x_r)<
wd=0; abs(xg*1.2))&&(abs(x_r)>abs(xg*0.8)))
} {
else if (wd>18.85) stop=1;
{ }
wd=18.85; }
} if (xg==0)
wi=(30.912*vd)-(1.7558*wk); {
if (wi<0) if ((abs(x_r)<0.2)&&(abs(x_r)>(-0.2))&&
{ (abs(y_r)<
wi=0; abs(yg*1.2))&&(abs(y_r)>abs(yg*0.8)))
} {
else if (wi>19.16) stop=1;
{ }
wi=19.16; }
} }
%pasar wd y wi a frecuencia }
wdf=(wd*20)/(2*pi);
wif=(wi*20)/(2*pi); Como se puede observar al final se tienen varias condi-
%esperar los primeros 4 periodos ciones para que el carrito pare al llegar al punto deseado,
if (coni>4)
{ esto se debe a que se está manejando un rango de error del
%calcular wd y wi reales 20% por lo cual si se desea llegar a x = 0 o y = 0 se tiene
wd_r=(2*pi*velocidadPromD)/20; un rango de distancia entre -20cm y 20cm, ası́ mismo si es
wi_r=(2*pi*velocidadPromI)/20;
} a un punto diferente de cero el rango varı́a entre 0.8 y 1.2
else del valor deseado.
{
wd_r=0; V. DISE ÑO VECTOR DE EVASI ÓN DE
wi_r=0; OBST ÁCULOS
}
%calcular vd, wk y theta reales Para tener claro donde se encuentra algún objeto que
vd_r=(0.016175*wd_r)+(0.016175*wi_r); se encuentre en el camino del carrito se debe tener en
wk_r=0.28477*wd_r-0.28477*wi_r;
theta_r_1=theta_r; cuenta la distancia que lee el sensor y el ángulo y posición
theta_r=theta_r_1+(wk_r*T); de cada detector en la referencia del vehı́culo (θsi y dsi
%limitar theta de -pi a pi respectivamente, los valores son:
if (theta_r>(2*pi))
{
theta_r=theta_r-(2*pi); Sensor θsi xsi ysi
} s1 45o 24cm 6.5cm
if (theta_r>pi)
{ s2 0o 27.5cm 0cm
theta_r=theta_r-(2*pi); s3 -45o 24cm -6.5cm
}
if (theta_r<(-2*pi)) TABLE I
{ VALORES DE ÁNGULO Y COORDENADAS EN X Y Y DE LOS SENSORES
theta_r=theta_r+(2*pi);
}
if (theta_r<-pi)
{ La distancia del obstáculo se calcula de la siguiente
theta_r=theta_r+(2*pi); manera:
}
%hallar X y Y reales       
x_r_1=x_r; xob cos(θsi ) -sen(θsi ) d1 x
x_r=(x_r_1)+(T*vd_r*cos(theta_r)); = + si
y_r_1=y_r;
yob sen(θsi ) cos(θsi ) 0 ysi
y_r=(y_r_1)+(T*vd_r*sin(theta_r)); Ahora en cuanto a la referencia global se realiza de la
%hacer que el carrito pare cuando llegue al ...
punto deseado siguiente manera:
if ((yg!=0)&&(xg!=0))       
{ xoi cos(θ) -sen(θ) xob x
if ((abs(y_r)<abs(yg*1.2))&&(abs(y_r)> = + r
yoi sen(θ) cos(θ) yob yr
abs(yg*0.8))&&
(abs(x_r)<abs(xg*1.2))&&(abs(x_r)> Reemplazando los valores de la tabla I se tienen los
abs(xg*0.8))) siguientes valores:
{
stop=1; • Sensor 1
} xob1 = 0.707d1 + 0.24 (23)
}
else yob1 = 0.707d1 + 0.065 (24)
{
if (yg==0) • Sensor 2
{
xob2 = d2 + 0.275 (25)
 yob2 = 0 (26) #include <18f4550.h>
#device ADC=10
• Sensor 3 #fuses INTRC_IO, NOWDT, PUT ,BROWNOUT, NOLVP,
CPUDIV1
xob3 = 0.707d3 + 0.24 (27) #use delay(clock=8000000)
#use rs232(baud=9600,parity=N,xmit=PIN_C6,
rcv=PIN_C7,bits=8)
yob2 = −0.707d3 − 0.065 (28) #include <stdlib.h>
#INCLUDE <math.h>
Las coordenadas en X y Y que aporta cada sensor son:
En el main se inicializan los valores y se realizar las
xoii = cos(θ)xobi − sen(θ)yobi + xr (29) configuraciones de las herramientas a utilizar:

yoii = sen(θ)xobi − cos(θ)yobi + xr (30) void main()

{
Las coordenadas generales del vector de desvı́o están %asignar puertos de salida
dadas por: set_tris_b(0b11100111);
set_tris_a(0b00001111);
set_tris_d(0b00000000);
    %registros del timer
Uaox (xoi1 − xr ) + (xoi2 − xr ) + (xoi3 − xr ) Timer2=249;
=
Uaoy (yoi1 − yr ) + (yoi2 − yr ) + (yoi3 − yr ) Poscaler=1;
setup_timer_0(rtcc_internal|rtcc_div_8);
El vector de desvı́o estarı́a dado finalmente por la tangente setup_timer_1(t1_internal|t1_div_by_8);
setup_timer_2(t2_div_by_16,Timer2,Poscaler);
inversa entre Uaoy y Uaox . setup_timer_3(t3_internal|t3_div_by_8);
set_timer1(53036);
VI. ESTRUCTURA DEL C ÓDIGO %habilitar interrupciones externas y globales
enable_interrupts(int_ext);
A. Tiempo de muestreo enable_interrupts(global);
ext_int_edge(l_to_h);
Para que el sistema de control de velocidad tenga tiempo %configuracion pwm
setup_ccp1(ccp_pwm);
de muestreo de 50ms se hace uso de un timer previamente setup_ccp2(ccp_pwm);
configurado para cambiar una bandera que indique el fin de set_pwm1_duty(0);
este lapso. set_pwm2_duty(0);
%inicializar puertos de salida
output_high(pin_D2);
#int_timer1 output_low(pin_A1);
void timer_1(){ %configuracion ADC
tiempo=1; setup_adc(ADC_CLOCK_INTERNAL);
set_timer1(53036); //condicion para temporizador setup_adc_ports(AN0_TO_AN3);
50ms controlador inicio=0;
disable_interrupts(int_timer1); %habilitar interrupcion externa 1 y 2
} enable_interrupts(int_ext1);
enable_interrupts(int_ext2);
%inicializar valores del control de angulo
a=((kp*T)+(ki*T*T)+(kd))/T;
B. Puesta en marcha del vehiculo b=((kp*T)+kd)/T;
c=kd/T;
Para poner el carrito en marcha se hace uso de un switch %asignar angulo goal
que conecta la baterı́a al circuito, sin embargo para poder phioriginal=theta_r+(atan2((yg),(xg)));
ubicar el carrito con comodidad se equipa el sistema con
un pulsador que se toma en cuenta en el PIC como una D. División del ángulo deseado
interrupción externa, al ser pulsado el código genera un delay
Debido a que hubo problemas con ángulos mayores a 90
de 3 segundos y permite el comienzo del programa.
grados se optó por hacer una división de ángulos para que
poco a poco el carrito llegue al ángulo objetivo:
#int_ext
void inicioP(){
delay_ms(2000); void meta()
output_low(pin_D2); {
delay_ms(1000); %angulo deseado a partir de la combinacion ...
inicio=1; lineal de vectores
} phi=(alpha*phidesvio)+((1-alpha)*phioriginal);
%limitar phi
if (phi>(2*pi))
C. Configuración del PIC {
phi=phi-(2*pi);
Para comnezar el PIC debe saber que librerias y her- }
if (phi>pi)
ramientas se utilizaran, estas se definen al comienzo del {
código: phi=phi-(2*pi);
 } {
if (phi<(-2*pi)) distancia1=100;
{ }
phi=phi+(2*pi); %SENSOR 2
} %configurar canal del ADC
if (phi<-pi) set_adc_channel(2);
{ delay_us(20);
phi=phi+(2*pi); %leer valor de voltaje
} adcV=read_adc();
%caso en que el angulo es mayor a 90 %llevar el valor del ADC a voltaje
if (phi>(pi/2)&&(condicion==0)) vol2=(adcV*5)/1023;
{ if (vol2>0.2)
%asignar primero 90 grados {
theta_g=(pi/2); %aplicar ecuacion del sensor
%activar variable de 90 grados distancia2=25.446/vol2;
condicion=1; }
} else
%caso en que el angulo es menor a -90 {
else if (phi<(-pi/2)&&(condicion==0)) distancia2=100;
{ }
%asignar primero -90 grados %SENSOR 3
theta_g=(-pi/2); %configurar canal del ACD
%activar variable de 90 grados set_adc_channel(3);
condicion=1; delay_us(20);
} %leer valor de voltaje
%caso en que el angulo esta entre -90 y 90 adcV=read_adc();
else if (condicion==0) %llevar el valor del ADC a voltaje
{ vol3=(adcV*5)/1023;
%dejar el angulo objetivo igual if (vol3>0.2)
theta_g=phi; {
condicion=1; %aplicar la ecuacion del sensor
} distancia3=25.446/vol3;
%caso en que ya giro 90 grados }
if ((theta_r≥theta_g)&&(phi>(pi/2))) else
{ {
%asignar el angulo restante distancia3=100;
theta_g=phi; }
} }
%caso en que ya giro -90 grados
if ((theta_r≤theta_g)&&(phi<(-pi/2))) Ya teniendo las distancias de cada sensor se procede a
{ aplicar la evasión de obstáculos, es decir que para una
%asignar angulo restante
theta_g=phi; distancia menor a cierto valor se calcule el vector de desvı́o:
}
}
void distancias()
{
E. Evasión de obstáculos %SENSOR 1
if (distancia1<22)
Primero se debe obtener la lectura de los 3 sensores de {
distancia, para esto se configura cada canal del ADC y se %activar una variable ligada al sensor
sensor1=1;
aplica la ecuación dada en la Fig 9, para valores de voltaje %distancia al obstaculo
muy pequeños se asigna un valor de distancia de 100cm que xob1=(0.707*distancia1)+0.24;
posteriormente hará que el carrito solo se dirija al punto, la yob1=(0.707*distancia1)+0.065;
xoi1=(cos(theta_r)*xob1)-(sin(theta_r)*yob1)
interpretación de los valores del sensor re lleva a cabo en el +x_r;
código de la siguiete manera: yoi1=(sin(theta_r)*xob1)+(cos(theta_r)*yob1)
+y_r;
%prender LED
void instru() output_high(pin_D0);
{ }
%SENSOR 1 (izquierdo) else
%configurar canal del ADC {
set_adc_channel(0); %desactivar variable ligada al sensor
delay_us(20); sensor1=0;
%leer valor de voltaje %igualar X y Y del o b s t c u l o a valores ...
adcV=read_adc(); reales
%llevar el valor del ADC a voltaje xoi1=x_r;
vol1=(adcV*5)/1023; yoi1=y_r;
if (vol1>0.2) %apagar LED
{ output_low(pin_D0);
%aplicar ecuacion del sensor }
distancia1=25.446/vol1;
} %SENSOR 2 (frente)
else if (distancia2<35)
 { Uaox=(xoi1-x_r)+(xoi2-x_r)+(xoi3-x_r);
%activar variable ligada al sensor Uaoy=(yoi1-y_r)+(yoi2-y_r)+(yoi3-y_r);
sensor2=1; phidesvio=-pi+(atan2((Uaoy),(Uaox)));
%distancia al obstaculo condicion=0;
xob2=distancia2+0.275; condicion2=1;
yob2=0; %aumentar la velocidad lineal
xoi2=(cos(theta_r)*xob2)-(sin(theta_r)*yob2)+x_r; vd=0.2;
yoi2=(sin(theta_r)*xob2)+(cos(theta_r)*yob2)+y_r; alpha=0.5;
%prender LED }
output_high(pin_D1); else
} {
else %disminuir velocidad lineal y borrar vector ...
{ desvio
%desactivar variable ligada al sensor vd=0.1;
sensor2=0; phidesvio=0;
%igualar X y Y del obstaculo a valores reales alpha=0;
xoi2=x_r; }
yoi2=y_r; }
%apagar LED
output_low(pin_D1);
} F. Evasión de abismos
Para evitar que el carrito se caiga por una escalera o
%SENSOR3 (derecho)
if (distancia3<22) simplemente un ”abismo” se tiene un sensor digital FC-51
{ que tiene una inclinación total de 90 grados hacia el piso y
%activar varible ligada al sensor envı́a un valor de ’1’ lógico si no detecta nada al alcance, o
sensor3=1;
%distancia al obstaculo de lo contrario un ’0’ lógico, el código implementado es el
xob3=(0.707*distancia3)+0.24; siguiente:
yob3=(-0.707*distancia3)-0.065;
xoi3=(cos(theta_r)*xob3)-(sin(theta_r)*yob3)+x_r;
yoi3=(sin(theta_r)*xob3)+(cos(theta_r)*yob3)+y_r;while (input(pin_B3)==1)
%prender LED {
output_high(pin_D3); set_pwm1_duty(0);
} set_pwm2_duty(0);
else }
{
%desactivar variable ligada al obstaculo
sensor3=0; G. Código final
%igualar X y Y del obstaculo a valores reales Por último se tiene un while infinito que recopila todo
xoi3=x_r;
yoi3=y_r; lo mencionado anteriormente, para comenzar no se ejecuta
%apagar LED nada hasta que el pulsador es presionado, luego se toma
output_low(pin_D3); en cuenta la variable stop para detenerse en caso de que
}
%asignar a una variable si se detecta uno o ... ya se haya llegado al punto objetivo, para observar el buen
varios sensores funcionamiento se cambia el valor de salida asignado a un
sensores=sensor1+sensor2+sensor3; LED que indica que el código no está en un bucle infinito,
}
posteriormente se llama a todas las funciones explicadas
anteriormente en el orden indicado para obtener el resultado
Por último se asigna el vector de desvı́o dependiendo de si
deseado, después se envı́a al pin serial la información que
se detecta o no alguno o varios sensores, se asignan ciertas
el dispositivo bluetooth HC-05 debe enviar al computador
condiciones que permiten ”dividir” el ángulo deseado, es
para la visualización del recorrido en matlab, y finalmente
decir, si se desea ir a 120 grados primero se pone como
se espera a que pasen los 50ms para volver a ejecutar todo
objetivo 90 grados y luego el valor restante, y por último se
de nuevo; el código implementado es:
evalua si hay o no detección de sensores para incluir o no
el vector de desvı́o:
%no comenzar hasta que inicio sea 1
while (inicio==0);
void sensado() %poner registro del timer
{ set_timer1(53036);
%condicion para dividir el angulo while(true) {
if (condicion2==1 && sensores==0) %frenar si hay abismo
{ while (input(pin_B3)==1)
condicion=0; {
condicion2=0; set_pwm1_duty(0);
%asignar angulo nuevo set_pwm2_duty(0);
phioriginal=atan2((yg-y_r),(xg-x_r)); }
} %frenar si se llega al punto
%deteccion de sensores while (stop==1)
if (sensores≥1) {
{ set_pwm1_duty(0);
%calcular vector desvio set_pwm2_duty(0);
 } if (strcmp((char(datos(1,1))),'x'))
%reiniciar tiempo de 50ms %guardar el valor en x
tiempo=0; x = str2double(char(datos(1,2)));
enable_interrupts(int_timer1); %incrementar contador
%incrementar variable que evita lecturas ... contador=contador+1;
erroneas end
if (coni<10) %mirar si envio coordenada "Y"
{ if (strcmp((char(datos(1,1))),'y'))
coni++; %guardar el valor en y
} y = str2double(char(datos(1,2)));
%prender LED %incrementar contador
output_toggle(pin_D2); contador=contador+1;
%obtener promedio de velocidades end
velocidades();
%leer sensores end
instru(); %graficar x contra y
%obtener distancias al obstaculo plot(x,y,'r*');
distancias(); %dibujar en tiempo real
%generar vector de desvio xlim auto
sensado(); ylim auto
%dividir angulo axis equal
meta(); hold on
%controlador de angulo drawnow
contrAng(); %reiniciar contador
%controles de velocidad contador=0;
controladorDer(); end
controladorIzq();
%asignar valores de PWM VIII. RESULTADOS
set_pwm1_duty(duty1);
set_pwm2_duty(duty2); A continuación, se mostrarán los resultados tanto fı́sicos
% e n v o de datos serial como de algoritmo:
printf("x:%5.2f\n",x_r);
delay_ms(2); A. Montaje
printf("y:%5.2f\n",y_r);
delay_ms(2); Para realizar el circuito impreso para la baquelita se
%esperar a los 50ms
while(tiempo==0);
utilizó el programa de simulación de PROTEUS, el cual nos
} facilita el proceso gracias a sus amplias librerı́as para realizar
PCB. Una vez diseñado el circuito, en el cual también se
implementó el circuito que permite al robot avanzar tanto
VII. TRANSMISI ÓN DE LA TRAYECTORIA
hacia adelante como hacia atrás que nos servirá en futuras
ocasiones, se procede a realizar la técnica de planchado sobre
%obtener el HC-05 como un objeto la baquelita dejando ası́ resultados más que satisfactorios.
b = Bluetooth('HC-05',1);
%premitir transmisi n de datos
fopen(b);
%tiempo de espera
pause(2);
%configuraci n g r f i c a
figure('Name','Proyecto 1 Robotica')
title('Ubicacion Espacial del Robot');
xlabel('Posicion en X');
ylabel('Posicion en Y');
grid on;
hold on;
%inicializar variables
x=0;
y=0;
angu=0;
A=0;
B=0;
Ba=0; Fig. 12. Circuito diseñado en PROTEUS
contador=0;
while (1)
%debe recibir dos valores (X y Y) para salir ...
del while B. Transmisión de la trayectoria graficada en Matlab
while (contador6=2) Para realizar estas pruebas, se definió como coordenada
%Toma el valor recibido por el puerto y lo ...
guarda en la variable de llegada 2 metros en el eje x y 1 metro en el eje y. La
valor=fscanf(b,'%c',7); primera prueba se realizó sin obstaculizar la trayectoria del
%Dividir el string "valor" y el numero que ... vehı́culo y la segunda prueba, colocando un obstáculo y de
llega con ":"
datos = strsplit(valor,':') esta forma, comparar sus trayectorias. Se puede observar en
%mirar si envio coordenada "x" la Fig. 15 y en la Fig. 16 respectivamente.
 Fig. 16. Trayectoria del vehiculo con obstaculo

Fig. 13. Vista 3D de la PCB terminada

IX. CONCLUSIONES
• En anteriores oportunidades se habı́a caracterizado cada
motor con la herramienta PID TUNER de Matlab, pero
dicha caracterización dejaba un error bastante grande,
lo cual impedı́a que los controladores implementados
para cada motor realizaran un trabajo como lo esperado.
Gracias a la función de ARX de Matlab, se logró carac-
terizar cada motor de una forma mucho más precisa y
llevando el error casi a cero, con lo cual se obtuvo unos
controladores que realizaron un trabajo muy cercano a
lo esperado.
• En pruebas realizadas se observó que, en algunos mo-
mentos, cada Encoder enviaba una información errónea,
con lo cual se decidió protegerlos con una caja que lo
cubre totalmente y de esta forma disminuir cualquier
interferencia provocada por otras fuentes de luz. Adi-
Fig. 14. Prototipo definitivo del vehı́culo cional a esto, se optó por trabajar con el promedio de
los últimos cuatro valores enviados por el Encoder y
ası́, poder disminuir el ruido en la señal de frecuencia.
Este proceso ayudó de gran manera a los controladores
de velocidad de cada motor.
• Realizando mediciones de voltaje en el anteriormente
implementado puente H L293D se pudo observar que
estaba consumiendo un voltaje importante, razón por
la cual los motores no estaban llegando a la velocidad
nominal. Teniendo en cuenta esto, se trató de imple-
mentar la etapa de potencia con un transistor IRF840
en corte y saturación para cada motor, pero después de
unas cuantas pruebas se observó que un transisteor tenı́a
mayor tiempo muerto que el otro. Por último, se optó
por implementar el modulo puente H L298N, el cual ha
tenido un buen comportamiento, aportando mucha más
velocidad a los motores.
• Se observó que en algunas pruebas, las ruedas del
vehı́culo presentaban un derrape, lo cual provocaba que
Fig. 15. Trayectoria del vehiculo sin obstaculo el robot no tenga una buena percepción de su ubicación
actual. Debido a esto, se optó por forrar cada rueda con
un globo, lo que ayuda a que las ruedas ya no presenten
 ningún derrape.
• El funcionamiento del robot se ve afectado por lugares
con iluminación solar debido a la emisión de rayos
infrarrojos que interfieren con los sensores, ya que estos
basan su funcionamiento en Leds infrarrojos, ası́ mismo
los pisos que reflejan la luz también proporcionan in-
convenientes a los sensores lo cual hace que en algunas
pruebas su funcionamiento sea totalmente erróneo.
• Realizando las pruebas pertinentes para establecer un
rango de parada, se decide implementar un rango del
20 % de la distancia en cada coordenada, ya que si se
trata disminuir dicho rango cabe la posibilidad que el
robot no cumpla con las dos condiciones de coordenadas
y de esta forma no detenerse.
• Al intentar ser más preciso en la posición final del
vehı́culo, se opta en primera instancia por recalcular
el ángulo de destino en caso de que el robot haya
sobrepasado el punto de meta en cualquier coordenada
y de esta forma devolverse un poco. Esto generó que, en
esos casos, el vehı́culo realice giros sin poder encontrar
el punto de meta debido a no poder hacer vueltas
cerradas.
• Al realizar pruebas con diversas velocidades, se decide
implementar una velocidad lineal baja para dirigirse
hacia la meta ya que de esta forma se presenciaba
menos error en su trayectoria. Por otro lado, cuando el
prototipo detectaba un obstáculo, al tratar de esquivarlo
chocaba ya que su velocidad lineal no era suficien-
temente alta como para cambiar su trayectoria en el
menor tiempo posible, debido a esto, se decide aumentar
su velocidad lineal solo en el caso que se detecte un
obstáculo.
• Se observó que cuando se pretendı́a llegar a puntos
donde el ángulo de meta era superior a 90 grados o
inferior a -90 grados, el robot no lograba estabilizar
su ángulo, lo cual también impedı́a que el prototipo
evada algún obstáculo. Para solucionar este problema,
se decidió que en caso de que el ángulo de meta no
esté entre el rango de -90 a 90 grados, se dividiera el
ángulo y de esta forma, alcanzar en primera instancia
ya sea el ángulo de -90 o de 90 grados y posteriormente
completar el ángulo de meta.
X. B IBLIOGRAF ÍA
• Sistemas de control en tiempo discreto - Ogatha, 2da
Edición.
• Manual para programación de microcontrolador PIC en
CCS
• Diapositivas ”Navegación del Robot Móvil” - Diana
Marcela Ovalle Martinez.