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:
#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