Ciao a tutti! Sto facendo un progetto che consiste in un Robot Puma che dati degli input attraverso la cinematica inversa si muove in una posizione specifica negli assi x,y,z. Tuttavia qualcosa non torna, e non si posiziona nel punto desiderato. Le equazioni della cinematica dovrebbero essere corrette in quanto le ho prese da un libro di testi. Vi allego il mio codice, vi ringrazio anticipatamente!
#include<Servo.h>
Servo MyServo01;
Servo MyServo02;
Servo MyServo03;
const int h = 60; //anca
const int link01 = 100; //femore
const int link02 = 70; // tibia
float alpha,beta,gamma, rad, rad2, rad3;
void setup()
{
MyServo01.attach(9);
MyServo02.attach(10);
MyServo03.attach(11);
Serial.begin(9600);
}
void loop()
{
trigono_xyz(50 ,50, 50); //inserisco x,y,z
Serial.print("alpha= ");
Serial.print(alpha);
Serial.print(", beta= ");
Serial.print(beta);
Serial.print(", gamma= ");
Serial.print(gamma);
Serial.println();
delay(50);
MyServo01.write(alpha);
MyServo02.write(beta);
MyServo03.write(gamma);
}
void trigono_xyz(int x, int y, int z)
{
rad = atan2(y,x);
alpha=(180rad)/3.14;
rad3 = acos(((pow(x,2))+pow(y,2)+pow(z-h,2)-pow(link01,2)-pow(link02,2))/(2link01link02));
gamma=(180rad3)/3.14;
rad2 = atan2(xcos(alpha)+ysin(alpha), z-h) - atan2(link02sin(gamma), link01 + link02cos(gamma));
beta = (180*rad2)/3.14;
}