Linea camara

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: [1]

Imágenes: