Go Down

Topic: Cinemática Inversa Brazo Robot 5 ejes (Read 2778 times) previous topic - next topic

3Dadicto

Aug 16, 2017, 11:59 am Last Edit: Aug 16, 2017, 06:40 pm by 3Dadicto Reason: Los códigos deben verse si son menores a 9k
Buenas,

Estoy desarrollando un programa con la cinemática inversa para un brazo robot de 5 ejes de forma que cuando introduzca las coordenadas x,y,z, mediante cálculos trigonométricos obtenga los ángulos de giro de cada eje.  Estos ángulos se los aplica a cada servo y el brazo se posiciona en el espacio.

He conseguido que funcione y me de los resultados correctamente pero solo cuando pongo  la matriz DatosArray dentro del void setup() y encima de todas las ecuaciones:


Cuando lo pongo dentro del void Coordenada1() no consigo que funcione.  Lo que quiero conseguir es distintos void con coordenadas de forma que cuando introduzca las coordenadas en cada void me calcule la posición del brazo haciendo uso de las ecuaciones que hay definidas en el void setup().

Perdonad si no me he explicado bien y si lo que estoy preguntado es algo muy obvio pero soy novato en esto de programar.

Adjunto el programa:

Code: [Select]
#include <Servo.h>

Servo servo1;  //servo de giro de base
Servo servo2a;   //servo a de giro de brazo
Servo servo2b;   //servo b de giro de brazo
Servo servo3;   //servo de giro de antebrazo
Servo servo4;   //servo de giro de muñequilla
Servo servo5;   //servo de giro de pinza
Servo servo6;   //servo de apertura/cierre de pinza


//Ajustar valores de servos de posición cero
int home_servo1=93;
int home_servo2a=90;
int home_servo2b=93;
int home_servo3=72;
int home_servo4=95;
int home_servo5=80;
int home_servo6=140;  //abierta 140º y cerrada 70º

//Definicion de variables de la cinematica inversa

const float pi=3.1415;
float x;  //coordenadas de la pinza del robot
float y;
float z;

float b=185;  //longitud de brazo
float ab=138;  //longitud de antebrazo
float m=172;  //longitud de muñequilla
float H=85;  //altura de base
float cabGrados;  //angulo de cabeceo en grados
float cabRAD;  //angulo de cabeceo en radianes

float Axis1;  //Giro de la base en RAD
float Axis2;  //Giro del brazo en RAD
float Axis3;  //Giro del antebrazo en RAD
float Axis4;  //Giro de la muñequilla en RAD
float Axis5;  //Giro de la pinza en grados
float Pinza;  //Pinza en grados

float Axis1Grados;   //Giro de la base en Grados
float Axis2Grados;   //Giro del brazo en Grados
float Axis3Grados;   //Giro del antebrazo en Grados
float Axis4Grados;   //Giro de la muñequilla en Grados

float M;
float xprima;
float yprima;
float Afx;
float Afy;
float A;
float B;
float Hip;
float alfa;
float beta;
float gamma;

 

void setup() {
Serial.begin(9600);

// asignacion de servos a pines y centrar servos
servo1.attach(5);
servo1.write(home_servo1);
servo2a.attach(6);
servo2a.write(home_servo2a);
servo2b.attach(7);
servo2b.write(home_servo2b);
servo3.attach(8);
servo3.write(home_servo3);
servo4.attach(9);
servo4.write(home_servo4);
servo5.attach(10);
servo5.write(home_servo5);
servo6.attach(11);
servo6.write(home_servo6);


}

//////////////////////////////////////
void PosicionCero()
{
servo1.write(home_servo1);
servo2a.write(home_servo2a);
servo2b.write(home_servo2b);
servo3.write(home_servo3);
servo4.write(home_servo4);
servo5.write(home_servo5);
servo6.write(home_servo6);
}

//////////////////////////////////////
void Coordenada1()
{

float DatosArray[] = {384, -50, 176, 0, 0, -45};

x=DatosArray[0];
y=DatosArray[1];
z=DatosArray[2];
cabGrados=DatosArray[3];
Axis5=DatosArray[4];
Pinza=DatosArray[5];

cabRAD=cabGrados*pi/180;  //angulo de cabeceo en radianes

Axis1=atan2(y,x); 
M=sqrt(pow(x,2)+pow(y,2));
xprima=M;
yprima=z;
Afx=cos(cabRAD)*m;
B=xprima-Afx;
Afy=sin(cabRAD)*m;
A=yprima+Afy-H;
Hip=sqrt(pow(A,2)+pow(B,2));
alfa=atan2(A,B);
beta=acos((pow(b,2)-pow(ab,2)+pow(Hip,2))/(2*b*Hip));
Axis2=alfa+beta;
gamma=acos((pow(b,2)+pow(ab,2)-pow(Hip,2))/(2*b*ab));
Axis3=gamma;
Axis4=2*pi-cabRAD-Axis2-Axis3;

Axis1Grados=Axis1*180/pi;   //Giro de la base en Grados
Axis2Grados=90-Axis2*180/pi;   //Giro del brazo en Grados + conversion a angulos catia/servos
Axis3Grados=180-Axis3*180/pi;   //Giro del antebrazo en Grados + conversion a angulos catia/servos
Axis4Grados=180-Axis4*180/pi;   //Giro de la muñequilla en Grados + conversion a angulos catia/servos

servo1.write(home_servo1+Axis1Grados);
servo2a.write(home_servo2a+Axis2Grados);
servo2b.write(home_servo2b-Axis2Grados);
servo3.write(home_servo3+Axis3Grados);
servo4.write(home_servo4+Axis4Grados);
servo5.write(home_servo5+Axis5);
servo6.write(home_servo6+Pinza);

Serial.println("Coordenada 1: ");

Serial.print("Axis1 en grados: ");
Serial.println(Axis1Grados);
Serial.print("Axis2 en grados: ");
Serial.println(Axis2Grados);
Serial.print("Axis3 en grados: ");
Serial.println(Axis3Grados);
Serial.print("Axis4 en grados: ");
Serial.println(Axis4Grados);

}

//////////////////////////////////////


void loop()
{

 Coordenada1();


}


Gracias

ArielP

Hola!
Acabo de descargar el applet, y si no veo mal, no tienes un "void loop()", es decir, el applet no posee un cuerpo principal que se ejecute. Solo se definen variables globales, se ejecuta el código dentro de Setup() una vez, y....finaliza.

¿Puede ser ese el error?

Saludos!

3Dadicto

Hola!
Acabo de descargar el applet, y si no veo mal, no tienes un "void loop()", es decir, el applet no posee un cuerpo principal que se ejecute. Solo se definen variables globales, se ejecuta el código dentro de Setup() una vez, y....finaliza.

¿Puede ser ese el error?

Saludos!
Hola,

Si tengo un void loop() y dentro está el void coordenada1() que es el que quiero hacer funcionar como he explicado antes.   Dentro del void coordenada1() es donde quiero introducir las  coordenadas del espacio donde quiero que se mueva el brazo mediante las ecuaciones que hay dentro del void setup.  No se si me explico.

Gracias

surbyte

#3
Aug 16, 2017, 12:44 pm Last Edit: Aug 16, 2017, 03:37 pm by surbyte
ArielP, el código tiene loop no viste el programa adjunto, y solo te quedaste con lo que estaba posteado.
Ahora he editado el post para que se vea directamente.

Ese no es el problema, analizo y daré mi opinión.

EDITO : algo que he vistoy me parece muy mal es el uso de delay en tu brazo. No se si debes esperar o no el accionamiento de sensores pero cada vez que usas delay demoras todo el resto de las operaciones.

Tenlo presente!!
Alternativa: lee en Documentación Maquina de estados. No es fácil ni dificil pero hara que tu programa tenga otra velocidad.

Respecto a que se vean resultados en la rutina Coordenadas cual es el problema? Agrega los Serial.print o println que necesites dentro de dicha rutina.

3Dadicto

ArielP, el código tiene loop no viste el programa adjunto, y solo te quedaste con lo que estaba posteado.
Ahora he editado el post para que se vea directamente.

Ese no es el problema, analizo y daré mi opinión.

EDITO : algo que he vistoy me parece muy mal es el uso de delay en tu brazo. No se si debes esperar o no el accionamiento de sensores pero cada vez que usas delay demoras todo el resto de las operaciones.

Tenlo presente!!
Alternativa: lee en Documentación Maquina de estados. No es fácil ni dificil pero hara que tu programa tenga otra velocidad.

Respecto a que se vean resultados en la rutina Coordenadas cual es el problema? Agrega los Serial.print o println que necesites dentro de dicha rutina.
Hola,

He actualizado el código y el brazo funciona.  También funcionan los Serial.print dentro de la rutina (algo estaba haciendo mal).

El código actualizado lo he puesto en el primer mensaje.


Lo que yo quería saber y no consigo hacer es dejar todas las ecuaciones dentro del void setup y dentro de los void Coordenada1(), Coordenada2(), etc, meter solamente la matriz de DatosArray con las coordenadas.

La manera que he encontrado de que funcione es meter dentro del void Coordenada1() todas las ecuaciones y claro, si tengo que hacer esto para todos los void de coordenadas que genere el código resultante va a ser muy grande.





surbyte

El actualizado no lo posteaste asi que si te vamos a dar alguna sugerencia mejor sería que lo hagas.

3Dadicto

Adjunto código actualizado:

Code: [Select]
#include <Servo.h>

Servo servo1;  //servo de giro de base
Servo servo2a;   //servo a de giro de brazo
Servo servo2b;   //servo b de giro de brazo
Servo servo3;   //servo de giro de antebrazo
Servo servo4;   //servo de giro de muñequilla
Servo servo5;   //servo de giro de pinza
Servo servo6;   //servo de apertura/cierre de pinza


//Ajustar valores de servos de posición cero
int home_servo1=93;
int home_servo2a=90;
int home_servo2b=93;
int home_servo3=72;
int home_servo4=95;
int home_servo5=80;
int home_servo6=140;  //abierta 140º y cerrada 70º

//Definicion de variables de la cinematica inversa

const float pi=3.1415;
float x;  //coordenadas de la pinza del robot
float y;
float z;

float b=185;  //longitud de brazo
float ab=138;  //longitud de antebrazo
float m=172;  //longitud de muñequilla
float H=85;  //altura de base
float cabGrados;  //angulo de cabeceo en grados
float cabRAD;  //angulo de cabeceo en radianes

float Axis1;  //Giro de la base en RAD
float Axis2;  //Giro del brazo en RAD
float Axis3;  //Giro del antebrazo en RAD
float Axis4;  //Giro de la muñequilla en RAD
float Axis5;  //Giro de la pinza en grados
float Pinza;  //Pinza en grados

float Axis1Grados;   //Giro de la base en Grados
float Axis2Grados;   //Giro del brazo en Grados
float Axis3Grados;   //Giro del antebrazo en Grados
float Axis4Grados;   //Giro de la muñequilla en Grados

float M;
float xprima;
float yprima;
float Afx;
float Afy;
float A;
float B;
float Hip;
float alfa;
float beta;
float gamma;

 

void setup() {
Serial.begin(9600);

// asignacion de servos a pines y centrar servos
servo1.attach(5);
servo1.write(home_servo1);
servo2a.attach(6);
servo2a.write(home_servo2a);
servo2b.attach(7);
servo2b.write(home_servo2b);
servo3.attach(8);
servo3.write(home_servo3);
servo4.attach(9);
servo4.write(home_servo4);
servo5.attach(10);
servo5.write(home_servo5);
servo6.attach(11);
servo6.write(home_servo6);


}

//////////////////////////////////////
void PosicionCero()
{
servo1.write(home_servo1);
servo2a.write(home_servo2a);
servo2b.write(home_servo2b);
servo3.write(home_servo3);
servo4.write(home_servo4);
servo5.write(home_servo5);
servo6.write(home_servo6);
}

//////////////////////////////////////
void Coordenada1()
{

float DatosArray[] = {384, -50, 176, 0, 0, -45};

x=DatosArray[0];
y=DatosArray[1];
z=DatosArray[2];
cabGrados=DatosArray[3];
Axis5=DatosArray[4];
Pinza=DatosArray[5];

cabRAD=cabGrados*pi/180;  //angulo de cabeceo en radianes

Axis1=atan2(y,x); 
M=sqrt(pow(x,2)+pow(y,2));
xprima=M;
yprima=z;
Afx=cos(cabRAD)*m;
B=xprima-Afx;
Afy=sin(cabRAD)*m;
A=yprima+Afy-H;
Hip=sqrt(pow(A,2)+pow(B,2));
alfa=atan2(A,B);
beta=acos((pow(b,2)-pow(ab,2)+pow(Hip,2))/(2*b*Hip));
Axis2=alfa+beta;
gamma=acos((pow(b,2)+pow(ab,2)-pow(Hip,2))/(2*b*ab));
Axis3=gamma;
Axis4=2*pi-cabRAD-Axis2-Axis3;

Axis1Grados=Axis1*180/pi;   //Giro de la base en Grados
Axis2Grados=90-Axis2*180/pi;   //Giro del brazo en Grados + conversion a angulos catia/servos
Axis3Grados=180-Axis3*180/pi;   //Giro del antebrazo en Grados + conversion a angulos catia/servos
Axis4Grados=180-Axis4*180/pi;   //Giro de la muñequilla en Grados + conversion a angulos catia/servos

servo1.write(home_servo1+Axis1Grados);
servo2a.write(home_servo2a+Axis2Grados);
servo2b.write(home_servo2b-Axis2Grados);
servo3.write(home_servo3+Axis3Grados);
servo4.write(home_servo4+Axis4Grados);
servo5.write(home_servo5+Axis5);
servo6.write(home_servo6+Pinza);

Serial.println("Coordenada 1: ");

Serial.print("Axis1 en grados: ");
Serial.println(Axis1Grados);
Serial.print("Axis2 en grados: ");
Serial.println(Axis2Grados);
Serial.print("Axis3 en grados: ");
Serial.println(Axis3Grados);
Serial.print("Axis4 en grados: ");
Serial.println(Axis4Grados);

}

//////////////////////////////////////


void loop()
{

 Coordenada1();


}

surbyte

Bueno tu código esta bien, sin mucho para sugerir.

3Dadicto

Bueno tu código esta bien, sin mucho para sugerir.
Si, si el código funciona, lo que yo quería saber es si se puede dejar todas las ecuaciones dentro del void setup y dentro de los void Coordenada1(), Coordenada2(), etc, meter solamente la matriz de DatosArray con las coordenadas que son las variables y que estas variables utilicen las ecuaciones que están puestas en el void setup.  No se si me explico.

Gracias

surbyte

No y si.
No porque si las ecuaciones estan en el setup se ejecutarán solo una vez, asi que las tienes que tener disponibles para llamarlas cuando requieras de la conversión.

Lo que veo es que tienes un inconveniente con el momento en que pides datos.
Eso tal como esta te debe llenar el monitor serie de datos y no se entiende nada.
Porque no haces algo asi mejor.

Los solicitas cada 1 segundo por ejemplo.
Esto no cambia mucho tu código, solo cambia cuando muestra los datos pero Coordenadas1 se sigue ejecutando a toda velocidad y los servos se debería mover tmb... algo que no me gusta tanto.

Code: [Select]
#include <Servo.h>

Servo servo1;  //servo de giro de base
Servo servo2a;   //servo a de giro de brazo
Servo servo2b;   //servo b de giro de brazo
Servo servo3;   //servo de giro de antebrazo
Servo servo4;   //servo de giro de muñequilla
Servo servo5;   //servo de giro de pinza
Servo servo6;   //servo de apertura/cierre de pinza


//Ajustar valores de servos de posición cero
int home_servo1 = 93;
int home_servo2a = 90;
int home_servo2b = 93;
int home_servo3 = 72;
int home_servo4 = 95;
int home_servo5 = 80;
int home_servo6 = 140;  //abierta 140º y cerrada 70º

//Definicion de variables de la cinematica inversa

const float pi = 3.1415;
const float f180dpi = pi/180;
float x;  //coordenadas de la pinza del robot
float y;  //
float z;  //

float b = 185;  //longitud de brazo
float ab = 138;  //longitud de antebrazo
float m = 172;  //longitud de muñequilla
float H = 85;  //altura de base
float cabGrados;  //angulo de cabeceo en grados
float cabRAD;  //angulo de cabeceo en radianes

float Axis1;  //Giro de la base en RAD
float Axis2;  //Giro del brazo en RAD
float Axis3;  //Giro del antebrazo en RAD
float Axis4;  //Giro de la muñequilla en RAD
float Axis5;  //Giro de la pinza en grados
float Pinza;  //Pinza en grados

float Axis1Grados;   //Giro de la base en Grados
float Axis2Grados;   //Giro del brazo en Grados
float Axis3Grados;   //Giro del antebrazo en Grados
float Axis4Grados;   //Giro de la muñequilla en Grados

float M;
float xprima;
float yprima;
float Afx;
float Afy;
float A;
float B;
float Hip;
float alfa;
float beta;
float gamma;

unsigned long start;

void muevoServos(float ax1, float ax2, float ax3, float ax4, float ax5, float apinza) {
  servo1.write(home_servo1  + ax1);
  servo2a.write(home_servo2a+ ax2);
  servo2b.write(home_servo2b- ax2);
  servo3.write(home_servo3  + ax3);
  servo4.write(home_servo4  + ax4);
  servo5.write(home_servo5  + ax5);
  servo6.write(home_servo6  + apinza);
}


void setup() {
Serial.begin(9600);

// asignacion de servos a pines y centrar servos
servo1.attach(5);
  servo2a.attach(6);
servo2b.attach(7);
servo3.attach(8);
servo4.attach(9);
servo5.attach(10);
servo6.attach(11);

  muevoServos(0,0,0,0,0,0);
  }

//////////////////////////////////////
void Coordenada1() {

float DatosArray[] = {384, -50, 176, 0, 0, -45};

x = DatosArray[0];
y = DatosArray[1];
z = DatosArray[2];
cabGrados = DatosArray[3];
Axis5 = DatosArray[4];
Pinza = DatosArray[5];

cabRAD = cabGrados*pi/180;  //angulo de cabeceo en radianes

Axis1 = atan2(y,x); 
M   = sqrt(pow(x,2)+pow(y,2));
xprima = M;
yprima = z;
Afx   = cos(cabRAD)*m;
B   = xprima-Afx;
Afy   = sin(cabRAD)*m;
A   = yprima+Afy-H;
Hip   = sqrt(pow(A,2)+pow(B,2));
alfa   = atan2(A,B);
beta    = acos((pow(b,2)-pow(ab,2)+pow(Hip,2))/(2*b*Hip));
Axis2 = alfa+beta;
gamma = acos((pow(b,2)+pow(ab,2)-pow(Hip,2))/(2*b*ab));
Axis3 = gamma;
Axis4 = 2*pi-cabRAD-Axis2-Axis3;

Axis1Grados = Axis1*f180dpi;   //Giro de la base en Grados
Axis2Grados = 90-Axis2*f180dpi;   //Giro del brazo en Grados + conversion a angulos catia/servos
Axis3Grados = 180-Axis3*f180dpi;   //Giro del antebrazo en Grados + conversion a angulos catia/servos
Axis4Grados = 180-Axis4*f180dpi;   //Giro de la muñequilla en Grados + conversion a angulos catia/servos
}


void loop() {
 Coordenada1();
 muevoServos(Axis1Grados, Axis2Grados, Axis3Grados, Axis4Grados, Axis5, Pinza);
 if (millis()-start > 1000) {
      Serial.println("Coordenada 1: ");

      Serial.print("Axis1 en grados: ");
      Serial.println(Axis1Grados);
      Serial.print("Axis2 en grados: ");
      Serial.println(Axis2Grados);
      Serial.print("Axis3 en grados: ");
      Serial.println(Axis3Grados);
      Serial.print("Axis4 en grados: ");
      Serial.println(Axis4Grados);
      start = millis();
  }
}



Algo mas funcional y con presentación de datos cada 1 segundo.
Deberías considerar que solo calcule posicon y mueva cuando haya cambios en los sensores.

3Dadicto


Go Up