UASLP

FACULTAD DE INGENIERA

ALUMNOS:
MEDINA DELGADO ADAN
ROCHA MARTINEZ ANA ISABEL

MATERIA:
ROBOTICA

MANUAL DE HARDWARE Y SOFTWARE BRAZO DELTA

Manual de Hardware
Este manual contiene el diagrama elctrico del controlador principal del robot
delta y su conexin con los servomotores adems de sus especificaciones.

El controlador principal es una tarjeta de desarrollo que posee un micro

controlador Atmega328 (Tiene 32K de espacio para los programas, 23 lneas I/O,
de las cuales 6 son los canales para el ADC de 10 bits. Funciona hasta 20 MHz con
un cristal exterior, su voltaje operativo es de 1.8V a 5V), consta de 14 entradas
digitales configurables entrada i/o salidas que operan a 5 voltios. Cada pin puede
proporcionar o recibir como mximo 40 mA. Los pines 3, 5, 6, 9, 10 y 11 pueden
proporcionar una salida PWM (Pulse Width Modulation).
Si se conecta cualquier cosa a los pines 0 y 1, eso interferir con la comunicacin
USB. Diecimila tambin tiene 6 entradas analgicas que proporcionan una
resolucin de 10 bits. Por defecto miden de 0 voltios (masa) hasta 5 voltios,
aunque es posible cambiar el nivel ms alto, utilizando el pin Aref y algn cdigo
de bajo nivel.

La plataforma Arduino se programa mediante el uso de un lenguaje propio

basado en el popular lenguaje de programacin de alto nivel Processing. Sin
embargo, es posible utilizar otros lenguajes de programacin y aplicaciones
populares en Arduino. Como C, C++, Java, C#, Matlab, Ruby, Pyhton, entre otros.
Arduino est basado en C y soporta todas las funciones del estndar C y algunas
de C++.
Los registros de puertos permiten la manipulacin a ms bajo nivel y de forma ms
rpida de los pines de E/S del microcontrolador de las placas Arduino.11 Los pines

de las placas Arduino estn repartidos entre los registros B(0-7), C (analgicos) y
D(8-13).
Al poder usarse algunos de los pines como pulsos con modulacin se conectaron
los pines 9, 10 y 11 a los servomotores para controlarlos.
Diagrama Elctrico del Arduino UNO

Se emplearon 3 servomotores de la marca TowerPro modelo Mg995 que tienen un

torque de 15kg*cm a 6v y con 4.8v otorgan un torque de 10kg*cm con una
velocidad de 60grados / 0.20 segundos. Los engranes son de metal y tienen un
peso cada uno de 55g.
Estos servomotores se conectaron a una fuente externa de 5 voltios para no
exceder el amperaje soportado por el arduino ya que los servomotores consumen
ms.
El diagrama de conexin es el siguiente:
Lneas de control servomotores
Servo Pinza
Pin9 Pin10 Pin11
5V
GND

ARDUINO
UNO

Pin6

Puente a tierra del Arduino

En los servomotores los colores de las lneas son diferentes siendo la de tierra de
color caf, voltaje color rojo y control color naranja.

Estructura
La estructura est hecha en su totalidad de aluminio con tubo cuadrado de 1cm
de grosor, escuadra de aluminio de media pulgada, tubo de 1pulgada para la
base y lamina de aluminio, unido con remaches y tornillos.
A continuacin se muestra un dibujo del robot delta para mejor comprensin de
las partes estructurales y su descripcin.

El tringulo equiltero sobre el cual se montan los motores esta hecho de lmina
de aluminio cada lado mide 22cm y a la mitad tiene una hendidura de 2cm para
montar los ejes del brazo delta.

Los motores se unieron a esta base con silicn y remaches para poder sostener
esta base se le aadieron tres tubos de 1 pulgada en cada arista del tringulo
para que los ejes del brazo delta quedaran suspendidos en el aire.

El robot delta tiene 3 ejes que se unen en un denominado efector final, estos ejes
constan de 2 partes. La primera es una seccin lineal unida directamente con el
motor y con una bisagra a la segunda parte que tambin es lineal y consta de 4
rotulas 2 por extremo las cuales se unen a la bisagra del eje principal y la otra a la
bisagra del efector final.
Estas rotulas al no se consiguieron, y se opt por implementarlas con remaches
usndolos como pernos remachando solo en un punto para que pudiera rotar
sobre su eje. Al necesitar que el efector final quede paralelo al triangulo donde
estn los motores estas bisagras tienen 2 puntos de apoyo cada una por lo tanto
son 12 rotulas.

Para unir los motores con los ejes se emplearon los tornillos que traan estos y se
perforaron los tubos para montarlos.
El efector final se construy con el mismo tubo de aluminio de 1cm sobre una
base hecha de lmina de aluminio para que quedasen con la forma de vrtices
de un tringulo rectngulo, consta de 3 pedazos de tubo de 10 cm remachados
sobre lamina.

Sobre cada lado del efector final se unieron las bisagras del otro extremo de los 3
ejes que mueven los motores para as finalmente armar el brazo delta.
A continuacin se muestra una imagen del brazo delta sin la base.

Se presentaron problemas con las espigas como ejes ya que eran pesadas,
aunque los motores si podan con ellas se opt por cambiarlas debido a que

surgi un error en el diseo porque se apoyaban estas solo sobre un punto es

decir una sola rotula y se sustituyeron por 6 pedazos de escuadra de aluminio de
1cm con una longitud de 40 cm cada uno.
El brazo delta finalmente terminado ya con la base y la sustitucin de las espigas
se muestra en la siguiente imagen.

La pinza se hizo tambin de aluminio, con diseo de tijera, en la cual un extremo

esta fijo al motor y otro al eje que rota del motor para cerrar, se uni con
escuadra de aluminio y se remacho para sostenerla al servomotor.
La siguiente imagen es la pinza terminada con pedazos de goma en las puntas
para que no se resbalen los objetos, se uni al efector final con alambre y
plastilina cola-loca.

Cinemtica Inversa
El esquema cinemtico del robot delta consiste en 2 plataformas las cuales son
tringulos equilteros el fijo que contiene los motores y el mvil. Los ngulos de las
articulaciones son theta1, theta2 y theta3, y el punto E0 es la posicin efector final
con coordenadas (x0, y0, z0). Para resolver el problema de la cinemtica inversa
tenemos que crear la funcin con E0 coordenadas (x0, y0, z0) como parmetros
que devuelve (theta1, theta2, theta3).

En primer lugar designamos la longitud de un lado del tringulo fijo como f, la

longitud de la articulacin superior como rf, y la inferior como re, el marco de
referencia lo designamos como el centro del tringulo fijo.

Debido al diseo la articulacin superior F1J1 slo puede girar en el plano YZ,
formando un circulo con centro en la posicin del motor F1 y la longitud de la
articulacin rf de radio. En oposicin a la F1, J1 y E1 son los llamados juntas
universales, lo que significa que puede girar libremente E1J1 relativamente a E1,
formando esfera con centro en el punto E1 y de re radio.

Interseccin de la esfera y el plano YZ es un crculo con centro en el punto E'1 y

radio E'1J1, donde E'1 es la proyeccin de la E1 punto en el plano YZ. El J1 punto
se puede encontrar ahora como interseccin de los crculos de radio conocido,
con centros en E'1 y F1 (que debe elegir un solo punto de interseccin con menor
coordenada). Y si sabemos J1, podemos calcular el ngulo theta1. Primero
calculamos la posicin del punto E1y a partir del tringulo rectngulo formado
por el levantamiento de J1 obtenemos la posicin de X0, posteriormente
obtenemos el punto del motor, a partir de estas ecuaciones despejamos y
obtenemos el valor de theta1.

Para calcular los otros ngulos simplemente usamos la simetra del robot delta. En
primer lugar, vamos a girar el sistema de coordenadas en el plano XY alrededor
del eje Z a travs del ngulo de 120 grados hacia la izquierda. Para obtener theta
2 simplemente usamos la matriz de rotacin, para el theta3 giramos en sentido del
reloj

Cdigo de Arduino
#include <Servo.h>
#include<math.h>
Servo s1, s2, s3, s4;
int t1,t2,t3,t4;
int pot[10];
void setup()
{
//indicamos las salidas del Arduino que controla a los motores
s1.attach(9);
s2.attach(10);
s3.attach(11);
s4.attach(8);
//llenamos el arreglo de potencias para formar el numero
pot[0]=pow(10,0);
pot[1]=pow(10,1);
pot[2]=pow(10,2);
pot[3]=pow(10,3);
pot[4]=pow(10,4);
pot[5]=pow(10,5);
pot[6]=pow(10,6);
pot[7]=pow(10,7);

pot[8]=pow(10,8);
pot[9]=pow(10,9);
}
void loop()
{
if(Serial.available())
{
int r=Serial.read();//leemos 1er caracter del buffer serial
delay(100);//tiempo de espera
switch(r)
{
case 'a'://motor1
t1=leeNum();//almaceamos el valor del angulo del motor1
break;
case 'b'://motor2
t2=leeNum();//almaceamos el valor del angulo del motor2
break;
case 'c'://motor3
t3=leeNum();//almaceamos el valor del angulo del motor3
break;
case 'd'://pinza
t4=leeNum();//almaceamos el valor del angulo del motor de la pinza
break;
case '*'://al recibir el * nos indica que se han recibido todos los valores y ahora
se deben mover los motores
if(EsAngServo(t1))//verificamos que el valor que se va a escribir en los servos
entre dentro del rango valido
s1.write(t1);//movemos el motor el valor del angulo que indique la variable t
para cada servo correspondiente
if(EsAngServo(t1))
s2.write(t2);
if(EsAngServo(t1))
s3.write(t3);
if(EsAngServo(t1))
s4.write(t4);
break;
}
}
}
int leeNum()//lee num lee una cadena de numeros en orden invertido y la
convierte a entero

{
int num=0,i=0,r;
while((r=Serial.read())>0)//leemos un valor del puerto serial mientras el valor sea
mayor que cero
{
num+=(r-48)*pot[i];//num incrementa con el numeto leido multiplicado por el
arreglo pot el cual contiene las potencias de 10^i decrementamos 48 a r para
obtener su valor real como numero y no como ASCII
i++;
}
}
bool EsAngServo(int t)
{
return (t>=0&&t<=180);
}

Cdigo C#
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;
using System.IO.Ports;
using System.Windows.Forms;
namespace PosicionesPiezas
{
public class Serial
{
SerialPort serialPort1;
double e = 22;
// triangulo menor
double f = 22.5;
// triangulo mayor
double re = 23;
//l1
double rf = 40;
//l2
// trigonometric constants
static double sqrt3 = Math.Sqrt(3.0);
double pi = 3.141592653; // PI
double sin120 = sqrt3 / 2;
double cos120 = -0.5;
double tan60 = sqrt3;
double sin30 = Math.Sin(30);
double tan30 = 1.0 / sqrt3;

int dx1 = 20, dx2 = 288, dy1 = 20, dy2 = 171;//distancias del cuadro de vision y
movimiento 1 robot 2 camara
public Serial(SerialPort sp)
{
serialPort1 = sp;
}
public Angulo Envia(int x, int y, int z)
{
x = x * dx1 / dx2 - 12;//convierte las coordenadas de camara en
coordenadas de movimiento del robot
y = y * dy1 / dy2 - 12;
Angulo ang = delta_calcInverse(x, y, z);//calcula con cinematica inversa la
posicion
ang.procesa();//valida el resultado de la funcion anterior
serialPort1.WriteLine("a" + Invierte((int)ang.t1));//manda al motor 1 una
cantidad en grados
serialPort1.WriteLine("b" + Invierte((int)ang.t2));//manda al motor 2 una
cantidad en grados
serialPort1.WriteLine("c" + Invierte((int)ang.t3));//manda al motor 3 una
cantidad en grados
serialPort1.Write("*");//indica que se debe realizar el movimiento
return ang;
}
public void AgarraPinza(Angulo ang)
{
AbrePinza();//abre pinza
Thread.Sleep(500);
Baja(30, ang);//baja hasta el limite del suelo
Thread.Sleep(500);
CierraPinza();//cierra para tomar la pieza
Thread.Sleep(500);
Baja(-30, ang);//sube la pinza
Thread.Sleep(500);
Envia(posInicial());//mandamos a la posicion inicial
Thread.Sleep(500);
AbrePinza();//soltamos la pieza
Thread.Sleep(500);
}
void AbrePinza()
{
serialPort1.WriteLine("d" + Invierte(120));//escribe en el motor de la pinza 120
grados
serialPort1.Write("*");
}

void CierraPinza()
{
serialPort1.WriteLine("d" + Invierte(10));//escribe en el motor de la pinza 10
grados
serialPort1.Write("*");
}
void Baja(int cant, Angulo ang)//decrementa una cantidad al angulo actual
{
ang.t1 -= cant;
ang.t2 -= cant;
ang.t3 -= cant;
EnviaSProc(ang);
}
string Invierte(int i)//invierte numero para mandarlo al arduino
{
string cad = i.ToString();
List<char> listChar = new List<char>();
foreach (char c in cad)
{
listChar.Add(c);
}
listChar.Reverse();
cad = "";
foreach (char c in listChar)
{
cad += c;
}
return cad;
}
void Envia(Angulo ang)//Envia un angulo a los motores
{
ang.procesa();//procesa el angulo antes de ser enviado validando valores
negativos y que se encuentre dentro del rango
serialPort1.WriteLine("a" + Invierte((int)ang.t1));
serialPort1.WriteLine("b" + Invierte((int)ang.t2));
serialPort1.WriteLine("c" + Invierte((int)ang.t3));
serialPort1.Write("*");//indica que empieza el movimiento
}
public Angulo posInicial()//posicion inicial constante
{
Angulo ang = new Angulo();
ang.t1 = 124;
ang.t2 = 35;

ang.t3 = 66;
return ang;
}
void EnviaSProc(Angulo ang)//envia un angulo a los motores sin ser
procesado
{
serialPort1.WriteLine("a" + Invierte((int)ang.t1));
serialPort1.WriteLine("b" + Invierte((int)ang.t2));
serialPort1.WriteLine("c" + Invierte((int)ang.t3));
serialPort1.Write("*");
}
Status delta_calcAngleYZ(double x0, double y0, double z0)
{
Status s = new Status();
double y1 = -0.5 * 0.57735 * f; // f/2 * tg 30
y0 -= 0.5 * 0.57735 * e; // centra la esquina
// z = a + b*y
double a = (x0 * x0 + y0 * y0 + z0 * z0 + rf * rf - re * re - y1 * y1) / (2.0 * z0);
double b = (y1 - y0) / z0;
// discriminante
double d = -(a + b * y1) * (a + b * y1) + rf * (b * b * rf + rf);
if (d < 0)
{
s.Inicializa(true, 0);
return s; // si el discriminante es menor que cero significa que no existe y
retorna error
}
double yj = (y1 - a * b - Math.Sqrt(d)) / (b * b + 1);
double zj = a + b * yj;
double theta = Math.Atan(-zj / (y1 - yj)) * 180.0 / pi + ((yj > y1) ? 180.0 :
0.0);//calcula el valor del angulo
s.Inicializa(false, theta);
return s; //regresa el status que contiene el angulo
}
// (x0, y0, z0) -> (theta1, theta2, theta3)
Angulo delta_calcInverse(double x0, double y0, double z0)
{
Angulo ang = new Angulo();
double theta1 = 0;
double theta2 = 0;
double theta3 = 0;
Status status = delta_calcAngleYZ(x0, y0, z0);//se calcula el angulo y se
verifica el estado del discriminante

if (status.band == false)
{
theta1 = status.angulo;//si es valido se asigna theta1
status = delta_calcAngleYZ(x0 * cos120 + y0 * sin120, y0 * cos120 - x0 *
sin120, z0); // rota las coordenadas a +120 grados, apra calcular theta2
}
if (status.band == false)//checa si es valido el resultado de
delta_calcAngleYZ para theta2
{
theta2 = status.angulo;//si es valido se asigna theta2
status = delta_calcAngleYZ(x0 * cos120 - y0 * sin120, y0 * cos120 + x0 *
sin120, z0); // rota las coordenadas a -120 grados, apra calcular theta2
}
theta3 = status.angulo;//asigna theta3
ang.Inicializa(status.band, theta1, theta2, theta3);
return ang;
}
int xMin = -12, xMax = 12, yMin = -12, yMax = 12, inc = 5;//valores minimos y
maximos que alcanza el brazo dentro del area de vision de la camara
public void Area()//calcular que el area de vision de la camara corresponda
con el area de movimiento del robot
{
int x, y = yMin, z = 32;
for (x = xMin; x < xMax; x += inc)
Envia(x, y, z);
y = yMax;
for (x = xMin; x < xMax; x += inc)
Envia(x, y, z);
x = xMin;
for (y = yMin; y < yMax; y += inc)
Envia(x, y, z);
x = xMax;
for (y = yMin; y < yMax; y += inc)
Envia(x, y, z);
}
}
}
Clase Principal
using System;
using System.Collections.Generic;
using System.ComponentModel;

using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using Emgu.CV;
using Emgu.CV.Structure;
using Emgu.Util;
using System.Diagnostics;
using System.Threading;
namespace PosicionesPiezas
{
public partial class Form1 : Form
{
private Capture _capture;
private Image<Bgr, Byte> frame;
private StringBuilder msgBuilder;
private List<Objetivo> objetivos;
Angulo ang;
CinematicaInversa cn = new CinematicaInversa();
Serial s;
public Form1()
{
InitializeComponent();
s = new Serial(serialPort1);//inicializa la clase serial con el serial port del form
ang = new Angulo();//inicializa la variable angulo
if (serialPort1.IsOpen)
serialPort1.Close();//si el puerto esta abierto lo cierra
serialPort1.Open();//abre el puerto
s.posInicial();//mueve el brazo ala poscicion inicial para que la camara no
lo detecte
if (_capture == null)//si la captura es null
{
try
{
_capture = new Capture();//genera una nueva captura
}
catch (NullReferenceException excpt)
{
MessageBox.Show(excpt.Message);//si no captura bien el dispositivo
manda un mensaje de error
}
}
Application.Idle += ProcessFrame;
}
private void ReleaseData()
{

if (_capture != null)
_capture.Dispose();//al momento de cerrar la aplicacion elimina el
contenido de la variable _capture
}
private void ProcessFrame(object sender, EventArgs arg)//captura de la
camara metodo que se esta actualizando y mostrando el contenido de la
camara
{
frame = _capture.QueryFrame();//obtiene la vision de la camara
imageBox1.Image = frame;//la muestra en un imageBox
}
private void detectaCirculitos(Image<Bgr, Byte> imagenBase)
{
Image<Gray, Byte> FiltroCirculos = imagenBase.Convert<Gray,
Byte>().PyrDown().PyrUp();//Se filtran las imagenes de circulo en nuestra captura
imageBox2.Image = FiltroCirculos;
Gray cannyThreshold = new Gray(180);
Stopwatch watch = Stopwatch.StartNew();
msgBuilder = new StringBuilder("Performance: ");
#region deteccion de circulos
Gray circleAccumulatorThreshold = new Gray(500);
objetivos = new List<Objetivo>();
CircleF[] circles = FiltroCirculos.HoughCircles(cannyThreshold,
circleAccumulatorThreshold, 9.0, 15.0, 1, 0)[0]; //Regresa los circulos
watch.Stop();
msgBuilder.Append(String.Format("Hough circles - {0} ms; ",
watch.ElapsedMilliseconds));
foreach(CircleF circulo in circles) //LLenado de las posiciones en la imagen
de los circulos
{
Objetivo ob = new Objetivo(circulo.Center.X, circulo.Center.Y,
circulo.Radius);
objetivos.Add(ob);
}
#endregion
watch.Stop();
#region Dibujar circulos
Image<Bgr, Byte> circleImage = imagenBase.CopyBlank();
foreach (CircleF circle in circles)
circleImage.Draw(circle, new Bgr(Color.Brown), 2);
#endregion
imageBox4.Image = circleImage;//muestra con circulos las posiciones de
los circulos
}
private void Form1_FormClosing(object sender, FormClosingEventArgs e)

{
if (serialPort1.IsOpen)//al momento de cerrar cierra el puerto serial
serialPort1.Close();
}
private void TomaFoto(object sender, EventArgs e)
{
s.posInicial();//mueve el brazo a una poscion para que no el estorbe a la
camara
Thread.Sleep(4000);
imageBox3.Image = frame;//captura la imagen
detectaCirculitos(frame);//detecta las pocisiones delos circulos
foreach (Objetivo o in objetivos)//objetivos es una lista alli se quedan
guardadas las posiciones de los circulitos en pantalla
{
s.AgarraPinza(s.Envia((int)o.Centro.Y, (int)o.Centro.X, 32));
}
}
private void button2_Click(object sender, EventArgs e)
{
s.Area();//calcula el area de movimiento del brazo para acomodar la
camara
}
}
}
Cotizacin
Concepto
Arduino UNO
Servomotor tower pro mg995
Tubo aluminio 1cm
Bolsa de remaches
Tubo aluminio 1 pulgada
Lamina de aluminio 3mm
Escuadra de aluminio de 1cm
Envo de motores y arduino

Cantidad
1
4
2
1
3
1
3

Precio
unitario
$400
$210
$50
$120
$70
$175
$50
$120

Subtotal
$400
$840
$100
$120
$210
$175
$150
$120

Subtotal
IVA 16%
TOTAL

$2.105
$336.8
$2441.8