Make: Robotic Arms

173Chapter 9: Inverse Kinematics for 3D Movement

beta = acos((linkTwo*linkTwo + linkThree*linkThree -

c*c)/(2*linkTwo*linkThree))*(180/PI);

alphaOne = acos(d/c);

alphaTwo = acos((linkTwo*linkTwo + c*c -

linkThree*linkThree)/(2*linkTwo*c));

if(z > linkOne){

alphaFinal = (alphaOne+alphaTwo)*( 180/PI);

}

else if(z < linkOne){

alphaFinal = (alphaTwo-alphaOne)*(180/PI);

}

Serial.print("Theta: ");

Serial.print(theta);

Serial.print(" Alpha: ");

Serial.print(alphaFinal);

Serial.print(" Beta: ");

Serial.println(beta);

delay(100);

}

One of the first things we need to do is set up

the many variables we need to complete our

calculations. Some of the most ...