Diferencia entre revisiones de «Linea camara»

De Proyecto Butiá
Saltar a: navegación, buscar
Línea 12: Línea 12:
 
===Código===
 
===Código===
 
<source lang="python">
 
<source lang="python">
−
def main():
+
import pygame
−
    print "Hello World!"
+
import pygame.camera
 +
import pygame.surfarray as surfarray
 +
import sys
 +
sys.path.insert(0, '/usr/share/sugar/activities/TurtleBots.activity/plugins/butia')
 +
from pygame.locals import *
 +
from pybot import usb4butia
 +
import time
 +
 
 +
pygame.init()
 +
pygame.camera.init()
 +
robot = usb4butia.USB4Butia()
 +
robot.refresh()
 +
 
 +
def saveSurface(pixels, filename):
 +
try:
 +
surf = pygame.surfarray.make_surface(pixels)
 +
except IndexError:
 +
(width, height, colours) = pixels.shape
 +
surf = pygame.display.set_mode((width, height))
 +
pygame.surfarray.blit_array(surf, pixels)
 +
pygame.image.save(surf, filename)
 +
 
 +
def capturarImg(camara):
 +
camara.start()
 +
im = camara.get_image()
 +
camara.stop()
 +
return im
 +
 
 +
def inclinaciones(pixeles):
 +
i = 0
 +
l = largoLista(pixeles)
 +
l = l - 1
 +
incl = []
 +
while i < l:
 +
incl.append(inclinacion(pixeles[i],pixeles[i + 1]))
 +
i = i + 1
 +
return incl
 +
 
 +
def largoLista(lista):
 +
i = 0
 +
for a in lista:
 +
i = i + 1
 +
return i
 +
 
 +
def pendientesAfactores(pendientes):
 +
i = 0
 +
l = largoLista(pendientes)
 +
while i < l:
 +
if pendientes[i] > 30 or pendientes[i] == 0:
 +
pendientes[i] = 1
 +
elif pendientes[i] > 10:
 +
pendientes[i] = 1.1
 +
elif pendientes[i] > 5:
 +
pendientes[i] = 1.3
 +
elif pendientes[i] > 2:
 +
pendientes[i] = 1.5
 +
elif pendientes[i] > 0:
 +
pendientes[i] = 1.7
 +
elif pendientes[i] < -30:
 +
pendientes[i] = 1
 +
elif pendientes[i] < -10:
 +
pendientes[i] = 0.9
 +
elif pendientes[i] < -5:
 +
pendientes[i] = 0.7
 +
elif pendientes[i] < -2:
 +
pendientes[i] = 0.5
 +
else:
 +
pendientes[i] = 0.3
 +
i += 1
 +
return pendientes
 +
 
 +
def calculoFactorVelocidad(pendientes):
 +
suma = float(0)
 +
largo = largoLista(pendientes)
 +
for i in range(0, largo -1):
 +
suma += pendientes[i]
 +
suma = suma / largo
 +
return suma
 +
 
 +
def inclinacion((x1,y1),(x2,y2)):
 +
if x2 == x1:
 +
m = 0
 +
else:
 +
m = (y2 - y1) / (x2 - x1)
 +
return m
 +
 
 +
def movimientoRobot(robot, f):
 +
if f > 1:
 +
if f == 2:
 +
f = 0
 +
else:
 +
f = f - 1
 +
f = 1 - f
 +
velocidadDerecha = 200 * f
 +
velocidadIzquierda = 200
 +
else:
 +
velocidadDerecha = 200
 +
velocidadIzquierda = 200 * f
 +
 
 +
robot.set2MotorSpeed(1,int(velocidadDerecha),1,int(velocidadIzquierda))
 +
return 0
 +
 
 +
def hallarPixelesIma(im, cant):
 +
ima = surfarray.array3d(im)
 +
pixeles = []
 +
w = im.get_width()
 +
h = im.get_height()
 +
x = h - 1
 +
while x >= 0:
 +
i = 0
 +
while i < w and (int(ima[i][x][0]) + int(ima[i][x][1]) + int(ima[i][x][2])) / 3 > 120:
 +
i = i + 1
 +
if i < w:
 +
j = w - 1
 +
while j > 0 and (int(ima[j][x][0]) + int(ima[j][x][1]) + int(ima[j][x][2])) / 3 > 120:
 +
j = j - 1
 +
y = (i + j) / 2
 +
pixeles.append((y,x))
 +
x = x - 30
 +
else:
 +
x = x - 1
 +
return pixeles
 +
 
 +
def estaCentrado((x,y)):
 +
return (x > 50 and x < 270)
 +
 
 +
def izquierda((x,y)):
 +
return (x < 50)
 +
 
 +
def derecha((x,y)):
 +
return (x > 270)
 +
 
 +
def calculoFactorDescentrado(p):
 +
factorDescentrado = 1
 +
hayCentrados = False;
 +
i = 0
 +
cantPixeles = largoLista(p)
 +
while i < cantPixeles and not hayCentrados:
 +
if estaCentrado(p[i]):
 +
hayCentrado = True
 +
i += 1
 +
if(not hayCentrados):
 +
if(izquierda(p[0])):
 +
factorDescentrado = 1.9
 +
elif(derecha(p[0])):
 +
factorDescentrado = 0.1
 +
return factorDescentrado
 +
 
 +
def obtenerX((x,y)):
 +
return x
 +
 
 +
def centradoFactor(k,p):
 +
xs = 0
 +
for pix in p:
 +
xs += obtenerX(pix)
 +
xs = xs / largoLista(p)
 +
if xs < 25:
 +
k = 1.9
 +
elif xs < 55:
 +
if k >= 1 and k < 1.6:
 +
k = k + 0.4
 +
elif k < 1:
 +
k = 1.5
 +
elif k < 1.7:
 +
k = k + 0.3
 +
elif k < 1.8:
 +
k = k + 0.2
 +
elif xs < 67:
 +
if k >= 1 and k < 1.7:
 +
k = k + 0.3
 +
elif k < 1:
 +
k = 1.3
 +
elif xs > 130:
 +
k = 0.1
 +
elif xs > 105:
 +
if k <= 1 and k > 0.4:
 +
k = k - 0.4
 +
elif k > 1:
 +
k = 0.5
 +
elif k > 0.3:
 +
k = k - 0.3
 +
elif k > 0.2:
 +
k= k - 0.2
 +
elif xs > 93:
 +
if k <= 1 and k > 0.3:
 +
k = k - 0.3
 +
elif k > 1:
 +
k  = 0.7
 +
return k
 +
 +
 
 +
camlist = pygame.camera.list_cameras()
 +
if camlist:
 +
while cantVeces < 500:
 +
s = capturarImg(pygame.camera.Camera(camlist[0],(160,120)))
 +
p = hallarPixelesIma(s,cantVeces)
 +
pendientes = inclinaciones(p)
 +
factores = pendientesAfactores(pendientes)
 +
k = 1
 +
if largoLista(p) == 0:
 +
robot.set2MotorSpeed(0,0,0,0)
 +
else:
 +
if largoLista(p) > 1:
 +
k = calculoFactorVelocidad(factores)
 +
k = centradoFactor(k,p)
 +
movimientoRobot(robot, k)
  
−
if __name__ == '__main__':
 
−
    main()
 
 
</source>
 
</source>
  

Revisión del 21:12 1 jul 2015

Introducción

Consiga: Realizar un seguidor de línea utilizando una cámara en lugar de un sensor de grises para captar el entorno.
Tutor: Federico Andrade
Grupo:

  1. Josefina Fasoli
  2. Gonzalo Herrera
  3. Camila Serena

Algoritmo

A partir de la imagen recabada por la cámara, se toman puntos del centro de la linea a seguir. Con los puntos identificados, se calculan las respectivas pendientes generadas, para luego promediarlas y transformar este nuevo valor en un factor f utilizado para calcular la velocidad de cada rueda, que permite que haga el movimiento correspondiente.

Código

import pygame
import pygame.camera
import pygame.surfarray as surfarray
import sys 
sys.path.insert(0, '/usr/share/sugar/activities/TurtleBots.activity/plugins/butia')
from pygame.locals import *
from pybot import usb4butia
import time

pygame.init()
pygame.camera.init()
robot = usb4butia.USB4Butia()
robot.refresh()

def saveSurface(pixels, filename):
	try:
		surf = pygame.surfarray.make_surface(pixels)
	except IndexError:
		(width, height, colours) = pixels.shape
		surf = pygame.display.set_mode((width, height))
		pygame.surfarray.blit_array(surf, pixels)
	pygame.image.save(surf, filename)

def capturarImg(camara):
	camara.start()
	im = camara.get_image()
	camara.stop()
	return im

def inclinaciones(pixeles):
	i = 0
	l = largoLista(pixeles)
	l = l - 1
	incl = []
	while i < l:
		incl.append(inclinacion(pixeles[i],pixeles[i + 1]))
		i = i + 1
	return incl

def largoLista(lista):
	i = 0
	for a in lista:
		i = i + 1
	return i

def pendientesAfactores(pendientes):
	i = 0
	l = largoLista(pendientes)
	while i < l:
		if pendientes[i] > 30 or pendientes[i] == 0:
			pendientes[i] = 1
		elif pendientes[i] > 10:
			pendientes[i] = 1.1
		elif pendientes[i] > 5:
			pendientes[i] = 1.3
		elif pendientes[i] > 2:
			pendientes[i] = 1.5
		elif pendientes[i] > 0:
			pendientes[i] = 1.7
		elif pendientes[i] < -30:
			pendientes[i] = 1
		elif pendientes[i] < -10:
			pendientes[i] = 0.9
		elif pendientes[i] < -5:
			pendientes[i] = 0.7
		elif pendientes[i] < -2:
			pendientes[i] = 0.5
		else:
			pendientes[i] = 0.3
		i += 1
	return pendientes

def calculoFactorVelocidad(pendientes):
	suma = float(0)
	largo = largoLista(pendientes)
	for i in range(0, largo -1):	
		suma += pendientes[i]
	suma = suma / largo
	return suma

def inclinacion((x1,y1),(x2,y2)):
	if x2 == x1:
		m = 0
	else:	
		m = (y2 - y1) / (x2 - x1)
	return m

def movimientoRobot(robot, f):
	if f > 1:
		if f == 2:
			f = 0
		else:
			f = f - 1
		f = 1 - f
		velocidadDerecha = 200 * f
		velocidadIzquierda = 200
	else:
		velocidadDerecha = 200
		velocidadIzquierda = 200 * f	

	robot.set2MotorSpeed(1,int(velocidadDerecha),1,int(velocidadIzquierda))
	return 0 

def hallarPixelesIma(im, cant):
	ima = surfarray.array3d(im)
	pixeles = []
	w = im.get_width()
	h = im.get_height()
	x = h - 1
	while x >= 0:
		i = 0
		while i < w and (int(ima[i][x][0]) + int(ima[i][x][1]) + int(ima[i][x][2])) / 3 > 120:
			i = i + 1
		if i < w:
			j = w - 1
			while j > 0 and (int(ima[j][x][0]) + int(ima[j][x][1]) + int(ima[j][x][2])) / 3 > 120:
				j = j - 1
			y = (i + j) / 2
			pixeles.append((y,x))
			x = x - 30
		else:
			x = x - 1
	return pixeles

def estaCentrado((x,y)):
	return (x > 50 and x < 270)

def izquierda((x,y)):
	 return (x < 50)

def derecha((x,y)):
	return (x > 270)

def calculoFactorDescentrado(p):
	factorDescentrado = 1
	hayCentrados = False;
	i = 0
	cantPixeles = largoLista(p)
	while i < cantPixeles and not hayCentrados:
		if estaCentrado(p[i]):
			hayCentrado = True
		i += 1
	if(not hayCentrados):
		if(izquierda(p[0])):
			factorDescentrado = 1.9
		elif(derecha(p[0])):
			factorDescentrado = 0.1
	return factorDescentrado	

def obtenerX((x,y)):
	return x

def centradoFactor(k,p):
	xs = 0
	for pix in p:
		xs += obtenerX(pix)
	xs = xs / largoLista(p)
	if xs < 25:
		 k = 1.9
	elif xs < 55:
		if k >= 1 and k < 1.6:
			k = k + 0.4
		elif k < 1:
			k = 1.5
		elif k < 1.7:
			k = k + 0.3
		elif k < 1.8:
			k = k + 0.2
	elif xs < 67:
		if k >= 1 and k < 1.7:
			k = k + 0.3
		elif k < 1:
			k = 1.3
	elif xs > 130:
		k = 0.1
	elif xs > 105:
		if k <= 1 and k > 0.4:
			k = k - 0.4
		elif k > 1:
			k = 0.5
		elif k > 0.3:
			k = k - 0.3
		elif k > 0.2:
			k= k - 0.2
	elif xs > 93:
		if k <= 1 and k > 0.3:
			k = k - 0.3
		elif k > 1:
			k  = 0.7
	return k
				

camlist = pygame.camera.list_cameras()
if camlist:
	while cantVeces < 500:
		s = capturarImg(pygame.camera.Camera(camlist[0],(160,120)))
		p = hallarPixelesIma(s,cantVeces)
		pendientes = inclinaciones(p)
		factores = pendientesAfactores(pendientes)
		k = 1
		if largoLista(p) == 0:
			robot.set2MotorSpeed(0,0,0,0)
		else:
			if largoLista(p) > 1:			
				k = calculoFactorVelocidad(factores)
			k = centradoFactor(k,p)
			movimientoRobot(robot, k)

Multimedia

Link a video de youtube:

Imágenes:

Obtenido de «http://www.fing.edu.uy/inco/proyectos/butia/mediawiki/index.php?title=Linea_camara&oldid=7414»