UNIVERSIDAD NACIONAL

DE SAN ANTONIO ABAD

DEL CUSCO

FACULTAD DE INGENIERIA ELCTRICA,

ELECTRNICA, INFORMTICA Y MECNICA

ESCUELA PROFESIONAL DE INGENIERA

INFORMTICA Y DE SISTEMAS

Reconocimiento de obstculos del

4WD por medio de cmara del
celular
CURSO: Robotica y Procesamientp de Seales
Docente: Ing. Jos Mauro Pillco Quispe
ALUMNOS:
Atausinchi Fernandez, Richarth (022155)
Choque Velasquez, Ramiro (080211)
Quispe Loaiza, Brand Orion (131092)
Garrafa Villanta, Fernando Daniel (030424)

Cusco Per
2017 - I

 CONTROL POR BLUETOOTH
I. Diseo de circuito
En el presente trabajo daremos a conocer cmo se realiza el armado del carrito y tambin
cmo est hecho el cdigo para su funcionamiento para el manejo.
Materiales
Una estructura para dar forma al carrito
Arduino
4 motores
Un controlador de motores L298N
1 Bluetooth
Cables
4 ruedas (llantas)
1 Protoboard
1 sensor de proximidad

ARMADO DEL CARRITO

Para el desarrollo de este se hace las siguientes conexiones

1. Colocamos las 4 ruedas en los 4 lugares conocidos del carro en la estructura escogida
para hacer el trabajo.
2. Luego hacemos las conexiones de las ruedas con cada uno de los 4 motores de
preferencia lo soldamos para un mejor funcionamiento.
3. Una vez acopladas las ruedas conectamos los cables de cada par de motores al driver
o controlador, el primer par al controlador Motor A y el segundo par al controlador
motor B.
4. Una vez hecha las conexiones de los motores hacemos las conexiones de las bateras
la cual estar conectado al controlador de motores L298N para suministrar energa.
Alimentar con la batera a la entrada de 6-12V y GND, despus sacar cables de las
entradas GND y 5V para alimentar al Protoboard.
5. El controlador de suministros es muy importante, puesto es quien dar y har las
instrucciones a los motores tanto para la velocidad como para los giros que pueden ser
a la derecha, izquierda, adelante y atrs.
6. El Controlador L298N a su vez de hacer las conexiones tambin se hace conexiones
de los pines de este controlador que van hacia el arduino como son ENA, IN1, IN2,
que son para el manejo del primer par de motores, tambin el del segundo para de
motores que son ENB, IN3,IN4.
7. Luego de hacer la conexiones del controlador de al arduino en los pines escogidos nos
vamos a realizar la conexin del bluetooth y para ello tenemos tener muy en cuenta
los 4 pines de conexin que tiene nuestro bluetooth como son RX, TX, y los ms
conocido que son VCC, GND.
a. Para un mejor manejo del bluetooth se puede configurar cambiar el nombre y
poder as ubicar entre muchos el nuestro.
8. Colocar el sensor de proximidad en la parte Fontal del carrito, y conectar el ping trig
al puerto 8 y el echo al puerto 9
 9. Por ltimo tener en cuenta las conexiones del arduino como son el de 5V y el GND
(tierra).

II. Cdigo fuente ARDUINO

#include <Ultrasonic.h>

#define trigPin 8
#define echoPin 9
int message = 'g';
int motor1Pin1 = 2; // pin 2 on L293B
int motor1Pin2 = 3; // pin 7 on L293B
int motor1Pin3 = 4; // pin 10 on L293B
int motor2Pin1 = 5; // pin 15 on L293B
int motor2Pin2 = 6; // pin 10 on L293B
int motor2Pin3 = 7; // pin 15 on L293B
Ultrasonic ultrasonido(trigPin,echoPin);

void setup() {
//pinMode(13, OUTPUT); // Declaramos que utilizaremos el pin 13 como salida
Serial.begin(9600); //Se inicia la comunicacin serie
pinMode(motor1Pin1,OUTPUT);
pinMode(motor1Pin2,OUTPUT);
pinMode(motor1Pin3,OUTPUT);
pinMode(motor2Pin1,OUTPUT);
pinMode(motor2Pin2,OUTPUT);
pinMode(motor2Pin3,OUTPUT);
}

void loop() {
message = Serial.read();
int d=ultrasonido.Ranging(CM);
if(d>50){
seguir(message);
}
else{
parar();
atras();
}
}
//ADELANTE
void seguir(int message){
if(message=='w'){
digitalWrite(motor1Pin2,LOW);
digitalWrite(motor1Pin1,HIGH);
digitalWrite(motor1Pin3,HIGH);
digitalWrite(motor2Pin1,HIGH);
 digitalWrite(motor2Pin2,LOW);
digitalWrite(motor2Pin3,HIGH);
}
//ATRAS
if(message=='s'){
digitalWrite(motor1Pin2,HIGH);
digitalWrite(motor1Pin1,LOW);
digitalWrite(motor1Pin3,HIGH);
digitalWrite(motor2Pin1,HIGH);
digitalWrite(motor2Pin2,HIGH);
digitalWrite(motor2Pin3,LOW);
}
//ADELANTE Izq
if(message=='a'){
digitalWrite(motor1Pin2,LOW);
digitalWrite(motor1Pin1,HIGH);
digitalWrite(motor1Pin3,HIGH);

digitalWrite(motor2Pin1,HIGH);
digitalWrite(motor2Pin2,HIGH);
digitalWrite(motor2Pin3,LOW);
delay(250);
parar();
}
//ADELANTE der
if(message=='d'){

digitalWrite(motor1Pin2,HIGH);
digitalWrite(motor1Pin1,LOW);
digitalWrite(motor1Pin3,HIGH);

digitalWrite(motor2Pin1,HIGH);
digitalWrite(motor2Pin2,LOW);
digitalWrite(motor2Pin3,HIGH);
delay(250);
parar();
}
if(message=='p'){ // Boton Parar
digitalWrite(motor1Pin2,LOW);
digitalWrite(motor1Pin1,LOW);
digitalWrite(motor1Pin3,LOW);
digitalWrite(motor2Pin1,LOW);
digitalWrite(motor2Pin2,LOW);
digitalWrite(motor2Pin3,LOW);
}
}
void parar()
{
 int d1=0;
int d2=0;

digitalWrite(motor1Pin2,LOW);
digitalWrite(motor1Pin1,LOW);
digitalWrite(motor1Pin3,LOW);
digitalWrite(motor2Pin1,LOW);
digitalWrite(motor2Pin2,LOW);
digitalWrite(motor2Pin3,LOW);

}
void atras()
{
digitalWrite(motor1Pin2,HIGH);
digitalWrite(motor1Pin1,LOW);
digitalWrite(motor1Pin3,HIGH);
digitalWrite(motor2Pin1,HIGH);
digitalWrite(motor2Pin2,HIGH);
digitalWrite(motor2Pin3,LOW);
delay(100);
parar();
}

III. Cdigo fuente PARA RECONOCIMIENTO DE OBSTACULOS

CODIGO PYTHON

# python robo_pelota.py --ip 192.168.0.15

# importamos los paquetes necessarios

import numpy as np
import imutils
import cv2
import math
import serial
import threading
import time
import urllib # Para leer imagen desde la URL

# import serial

# Conectamos el dispositivo bluetooth

# ser = serial.Serial('COM4', 9600)
# Construct the argument parse and parse the arguments
# Construye el argumento para luego ser analizado
# parametros
# -------------------------------------- CONFIGURACION ---------------------------------------------
---
ANCHO = 900
fuente = cv2.FONT_HERSHEY_SIMPLEX
dispBLUE = '-' # Cargando bluetooh serial
puertoCOM = 'COM4'
rutaIP = "http://192.168.137.201:4747/mjpegfeed?960x720" # 320x240 640x480 960x720
1280x720 1920x1080
# -----------------------------------------------------------------------------------------------------
# extra
inf0 = {'rojo': (166, 84, 141), 'verde': (66, 122, 129), 'azul': (97, 100, 117), 'amarillo': (23, 59,
119)}
sup0 = {'rojo': (186, 255, 255), 'verde': (86, 255, 255), 'azul': (117, 255, 255), 'amarillo': (54,
255, 255)}

# define los lmites inferior y superior de los colores en el espacio de color HSV
inf3 = {'rojo': (166, 84, 141)}
sup3 = {'rojo': (186, 255, 255)}

inf = {'amarillo': (23, 59, 119)}

sup = {'amarillo': (54, 255, 255)}

inf2 = {'azul': (97, 100, 117)}

sup2 = {'azul': (117, 255, 255)}

# define colores estndar para el crculo alrededor del objeto

colores = {'rojo': (0, 0, 255), 'verde': (0, 255, 0), 'azul': (255, 0, 0), 'amarillo': (0, 255, 217)}

# modulo conectar dispositivo bluetooh

def conectar_blue():
try:
print("Conectando antena bluetooth de ROBOCAR...")
try:
disp = serial.Serial(puertoCOM, 9600)
print("Bluetooth conectado satisfactoriamente")
return disp
except Exception as e:
raise e
except Exception as e:
print("No se pudo conectar")
# Modulo enviar instruccion a arduino
# tecla: avance Ejm: w=avance, a=izquierda, d=derecha, s=abajo, p=detenerse, g=avance
rapido
# num: iteraciones
def cmd_blue(tecla, num=1):
for i in range(1, num):
dispBLUE.write(tecla.encode())

# pts = deque(maxlen=args["buffer"])
def distancia_camara_objeto(aTamFotoPix, aTamObjPix):
longitud_focal = 4.235 # en mm
tam_real_obj = 7 * 10 # tamao real del objeto en mm( 7cm pelota *10=70mm)
alto_sensor = 3.52 # altura del sensor en px
tam_foto = aTamFotoPix # tamao de la imagen en pixeles
# calcula y retorna la distancia desde la imagen a la camara
distancia = (longitud_focal * tam_real_obj * tam_foto) / (aTamObjPix * alto_sensor)
return distancia

def angulo_puntos(px, py, px2, py2):

val = (py2 - py) / (px2 - px) # calculate slope between the two points
val = val - pow(val, 3) / 3 + pow(val, 5) / 5 # find arc tan of the slope using taylor series
approximation
val = ((int)(val * 180 / 3.14)) % 360 # Convert the angle in radians to degrees
if (px2 < px):
val = val + 180
if (val < 0):
val = 360 + val
return val # si no se ha suministrado una ruta de vdeo, tome la referencia a la webcam

# Dispositivo - Camara
camara = cv2.VideoCapture(rutaIP)
# sigue ejecutando el bucle
dispBLUE = conectar_blue()
cont_uno = 0
dist_avance1 = 0
dist = 0
while True:
# toma el frame actual
 (tomado, frame) = camara.read()

# si estamos viendo un video y no hemos tomado un fotograma,

# entonces hemos llegado al final del video
# if args.get("video") and not tomado:
# break

# Transmisin de imgenes IP webcam

# URL = 'http://10.254.254.102:8080/shot.jpg'
# urllib.urlretrieve(URL, 'shot1.jpg')
# frame = cv2.imread('shot1.jpg')

# tomala la captura
# cv2.imwrite('cap_robo.jpg', frame)

# cambia el tamao del frame, desenfoca(difuminado) y lo convierte este al espacio de

colores HSV
frame = imutils.resize(frame, width=ANCHO)

difuminado = cv2.GaussianBlur(frame, (11, 11), 0)

hsv = cv2.cvtColor(difuminado, cv2.COLOR_BGR2HSV)
# escala de grises
grises = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
# para cada color en el dicionario verifica el objeto en el frame
cx = 0
cy = 0
# Contador de amarillo comando
# ----------------------------- PELOTA 1---------------------------------------------------
for key, value in sup.items():
# construye una mascara para los colores del diccionario luego realiza
# una serie de delataciones y erosiones para eliminar las pequeas
# manchas(ruido) dejadas en la mascara
kernel = np.ones((9, 9), np.uint8)
mascara = cv2.inRange(hsv, inf[key], sup[key])
mascara = cv2.morphologyEx(mascara, cv2.MORPH_OPEN, kernel)
mascara = cv2.morphologyEx(mascara, cv2.MORPH_CLOSE, kernel)

# encuentra el contorno en la mascara e inicializa el actual

# (x, y) centro de la pelota
contornos = cv2.findContours(mascara.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)[-2]

# solo se ejecuta si al menos un contorno fue encontrado

 if len(contornos) > 0:
# encuentra el contorno ms grande en la mscara, luego lo
# utilza para calcular el crculo mnimo de inclusin y el centroide del objecto(pelota)
c = max(contornos, key=cv2.contourArea)
((x, y), radio) = cv2.minEnclosingCircle(c)
M = cv2.moments(c)
# centro = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
cx, cy = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))

# solo procede si el radio cumple un tamao mnimo.

# Corregir este valor para el tamao de nuestro objeto, tamao deseado
if radio > 0.5:
# dibuja el circulos y el centroide en el frame,
# luego actualiza los puntos de rastreo
cv2.circle(frame, (int(x), int(y)), int(radio), colores[key], 2)
cv2.circle(frame, (int(x), int(y)), 1, colores[key], 2)
# cv2.line(frame, (int(30), int(30)), (int(x), int(y)), colores[key], 2)
cv2.line(frame, (cx, cy), ((800, 800)[0], cy), (255, 255, 255), 1, cv2.LINE_AA)
cv2.putText(frame, str(cx) + "," + str(cy), (cx, cy + 20), fuente, 0.4, (255, 255, 0),
1, cv2.LINE_AA)
# dist = distancia_camara_objeto(aTamFotoPix, aTamObjPix, aTamRealObj)
dist = (distancia_camara_objeto(camara.get(4), (radio * 2))) / 10
cv2.putText(frame, "Dist: " + str(int(dist)) + " cm", (int(cx), int(cy) + 40), fuente,
0.6,
(255, 255, 0), 2, cv2.LINE_AA)
if dist > 40:
cmd_blue('d', 2)
cont_uno = cont_uno + 1
print(dist)

else:
cmd_blue('w', 2)

# cmd_blue('w', 2)
# cmd_blue('a', 3)
if cont_uno > 4:
print('Dist', dist)
print('Cont_uno', cont_uno)
numerador = dist * (math.cos((cont_uno * 7)*math.pi/180))
print(numerador)
dist_avance1 = int((numerador / 4.3) + 10)
print('B1', dist_avance1)
cont_uno = 0
 if dist_avance1 > 0:
cmd_blue('w', dist_avance1)
print('B2', dist_avance1)
cmd_blue('a', 15)
cmd_blue('w', 18)
# -----------------------------------------------------------------------------------------
# ----------------------------- PELOTA 2---------------------------------------------------
for key, value in sup2.items():
# construye una mascara para los colores del diccionario luego realiza
# una serie de delataciones y erosiones para eliminar las pequeas
# manchas(ruido) dejadas en la mascara
kernel = np.ones((9, 9), np.uint8)
mascara = cv2.inRange(hsv, inf2[key], sup2[key])
mascara = cv2.morphologyEx(mascara, cv2.MORPH_OPEN, kernel)
mascara = cv2.morphologyEx(mascara, cv2.MORPH_CLOSE, kernel)

# encuentra el contorno en la mascara e inicializa el actual

# (x, y) centro de la pelota
contornos = cv2.findContours(mascara.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)[-2]
centro = None

# solo se ejecuta si al menos un contorno fue encontrado

if len(contornos) > 0:
# encuentra el contorno ms grande en la mscara, luego lo
# utilza para calcular el crculo mnimo de inclusin y el centroide del objecto(pelota)
c = max(contornos, key=cv2.contourArea)
((x2, y2), radio2) = cv2.minEnclosingCircle(c)
M = cv2.moments(c)
# centro = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
cx2, cy2 = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))

# solo procede si el radio cumple un tamao mnimo.

# Corregir este valor para el tamao de nuestro objeto, tamao deseado
if radio2 > 0.5:
# dibuja el circulos y el centroide en el frame,
# luego actualiza los puntos de rastreo
cv2.circle(frame, (int(x2), int(y2)), int(radio2), colores[key], 2)
cv2.circle(frame, (int(x2), int(y2)), 1, colores[key], 2)
cv2.line(frame, (cx, cy), (cx2, cy2), (255, 255, 255), 1, cv2.LINE_AA)
cv2.putText(frame, str(cx2) + "," + str(cy2), (cx2, cy2 + 20), fuente, 0.4, (255, 255,
0), 1,
cv2.LINE_AA)
 # calculando el angulo
# -------------------------------------------
cx = float(cx)
cy = float(cy)
cx2 = float(cx2)
cy2 = float(cy2)
angulo = int(math.atan2((cy - cy2), (cx2 - cx)) * 180 // math.pi)
cv2.putText(frame, str(angulo) + " grados", (int(cx) - 30, int(cy) - 30), fuente, 1, (0,
255, 0), 2,
cv2.LINE_AA)
# dist = distancia_camara_objeto(aTamFotoPix, aTamObjPix, aTamRealObj)
dist2 = (distancia_camara_objeto(camara.get(4), (radio2 * 2))) / 10
cv2.putText(frame, "Dist: " + str(int(dist2)) + " cm", (int(cx2), int(cy2) + 40),
fuente, 0.6,
(255, 255, 0), 2, cv2.LINE_AA)
# -------------------------------------------
# -----------------------------------------------------------------------------------------
# Muestra el frame en nuestra pantalla
cv2.imshow("Pelotines :)", frame)

tecla = cv2.waitKey(1) & 0xFF

# si se presiona la teclado 'q': se detiene el bucle
if tecla == ord("q"):
break

# limpia la cmara y cerramos las ventanas abiertas

camara.release()
cv2.destroyAllWindows()

IV. Imagenes
V. Link del video

https://youtu.be/bHZzKxbRoZo