3DOF Inverse Kinematics For Arm/Leg of Robot Using Arduino

Inverse Kinematics

Arduino Code:

const float cx=2; //coxa
const float fm=6.2; //femur
const float tb=8.3; // tibia
float L, L1;
float alpha, alpha1,alpha2,beta,gama;
void setup()
{
 Serial.begin(9600); 
}
void loop()
{
trigono_xyz(2, 4, 6); //contoh x,y,z
Serial.print("gama= ");
Serial.print(gama);
Serial.print(", alpha= ");
Serial.print(alpha);
Serial.print(", beta= ");
Serial.print(beta);
Serial.println();
}

void trigono_xyz(float x, float y, float z)
{
 L1=sqrt(sq(x)+sq(y));
 gama=atan(x/y)/PI*180;
 L=sqrt(sq(L1-cx)+sq(z));
 beta=acos((sq(tb)+sq(fm)-sq(L))/(2*tb*fm))/PI*180;
 alpha1=acos(z/L)/PI*180;
 alpha2=acos((sq(fm)+sq(L)-sq(tb))/(2*fm*L))/PI*180;
 alpha=alpha1+alpha2;
}

Youtube: http://youtu.be/ziz6FRfRfJM

8 thoughts on “3DOF Inverse Kinematics For Arm/Leg of Robot Using Arduino

    • Maaf baru respon, kodenya dimana naruhnya dah lupa mesti diubek2 lagi,, dan mungkin sdr rizky dah ga perlu lagi :D,, maaf ya

  1. Hey clean code 🙂 Can u explain the void loop part? and “trigono_xyz” is this the co ordinates for the end effector?

    • yes, xyz in trigono_xyz is end effector coordinates, on the example code above, void loop part just call trigono_xyz() function, eg, end effector we desire is x=2, y=4, z=6, we write trigono_xyz(2,4,6) inside void loop, trigono_xyz will calculate the angle for each joint and display it on serial monitor, you could use another xyz end effector position as long as lenght of coxa, femur and tibia could reach it.

Leave a Reply

Your email address will not be published. Required fields are marked *

This site uses Akismet to reduce spam. Learn how your comment data is processed.