Copia Movimientos emulado en un brazo robot

Concepto:

Un Brazo Robot está constituido por una serie de elementos o eslabones unidos mediante
articulaciones que permiten un movimiento relativo entre cada eslabón consecutivo. La
constitución física de gran parte de los brazos robot guarda cierta similitud con la anatomía del
brazo humano, es decir, que poseen ciertas características antropomórficas, por lo que en
ocasiones a los distintos elementos que componen el robot se les denomina en términos como
Base, Hombro, Codo, Brazo, Muñeca y Pinza.

Cada articulación provee al Dispositivo de al menos un grado de libertad, así que yo lo llamo a
cada uno de los movimientos independientes que puede realizar cada articulación con respecto
a la anterior, se denomina grado de libertad (GDL).

Brazo

Muñeca

Pinza Codo

Hombro

Base

Diseño a tomas en cuenta:

Los Brazos robot de 6 grados de libertad trabajan con eficiencia, gracias a la combinación de
elementos mecánicos y servo motores, los cuales ofrecen una precisión y libertad de trabajo
en comparación a otros sistemas de trasmisión de potencia. Su estructura especial les permite
alcanzar estas distancias con rapidez: su escaso peso propio hace posible trabajar a elevada
velocidad y los convierte en idóneos para aplicaciones de recogida y colocación.

Cinemática de un robot:

“La cinemática de un robot estudia el movimiento del mismo con respecto a un sistema de
referencia sin tener en cuenta la causas que la producen"

Existen dos problemas fundamentales a resolver en la cinemática de un robot cualquiera; el

primero de ellos se conoce como el problema cinemático directo, y consiste en determinar cuál
es la posición (X,Y) y orientación del extremo final del robot, con respecto a un sistema de
coordenadas que se toma como referencia, el segundo, denominado problema cinemático
inverso, resuelve la configuración (Movimiento) que debe adoptar el robot para una posición y
orientación del extremo conocidas.

Las variables que se utilizan en el modelo cinemático del robot SCARA de dos grados de
libertad (GDL), son las longitudes de cada uno de los eslabones, l1 y l2, las posiciones
articulares de cada eslabón, q1 y q2, que están tomados en referencia al sistema de
coordenadas con centro en la base del robot y la posición del órgano terminal del robot
indicada por las coordenadas del mismo en el sistema de referencia.

Como referencia tomaremos a un Brazo robot con 1 Grado de libertad.

Cinemática de un robot con 1 Grado de Libertad:

La medición de ángulos:
Los ángulos miden posiciones relativas de dos rectas en el plano. La medición de ángulos y,
por tanto, en trigonometría, se emplean tres unidades:

 Radián: unidad angular natural en trigonometría. En una circunferencia completa hay

2π radianes (algo más de 6,28).
 Grado sexagesimal, (minutos y segundos): unidad angular que divide una
circunferencia en 360 grados.
 Grado centesimal: unidad angular que divide la circunferencia en 400 grados
centesimales.

Un arco en una porción de circunferencia y su relación con el ángulo y su radio es:

C=R*ángulo.
Las coordenadas cartesianas de un punto en el eje cartesiano son:

X= L * cos (ángulo)
Y= L * sen (ángulo)

La cinemática directa de un robot, consiste pues, en calcular estas coordenadas, mediante su

posición dada por la longitud de su brazo y por su ángulo.

Y sus coordenadas polares:

Ángulo= arcotan (Y/x)

Distancia= RaizCuadrada(X^2+Y^2)

Cinemática de un robot con 2 Grados de Libertad:

Tenemos el mismo concepto pero aplicado a una extremidad con su propio Angulo y grado de
libertad.
Código:

#include <Servo.h> int anguloCodo;

int Grabar = 10; int valorPOTBrazo;

int Reproducir = 9; int anguloBrazo;

int Resetear = 8; int valorPOTMuneca;

int servoPinBase = 2; int anguloMuneca;

int servoPinHombro =3; int valorPOTPinza;

int servoPinCodo = 4; int anguloPinza;

int servoPinBrazo = 5;

int servoPinMuneca = 6; int ultimaFila=0;

int servoPinPinza = 7; int anguloBaseRep=0;

int POTBase = A0; int anguloHombroRep=0;

int POTHombro=A1; int anguloCodoRep=0;

int POTCodo=A2; int anguloBrazoRep=0;

int POTBrazo=A3; int anguloMunecaRep=0;

int POTMuneca=A4; int anguloPinzaRep=0;

int POTPinza=A5; int angulo1=0;

int MovGuardado[20][6]; int angulo2=0;

int i=0; int angulo3=0;

int j=0; int angulo4=0;

int iGrabar=0; int angulo5=0;

int jGrabar=0; int angulo6=0;

int iGrabarActual=0; Servo Base;

int iReproducir=0; Servo Hombro;

int jReproducir=0; Servo Codo;

int valorPOTBase; Servo Pinza;

int anguloBase; Servo Brazo;

int valorPOTHombro; Servo Muneca;

int anguloHombro;

int valorPOTCodo;
void setup() {
Serial.begin(9600);
pinMode(Grabar, INPUT);
pinMode(Reproducir, INPUT);
pinMode(Resetear, INPUT);
pinMode(POTBase, INPUT);
pinMode(POTHombro, INPUT);
pinMode(POTCodo, INPUT);
pinMode(POTBrazo, INPUT);
pinMode(POTMuneca, INPUT);
pinMode(POTPinza, INPUT);
Base.attach(servoPinBase);
Hombro.attach(servoPinHombro);
Codo.attach(servoPinCodo);
Pinza.attach(servoPinPinza);
Brazo.attach(servoPinBrazo);
Muneca.attach(servoPinMuneca);
for(i=0; i<20; i++)
{ for(j=0; j<6; j++)
{ MovGuardado[i][j]=0;
}
}
Serial.print("Inicializado: ");
}
void loop()
{ if(digitalRead(Reproducir) == LOW)
{
valorPOTBase = analogRead(POTBase);
anguloBase =
map(valorPOTBase,0,1023,0,180);
Base.write(anguloBase);
valorPOTHombro =
analogRead(POTHombro);
anguloHombro =
map(valorPOTHombro,110,890,0,180);
Hombro.write(anguloHombro);
valorPOTCodo =
analogRead(POTCodo);
anguloCodo =
map(valorPOTCodo,110,890,0,180);
Codo.write(anguloCodo);
valorPOTPinza =
analogRead(POTPinza);
anguloPinza =
map(valorPOTPinza,110,890,0,180);
Pinza.write(anguloPinza);
valorPOTBrazo =
analogRead(POTBrazo);
anguloBrazo =
map(valorPOTBrazo,110,890,0,180);
Brazo.write(anguloBrazo);
valorPOTMuneca =
analogRead(POTMuneca);
anguloMuneca =
map(valorPOTMuneca,110,890,0,180);
Muneca.write(anguloMuneca);
}
if(digitalRead(Resetear) == HIGH)
{ Serial.println("Borrando, espere...");
delay(100);
for(i=0; i<20; i++)
 { for(j=0; j<6; j++) {
MovGuardado[iGrabar][jGrabar]=anguloCo
MovGuardado[i][j]=0; do;

} Serial.println("Codo guardado");

Serial.print("Borrado: "); delay(300); }

Serial.println(i); }
if(jGrabar==3) {
Serial.println("Borrado Total");

iGrabar=0; MovGuardado[iGrabar][jGrabar]=anguloBra
zo;
ultimaFila=0; }
Serial.println("Brazo guardado");

delay(300); }
if ((digitalRead(Grabar) ==
HIGH)&&(ultimaFila!=19)) if(jGrabar==4) {

{ Serial.println("Grabando posicion");
MovGuardado[iGrabar][jGrabar]=anguloMu
delay(100);
neca;
for(iGrabar; iGrabar<=ultimaFila;
Serial.println("Muneca guardado");
iGrabar++)
delay(300);
{ for(jGrabar=0; jGrabar<6;
jGrabar++) } if(jGrabar==5) {

{
MovGuardado[iGrabar][jGrabar]=anguloPin
if(jGrabar==0)
za;
{
Serial.println("Pinza guardada");
MovGuardado[iGrabar][jGrabar]=anguloBas
e; delay(300); }

Serial.println("Base guardada"); }

delay(300); }
} ultimaFila+=1;

if(jGrabar==1) if(ultimaFila==19)

{ { Serial.println("Maximo de
posiciones guardadas, no se pueden añadir
mas posiciones");
MovGuardado[iGrabar][jGrabar]=anguloHo
mbro; } }

Serial.println("Hombro guardado"); if ((digitalRead(Grabar) ==

HIGH)&&(ultimaFila==19))
delay(300); }
{
if(jGrabar==2) {
 Serial.println("Maximo de posiciones angulo1=anguloBaseRep;
guardadas, no se pueden añadir mas
posiciones"); } }

} Serial.print("Reproduciendo base
posicion: "); Serial.println(iReproducir);
if (digitalRead(Reproducir) == HIGH)

{ Serial.println("Reproduciendo if(angulo1>MovGuardado[iReproducir][jRep
posicion"); roducir])

delay(100); {

angulo1=anguloBase; for(anguloBaseRep=angulo1;
anguloBaseRep >=
angulo2=anguloHombro; MovGuardado[iReproducir][jReproducir];
anguloBaseRep --)
angulo3=anguloCodo;
{
angulo4=anguloBrazo;
Base.write(anguloBaseRep);
angulo5=anguloMuneca;
delay(15);
angulo6=anguloPinza;
angulo1=anguloBaseRep;

} } }
for(iReproducir=0; iReproducir<iGrabar;
iReproducir++) if(jReproducir==1)

{ for(jReproducir=0; jReproducir<6; {
jReproducir++)
delay(100);
{ delay(100);
Serial.print("Reproduciendo Hombro
if(jReproducir==0) posicion: "); Serial.println(iReproducir);

{ delay(100);
if(angulo2<MovGuardado[iReproducir][jRep
Serial.print("Reproduciendo base
roducir])
posicion: "); Serial.println(iReproducir);
{
if(angulo1<MovGuardado[iReproducir][jRep for(anguloHombroRep=angulo2;
roducir]) anguloHombroRep <=
MovGuardado[iReproducir][jReproducir];
{
anguloHombroRep ++)
for(anguloBaseRep=angulo1;
{
anguloBaseRep <=
MovGuardado[iReproducir][jReproducir]; Hombro.write(anguloHombroRep);
anguloBaseRep ++)
delay(15);
{
angulo2=anguloHombroRep;
Base.write(anguloBaseRep);
}
delay(15);
 }
if(angulo3>MovGuardado[iReproducir][jRep
Serial.print("Reproduciendo Hombro roducir])
posicion: "); Serial.println(iReproducir);
{

if(angulo2>MovGuardado[iReproducir][jRep for(anguloCodoRep=angulo3;
roducir]) anguloCodoRep >=
MovGuardado[iReproducir][jReproducir];
{ anguloCodoRep --)

for(anguloHombroRep=angulo2; {
anguloHombroRep >=
MovGuardado[iReproducir][jReproducir]; Codo.write(anguloCodoRep);
anguloHombroRep --)
delay(15);
{
angulo3=anguloCodoRep;
Hombro.write(anguloHombroRep);
} } }
delay(15);
if(jReproducir==3)
angulo2=anguloHombroRep;
{
} } }
delay(100);
if(jReproducir==2)
Serial.print("Reproduciendo Brazo
{ delay(100); posicion: "); Serial.println(iReproducir);

Serial.print("Reproduciendo Codo
posicion: "); Serial.println(iReproducir); if(angulo4<MovGuardado[iReproducir][jRep
roducir])

if(angulo3<MovGuardado[iReproducir][jRep {
roducir])
for(anguloBrazoRep=angulo4;
{ anguloBrazoRep <=
MovGuardado[iReproducir][jReproducir];
for(anguloCodoRep=angulo3; anguloBrazoRep ++)
anguloCodoRep <=
MovGuardado[iReproducir][jReproducir]; {
anguloCodoRep ++)
Brazo.write(anguloBrazoRep);
{
delay(15);
Codo.write(anguloCodoRep);
angulo4=anguloBrazoRep;
delay(15);
} }
angulo3=anguloCodoRep;
Serial.print("Reproduciendo Brazo
} } posicion: "); Serial.println(iReproducir);

Serial.print("Reproduciendo Codo
posicion: "); Serial.println(iReproducir); if(angulo4>MovGuardado[iReproducir][jRep
roducir])
 { for(anguloMunecaRep=angulo5;
anguloMunecaRep >=
for(anguloBrazoRep=angulo4; MovGuardado[iReproducir][jReproducir];
anguloBrazoRep >= anguloMunecaRep --)
MovGuardado[iReproducir][jReproducir];
anguloBrazoRep --) {

{ Muneca.write(anguloMunecaRep);

Brazo.write(anguloBrazoRep); delay(15);

delay(15); angulo5=anguloMunecaRep;

angulo4=anguloBrazoRep; } } }

} } } if(jReproducir==5)

if(jReproducir==4) {

{ delay(100);

delay(100); Serial.print("Reproduciendo Pinza

posicion: "); Serial.println(iReproducir);
Serial.print("Reproduciendo Muneca
posicion: "); Serial.println(iReproducir);
if(angulo6<MovGuardado[iReproducir][jRep
roducir])
if(angulo5<MovGuardado[iReproducir][jRep
roducir]) {

{ for(anguloPinzaRep=angulo6;
anguloPinzaRep <=
for(anguloMunecaRep=angulo5; MovGuardado[iReproducir][jReproducir];
anguloMunecaRep <= anguloPinzaRep ++)
MovGuardado[iReproducir][jReproducir];
anguloMunecaRep ++) {

{ Pinza.write(anguloPinzaRep);

Muneca.write(anguloMunecaRep); delay(15);

delay(15); angulo6=anguloPinzaRep;

angulo5=anguloMunecaRep; } }

} } Serial.print("Reproduciendo Pinza
posicion: "); Serial.println(iReproducir);
Serial.print("Reproduciendo Muneca
posicion: "); Serial.println(iReproducir);
if(angulo6>MovGuardado[iReproducir][jRep
roducir])
if(angulo5>MovGuardado[iReproducir][jRep
roducir]) {

{ for(anguloPinzaRep=angulo6;
anguloPinzaRep >=
MovGuardado[iReproducir][jReproducir];
anguloPinzaRep --)
 {

Pinza.write(anguloPinzaRep); Hombro.write(angulo2);

delay(15); delay(15);

angulo6=anguloPinzaRep; } }

} } } if(anguloHombro<angulo2)

} } {

delay(1000); Serial.println("posicion inicial

Hombro");
if(anguloBase>angulo1)
for(angulo2; angulo2 >=
{ anguloHombro; angulo2 --)
Serial.println("posicion inicial Base"); {

for(angulo1; angulo1 <= anguloBase; Hombro.write(angulo2);

angulo1 ++)
delay(15);
{
} }
Base.write(angulo1);
if(anguloCodo>angulo3)
delay(15);
{
} }
Serial.println("posicion inicial Codo");
if(anguloBase<angulo1)
for(angulo3; angulo3 <= anguloCodo;
{ angulo3 ++)

Serial.println("posicion inicial Base"); {

for(angulo1; angulo1 >= anguloBase; Codo.write(angulo3);

angulo1 --)
delay(15);
{
} }
Base.write(angulo1);
if(anguloCodo<angulo3)
delay(15);
{
} }
Serial.println("posicion inicial Codo");
if(anguloHombro>angulo2)
for(angulo3; angulo3 >= anguloCodo;
{ angulo3 --)

Serial.println("posicion inicial {
Hombro");
Codo.write(angulo3);
for(angulo2; angulo2 <=
anguloHombro; angulo2 ++) delay(15);

{
 } } Serial.println("posicion inicial
Muneca");
if(anguloBrazo>angulo4)
for(angulo5; angulo5 >=
{ anguloMuneca; angulo5 --)

Serial.println("posicion inicial Brazo"); {

for(angulo4; angulo4 <= Muneca.write(angulo5);

anguloBrazo; angulo4 ++)
delay(15);
{
} }
Brazo.write(angulo4);
if(anguloPinza>angulo6)
delay(15);
{
} }
Serial.println("posicion inicial Pinza");

for(angulo6; angulo6 <= anguloPinza;

if(anguloBrazo<angulo4) angulo6 ++)
{ {

Serial.println("posicion inicial Brazo"); Pinza.write(angulo6);

for(angulo4; angulo4 >= delay(15);

anguloBrazo; angulo4 --)
} }
{
if(anguloPinza<angulo6)
Brazo.write(angulo4);
{
delay(15);
Serial.println("posicion inicial Pinza");
} }
for(angulo6; angulo6 >= anguloPinza;
if(anguloMuneca>angulo5) angulo6 --)

{ {
Serial.println("posicion inicial Pinza.write(angulo6);
Muneca");
delay(15);
for(angulo5; angulo5 <=
anguloMuneca; angulo5 ++) }

{ } }

Muneca.write(angulo5); }

delay(15);

} }

if(anguloMuneca<angulo5)