Universidad del Valle de Guatemala Ing.

Jos Barrera, Introduccin a la Robtica Departamento de Electrnica

LMPARA INTELIGENTE
Proyecto 2

Jos Bagur, 10275 Luis Sigenza, 10297 Emilio Miranda, 10411 Juan Medrano, 10330

UNIVERSIDAD DEL VALLE DE GUATEMALA

Introduccin a la Robtica

LMPARA INTELIGENTE
Proyecto presentado por Jos Antonio Bagur Njera, Emilio Jos Miranda Rivas, Luis Antonio Sigenza Leiva y Juan Fernando Medrano Yax.

Ciudad de Guatemala, 2013

LMPARA INTELIGENTE
Proyecto 2 Introduccin Se desarroll una dispositivo que se asemeja a una lmpara de escritorio compuesta por una base y tres eslabones, dos de los cuales le permiten movimiento vertical traslacional al tercer el eslabn, el cual mantiene al efector final: una cmara. En el caso del dispositivo desarrollado, no se implement una lmpara como efector final, sino una cmara de video sencilla con conexin usb (o webcam) para proveerle al dispositivo la capacidad de reconocer un rostro y coordinar, a travs de un circuito de control, los servomotores colocados en las juntas entre los eslabones. Se desarroll un algoritmo de procesamiento de imgenes capaz de traducir el movimiento del rostro capturado por la cmara, en una serie de ngulos de giro de los motores, de tal forma que el dispositivo se alinee con la nueva ubicacin vertical del rostro. Los algoritmos utilizados para el uso de la cmara, el procesamiento de imgenes y el clculo del movimiento de los motores (cinemtica inversa) fueron implementados en el lenguaje de programacin python y el software ejecutado en el ordenador de placa reducida raspberry pi. El control o drive de los servomotores se implement en el lenguaje de programacin processing y ejecut en la herramienta de prototipos Arduino Uno. La rapsberry pi y Arduino Uno se comunican a travs de un bus I2C en el cual solamente la tarjeta raspberry pi enva datos de ngulos de movimiento al Arduino Uno, el cual se encarga solamente de la generacin de las seales adecuadas (PWM) para que los servomotores mantengan su posicin. La estructura del dispositivo, es decir, todas las piezas mecnicas como eslabones, y la base, fueron creadas utilizando impresin 3D. Se utiliz un modelo virtual de brazo robtico ya existente a partir del cual se gener el modelo para la impresin 3D de cada uno de los eslabones. De esta forma se obtuvo una estructura mecnica suficientemente liviana y robusta para soportar el peso de los motores y de la cmara.

Fig. 1. Fotografas del brazo robtico construido.

Algoritmo de Reconocimiento de Rostro. Como se mencion en los prrafos anteriores, el algoritmo de procesamiento de imgenes se implement en el lenguaje de programacin python sobre la plataforma raspberry pi. El procesamiento de imgenes realizado consisti especficamente en el reconocimiento de un rostro en una imagen. Dicho procesamiento y reconocimiento fue relativamente sencillo de realizar con xito ya que se utiliz libreras libres disponibles para dicho propsito. Sobre openCV El conjunto de libreras utilizadas son las proporcionadas por openCV, que es un conjunto de libreras escritas en C/C++ diseadas especialmente para eficiencia computacional y un fuerte enfoque en procesamiento en tiempo real. Contiene libreras para proveerle a los dispositivos visin por computadora: la capacidad de obtener, analizar, reconocer e interpretar imgenes, y aprendizaje de mquina: la capacidad de entender patrones y desarrollar cdigo que evoluciona constantemente. Se seleccion openCV como el paquete para el procesamiento de imgenes y reconocimiento de rostro ya que: Es un conjunto abierto de libreras, es decir, le proveen al desarrollador interfaces y mtodos para modificar las libreras y utilizarlas de la mejor forma segn la aplicacin. Es soportado ampliamente por una comunidad de ms de 47000 desarrolladores y hay documentacin disponible suficiente. Tiempo de desarrollo corto ya que provee automticamente algoritmos optimizados para el reconocimiento de rostros. Ya est incluido en la distribucin Raspbian de Debian que est siendo utilizada como sistema operativo para la plataforma rapsberry pi.

De las libreras de openCV utilizadas las dos ms importantes y que efectivamente permiten el reconocimiento de rostros son: cv.HaarDetectObjects() y archivos entrenados para reconocimiento de objetos especficos como ojos, rostros o lneas. Se utiliz obviamente un archivo entrenado para reconocimiento de rostros llamada haarcascade_frontalface_alt.xml. Se puede encontrar informacin ms especfica en [1]. La tcnica implementada por la funcin HaarDetectObjects es reconocimiento de objetos basado en la clasificacin en cascada de caractersticas tipo haar (la palabra haar proviene de su similitud con las Haar wavelets en honor a Alfred Haar). La deteccin de objetos por este mtodo fue inicialmente introducida por Paul Viola y mejorada por Rainer Leinhart. El mtodo es el siguiente: primero un clasificador o classifier (especficamente un conjunto de clasificadores mejorados implementados en cascada sobre caractersticas tipo haar) es entrenado con algunos cientos de vistas ejemplo de un objeto en particular (p.ej.: un carro, rostro), llamadas pruebas positivas. Estas son escaladas a la

misma dimensin (20 x 20 pixeles, p.ej.). Adems se le entrena tambin con pruebas negativas del mismo tamao. Luego de que se entrena al clasificador, se puede aplicar a una regin de inters (del mismo tamao que se utiliz en la prueba) en una imagen de entrada. El clasificador devuelve un 1 si la regin de inters probablemente contiene el objeto, o un 0 de lo contrario. Para buscar un objeto en la imagen completa, se puede mover la ventana de bsqueda a lo largo de la imagen y verificar cada ubicacin utilizando el clasificador. El clasificador es diseado de tal forma que se pueda modificar su tamao para reconocer objetos de distintos tamaos, lo cual es ms eficiente que escalar la imagen como tal. De esta forma, para encontrar un objeto cuyo tamao se desconoce en una imagen, se debe hacer el escaneo varias veces a distintos tamaos de clasificador. El algoritmo implementado para realizar todo el procesamiento de imgenes desde la captura de la imagen de la cmara web hasta la obtencin del movimiento que los motores deben tener para poder seguir al rostro se presenta a continuacin.

Fig. 2. Algoritmo de alto nivel implementado para el reconocimiento de rostros y generacin

de ngulos de movimiento para motores.

Fig. 3. Dimensiones de la captura: 170 x 144 pixeles (ancho x alto). Dichas Dimensiones demostraron experimentalmente ser las ptimas.

Fig. 4. Resultados de las pruebas de deteccin de rostro. El marco rojo y punto verde indican el rea y centro del rostro detectado. El punto azul es el centro de la imagen.

Fig. 5. Captura de la pantalla de salida del software implementado. Ntese que segn la posicin del rostro que se detect, se calcula la distancia vertical a moverse as como las coordenadas x,y,z objetivo y las que se lograron alcanzar (ya que el robot tiene lmites de movimiento impuestos por la propia estructura).

Los cdigos implementados tanto en la plataforma raspberry como arduino se presentan al final de este documento.

Cinemtica Inversa

Fig. 1. Configuracin geomtrica de brazo robtico. El eslabn L3 se encuentra alineado con el eje x en todo momento, debido a esto es posible calcular las coordenadas del punto P1. De esta forma, las coordenadas del punto P1 son: ( )

Conociendo este punto se puede determinar que la magnitud de la lnea D es de: ( De la fig. 1 se observa que )

, donde el ngulo est dado por: ( )

El eslabn 1, 2 y D forman un tringulo. Debido a que se conocen todas las longitudes, es posible determinar cualquiera de los ngulos internos por medio de ley de cosenos. Ley de cosenos:

Fig. 2. Notacin para ley de cosenos. De esta forma se calcula el ngulo :

( De esta forma, el ngulo es:

( ( ( ) )

( De la fig. 1 se observa que

( est dado por:

, donde el ngulo

Se sabe que para un tringulo unitario sus catetos estn dados por:

Fig. 3. Notacin tringulo unitario Adems, se tiene la siguiente relacin trigonomtrica:

De esta forma se tiene que el ngulo

est dado por: ( ( ) )

De esta forma, el ngulo

es: ( )

) )

) )

De la fig. 1 se tiene que el ngulo

est dado por:

Tambin de la fig. 1 se tiene que el ngulo

est dado por: ( ( ) )

( (

) ) (

) ( )

) ( )

) )

Configuracin fsica

Fig. 4. Configuracin geomtrica real de brazo robtico. Parmetros reales del brazo robtico:

Para colocar los eslabones del brazo real en la posicin mostrada en la fig. 4 es necesario indicarles a los servomotores que controlan la posicin angular de cada eslabn como sigue a continuacin:

Debido a esto, es necesario realizar un ajuste en los ngulos calculados de forma terica y de esta forma el clculo de los ngulos en el brazo real es:

DESCRIPCIN DEL BRAZO ROBTICO

El diseo y los archivos del brazo robtico utilizado en el proyecto se obtuvieron de la pgina web Thingiverse (www.thingiverse.com). Thingiverse es un sitio web creado y administrado por la empresa de impresoras 3D MakerBot Industries en dnde usuarios de todo el mundo pueden compartir sus diseos digitales. El sitio provee principalmente de hardware open source que puede ser replicado por impresoras 3D, cortadoras lser, tornos, y muchas otras tecnologas.

Figura 1. Pgina principal de Thingiverse.

Luego de buscar exhaustivamente en el sitio mencionado y de analizar varios modelos encontrados y compartidos por usuarios de Espaa e Israel, se decidi utilizar el modelo de un brazo robtico creado y compartido por Jan-Jaap, originario de Holanda (http://www.thingiverse.com/thing:2433).

Figura 2. Pgina principal del brazo robtico creado y compartido por Jan-Jaap.

Ya que el proyecto posee una licencia GNU Creative Commons no comercial, se contact con el autor del diseo va correo electrnico para obtener su autorizacin

personal para replicar el proyecto, ya que algunas de las piezas fueron impresas por una empresa ajena a la Universidad del Valle de Guatemala. Se obtuvo el permiso y consentimiento por parte de Jan-Jaap para recrear su brazo robtico siempre y cuando no estuvieran involucrados en el proyecto, propsitos comerciales o econmicos.

Figura 3. Licencia GNU Creative Commons no comercial del brazo robtico de Jan-Jaap. Ya que algunas de las partes del brazo robtico, se imprimieron con una empresa ajena a la Universidad, se decidi pedir personalmente el permiso y consentimiento de Jan-Jaap.

Figura 4. Respuesta de Jan-Jaap autorizando imprimir el brazo robtico siempre y cuando no estuviera involucrado ningn propsito comercial o econmico en el proyecto.

Con la autorizacin y el consentimiento de Jan-Jaap, se imprimi el brazo robtico utilizando una impresora 3D. Esto se realiz tanto con la impresora 3D del departamento de Ingeniera Mecatrnica de la Universidad del Valle de Guatemala, como con la empresa guatemalteca MBau3D.

Figura 4. Recreacin digital del brazo robtico utilizado en el proyecto. Ntese que en este caso en particular, no se utiliz la garra mostrada.

El brazo robtico recreado, consiste de 13 partes impresas en plstico PLA negro. Se mueve a travs de 6 servo motores de la marca HITEC modelo HS-322HD Standard Deluxe, los cuales, individualmente pueden proveer un torque de hasta 51.00 oz/pulg. El brazo en su totalidad fue ensamblado utilizando tornillos metlicos y pegamento adhesivo instantneo de precisin.

Figura 5. Vista de uno de los eslabones del brazo robtico desde el programa Repetier. Dicho programa, es utilizado para controlar impresoras 3D as generar el cdigo G que dichos dispositivos pueden interpretar para imprimir distintos modelos y piezas.

Figura 5. Vista de la base de los servo motores del brazo robtico desde el programa Repetier. Dicho programa, es utilizado para controlar impresoras 3D as generar el cdigo G que dichos dispositivos pueden interpretar para imprimir distintos modelos y piezas.

Conclusiones - Se dise y construy exitosamente una lmpara con la capacidad de seguir el movimiento vertical de un rostro humano utilizando una cmara con conexin usb, sobre la plataforma raspberry pi, utilizando las libreras openCV en lenguaje python. - Las libreras openCV contienen las herramientas suficientes para acelerar el desarrollo de aplicaciones de procesamiento y reconocimiento de imgenes. - Se logr implementar exitosamente algoritmos de cinemtica inversa para un mecanismo de 3 eslabones. - La impresin 3D demostr ser un mtodo til para la construccin del mecanismo implementado con la nica desventaja que el tiempo de impresin es largo.

Bibliografa: [1] openCV Doc, 2013. Cascade Classification: Haar Feature-based Cascade Classifier for Object Detection. [Consultado: 24/11/2013]. En:
http://docs.opencv.org/modules/objdetect/doc/cascade_classification.html#cv.HaarDetectObj ects

C:\Users\JuanMed\Documents\Universidad\8vo_Semestre\Robotica\Proyecto_Robot\Codigo\Prueba5_MovimientoPasoaPaso.py domingo, 24 de noviembre de 2013 13:23

# -*- coding: utf-8 -*""" Created on Sun Nov 17 19:29:55 2013 @authors: JoseBag, EmilioMir, LuisSig, JuanMed """ import cv import numpy as np import math import smbus import time from imgproc import * from PIL import Image

# Constantes de operacion

# Posiciones absolutas en pixeles x_pix = 4 y_pix = 0 z_pix = 0 # Posiciones absolutas en cms x_cms = 0 y_cms = 13 z_cms = 0

# Desplazamiento en cada eje en pixeles x_offset_pix = 0 y_offset_pix = 0 z_offset_pix = 0 # Desplazamiento en cada eje en cms x_offset_cms = 0 y_offset_cms = 0 z_offset_cms = 0 # Habilitar o deshabilitar despligue en camera cameraDisplay = True FrameCount = 0 # Configuracion de camara (pixeles) ancho = 240 alto = 180 # Longitud de eslabones L1= 12.0 L2= 12.3 L3 = 4.0 theta1 = 0 theta2 = 0 theta3 = 0
-1-

C:\Users\JuanMed\Documents\Universidad\8vo_Semestre\Robotica\Proyecto_Robot\Codigo\Prueba5_MovimientoPasoaPaso.py domingo, 24 de noviembre de 2013 13:23

bus = smbus.SMBus(1)

# Pre: x,y,z,L1,L2,L3 contienen las coordenadas objetivo y las longitudes de eslabones. # Post: Se calcula el ngulo a rotar para la junta 1 # Params: x,y,z Coordenadas del punto objetivo para el efector final # L1,L2,L3 Longitudes eslabon 1, 2 y 3, respectivamente. # Return: Angulo theta 1 a rotar para junta 1 def getTheta1(x,y,z,L1,L2,L3): D = math.sqrt(((x-L3)**2)+(y**2)) F = ((L1**2)+(D**2)-(L2**2))/(2*L1*D) alfa = math.degrees(math.acos(F)) teta1 = math.degrees(math.atan2(y, x-L3)) + alfa return teta1

# Pre: x,y,z,L1,L2,L3 contienen las coordenadas objetivo y las longitudes de eslabones. # Post: Se calcula el ngulo a rotar para la junta 2 # Params: x,y,z Coordenadas del punto objetivo para el efector final # L1,L2,L3 Longitudes eslabon 1, 2 y 3, respectivamente. # Return: Angulo theta 1 a rotar para junta 2 def getTheta2(x,y,z,L1,L2,L3): D = math.sqrt(((x-L3)**2)+(y**2)) E = ((L1**2+(L2**2)-(D**2))/(2*L1*L2)) beta = math.degrees(math.atan2(math.sqrt(1-(E**2)),E )) teta2 = 180-beta return teta2

# Pre: theta1,theta2 contienen angulos a rotar para alcanzar posicion objetivo x,y,z # Post: Se calcula el angulo a rotar para junta 3 # Params: theta1, theta2 angulos a rotar para juntas 1 y 2, respectivametne # Return: Angulo theta 3 a rotar para junta 3 def getTheta3(theta1, theta2): teta3 = theta1-theta2 return teta3 # Pre: value contiene un angulo tipo entero entre 0 y 180 # Post: Se envia al controlador de servomotores el angulo a rotar # Params: value, contiene el angulo a rotar # Return -1 si el envio fue correcto def enviarAnguloI2C(value): bus.write_byte(0x04,value) return -1 # Pre: solicita a controlador las posiciones de los angulos a rotar # Post: Se obtiene del controlador el angulo a rotar para cada motor # Params: None # Return Angulos a rotar def leerAnguloI2C(): number = bus.read_byte(0x04) return number # Pre: pixeles contiene una cantidad en pixeles a convertir a centimetros # Post: Se devuelve el valor en centimetros correspondiente a los pixeles indicados
-2-

C:\Users\JuanMed\Documents\Universidad\8vo_Semestre\Robotica\Proyecto_Robot\Codigo\Prueba5_MovimientoPasoaPaso.py domingo, 24 de noviembre de 2013 13:23

# Params: pixeles, cantidad de pixeles a convertir a cms # Return centimetros def pixelesAcms(pixeles): return (pixeles * 113/(ancho*2.5)) # Pre: z_cms contiene el offset de la coordenada z del rostro # Post: Se calcula el angulo a rotar la base para el offset z_cms # Params: z_cms, desplazamiento en eje z en centimetros # Return angulo a rotar def getTheta4(z_cms): return math.degrees(math.atan2(z_cms,50))

# Pre: se ha capturado una imagen y ha cargado exitosamente el clasificador haar para rostros # Post: Las coordenadas en pixeles del offset del centro de la imagen con el centro del rostro detectado # Params: imagen a analizar "image" bajo el clasificado "faceCascade" # Return: cantidad en pixeles a moverse def DetectarRostro(image,faceCascade): # Definir parametros para el reconocimiento min_size = (20,20) image_scale = 2 haar_scale = 1.2 min_neighbors = 2 haar_flags = 0 caraX = 0 caraY = 0 width = 0 heigth = 0 #Crear una imagen "vacia" de las dimensiones de la imagen a analizar gray = cv.CreateImage((image.width,image.height),8,1) #Disminuir el tamao de imagen, para que acelerar el analisis smallImage = cv.CreateImage((cv.Round(image.width/image_scale),cv.Round(image.height/ image_scale)),8,1) #Convertir la imagen de entrada a colores en escala de grises cv.CvtColor(image,gray,cv.CV_BGR2GRAY) #Escalar la imagen en escala de grises a una tamano menor cv.Resize(gray,smallImage,cv.CV_INTER_LINEAR) #Uniformizar informacion de las imagenes cv.EqualizeHist(smallImage,smallImage) #Detectar todos los rostors faces = cv.HaarDetectObjects(smallImage,faceCascade,cv.CreateMemStorage(0),haar_scale, min_neighbors,haar_flags,min_size) # Para cada uno de los rostros detectados, dibujar un cuadro a su alrededor # y un punto en el centro # Dibujar un punto en el centro de la imagen pt0 = (ancho/2, alto/2) pt00 = (2 + ancho/2, 2+ alto/2) cv.Rectangle(image, pt0, pt00, cv.RGB(0,0,255), 3, 8, 0) # Analizar cada uno de los rostros detectados if faces:
-3-

C:\Users\JuanMed\Documents\Universidad\8vo_Semestre\Robotica\Proyecto_Robot\Codigo\Prueba5_MovimientoPasoaPaso.py domingo, 24 de noviembre de 2013 13:23

for ((x,y,w,h),n) in faces: if( cameraDisplay ): # Dibujar cuador y punto verde alrededor del rostro. pt1 = (int(x * image_scale), int(y * image_scale)) pt2 = (int((x + w) * image_scale), int((y + h) * image_scale)) pt3 = (int(((x+w/2)) * image_scale), int(((y+h/2)) * image_scale)) pt4 = (int(((x+w/2)) * image_scale) +2, int(((y+h/2)) * image_scale) +2) cv.Rectangle(image, pt1, pt2, cv.RGB(255, 0, 0), 3, 8, 0) cv.Rectangle(image, pt3, pt4, cv.RGB(0,255,0), 3, 8, 0) # Calcular offset del centro del rostro a partir de posicion detectada tomando como referencia # el centro de la imagen. caraX = int((x + (w/2))*image_scale) - ancho/2 # caraX: 0 cuando rostro esta en el centro de la imagen caraY = int((y + (h/2))*image_scale) - alto/2 # caraY: 0 cuando rostro esta en el centro de la imagen width = w height = h return (caraX,caraY)

# Inicializar una conexion con la camara capture = cv.CaptureFromCAM(-1) # Configurar las dimensioens de la imagen a tomar cv.SetCaptureProperty(capture, 3, ancho) cv.SetCaptureProperty(capture, 4, alto) # Si esta habilitado, abrir una ventana para analisis if( cameraDisplay ): cv.NamedWindow("Face Recognition Test") # Cargar clasificador entrenado para deteccion de rostros faceCascade = cv.Load("haarcascade_frontalface_alt.xml") print "****** Reconocimiento de Rostros *******"

while (cv.WaitKey(27)==-1): # Capturar una imagen de camara cv.GrabFrame(capture) cameraFrame = cv.RetrieveFrame(capture) print "++++++++++++++++++++ Foto : ",FrameCount," +++++++++++++++++++++++++++++" FrameCount += 1 # Obtener ancho y alto de foto tomada ancho = cameraFrame.width alto = cameraFrame.height # Obtener posicion de rostro z_offset_pix,y_offset_pix=DetectarRostro(cameraFrame,faceCascade) if( cameraDisplay ): #cv.PutText(cameraFrame, str(FrameCount),(300,200), cv.InitFont(font,
-4-

C:\Users\JuanMed\Documents\Universidad\8vo_Semestre\Robotica\Proyecto_Robot\Codigo\Prueba5_MovimientoPasoaPaso.py domingo, 24 de noviembre de 2013 13:23

1,0.5,0.5,1,8),255) cv.ShowImage("Face Recognition Test", cameraFrame)

# Invertir eje y_offset_pix = y_offset_pix*-1 print "cantidad a moverse: ", pixelesAcms(y_offset_pix) #print "Coordeanda OFFSET y: ",y_offset_pix

# Calcular posiciones absolutas actualizadas x_cms = x_pix # Permitir movimiento del braso si y solo si se el desplazamiento de camara # es mayor a 2 cms if( abs(pixelesAcms(y_offset_pix))>2): if(y_offset_pix > 0): y_cms = y_cms + 0.35 if(y_offset_pix < 0): y_cms = y_cms - 0.35 z_cms = z_cms + pixelesAcms(z_offset_pix)

print print print print

"Coordenadas objetivo: " " x: ", x_cms " y: ", y_cms " z: ", z_cms (13 <= y <= 24)

# Restringir rango de movimiento en Y if ( y_cms < 13): y_cms = 13 if ( y_cms >24): y_cms = 24 print print print print "Coordenadas finales: " " x: ", x_cms " y: ", y_cms " z: ", z_cms

# Las ecuaciones sirven solo para y > 0 theta1 theta2 theta3 theta4 theta4 = = = = = getTheta1(x_cms,y_cms,z_cms,L1,L2,L3) getTheta2(x_cms,y_cms,z_cms,L1,L2,L3) getTheta3(theta1, theta2) -1*getTheta4(z_cms) 0

# Restringir angulos de movimiento de la base if(theta4 > 80):

-5-

(-80 < theta4 < 80)

C:\Users\JuanMed\Documents\Universidad\8vo_Semestre\Robotica\Proyecto_Robot\Codigo\Prueba5_MovimientoPasoaPaso.py domingo, 24 de noviembre de 2013 13:23

theta4 = 80 if(theta4 < -80): theta4 = -80

# Enviar angulos calculados a Arduinos enviarAnguloI2C(int(theta1)) time.sleep(0.1) enviarAnguloI2C(int(theta2)) time.sleep(0.1) enviarAnguloI2C(int(theta3)) time.sleep(0.1) if(theta4 >= 0): enviarAnguloI2C(1) else: enviarAnguloI2C(2) time.sleep(0.1) enviarAnguloI2C(abs(int(theta4))) time.sleep(0.1) # Imprimir valores print "Theta 1: ", print "Theta 2: ", print "Theta 3: ", print "Theta 4: ", #time.sleep(10) # Si esta habilitado, liberar espacio en memoria ocupado por despliegue en pantalla if ( cameraDisplay ): cv.DestroyWindow("Face Recognition Test")

theta1 theta2 theta3 theta4

-6-

C:\Users\JuanMed\Documents\Universidad\8vo_Semestre\Robotica\Proyecto_Robot\Codigo\arduino\control\control.ino

domingo, 24 de noviembre de 2013 13:31

#include <Wire.h> #include <Servo.h> int dato_recibido = 0; char temporal[10]; int contador_datos=0; Servo servo_base_1; Servo servo_brazo_2; Servo servo_camara_3; Servo servo_rotacion_4; int sumar_restar = 1; restar // Dato recibido por Raspberry Pi por I2C

// // // // //

Crear objeto para control Crear objeto para control Crear objeto para control Crear objeto para control Permite identificar si el

de servo 1 de servo 2 de servo 3 de servo 4 angulo enviado se debe sumar o

void setup() { Wire.begin(0x04); // Inicializacin i2c como esclavo Wire.onReceive(receiveData); // Esperar envio de datos del Maestro Wire.onRequest(sendData); // Envio de datos cuando lo indique el Maestro Serial.begin(9600); // Baudrate 9600 servo_base_1.attach(8); // Pin de control servo 1 servo_brazo_2.attach(9); // Pin de control servo 2 servo_camara_3.attach(10); // Pin de control servo 3 servo_rotacion_4.attach(11); // Pin de control servo 4 //Posicion inicial de los motores servo_base_1.write(5); servo_brazo_2.write(43); servo_camara_3.write(118); servo_rotacion_4.write(90); } void loop() { delay(100); } // receiveData: funcion que actua en cuando recibe informacion del maestro void receiveData(int byteCount){ contador_datos+=1; // Contador de cuantos datos han sido recibidos while(Wire.available()) { dato_recibido = Wire.read(); // Leer dato recibido } if(contador_datos==1){ dato_recibido= 153 - dato_recibido; // Ajuste de angulo 1 servo_base_1.write(dato_recibido); // Envio de posicion a servo 1 Serial.print("Base: "); Serial.println(dato_recibido); } if(contador_datos==2){ dato_recibido= 158 - dato_recibido; // Ajuste de angulo 2 servo_brazo_2.write(dato_recibido); // Envio de posicion a servo 2 Serial.print("Brazo: "); Serial.println(dato_recibido); } if(contador_datos==3){ dato_recibido= 85 + dato_recibido; // Ajuste de angulo 3 servo_camara_3.write(dato_recibido); // Envio de posicion a servo 3 Serial.print("Camara: "); Serial.println(dato_recibido); }
-1-

C:\Users\JuanMed\Documents\Universidad\8vo_Semestre\Robotica\Proyecto_Robot\Codigo\arduino\control\control.ino

domingo, 24 de noviembre de 2013 13:31

if(contador_datos==4){

//Serial.print(dato_recibido); //dato_recibido= 90 + dato_recibido; // Ajuste de angulo 4 //servo_rotacion_4.write(dato_recibido); // Envio de posicion a servo 4 //Serial.print("Rotacion: "); Serial.println(dato_recibido); //contador_datos = 0; // Verificar si el siguiente angulo a recibir se debe sumar o restar if(dato_recibido == 1){ sumar_restar = 1; Serial.println("Sumar angulo"); }else{ sumar_restar = 2; Serial.println("Restar angulo"); } } // Ajustar rotacion del servo base if(contador_datos == 5){ if(sumar_restar == 1){ dato_recibido = 90 + dato_recibido; }else{ dato_recibido = 90 - dato_recibido; } servo_rotacion_4.write(dato_recibido); Serial.print("Rotacion: "); Serial.println(dato_recibido); contador_datos = 0;

} } // Envio de informacion al maestro void sendData(){ //Respuesta de recepcion de datos al Maestro (Raspberry) Wire.write(20); }

-2-