Make: Robotic Arms

177Chapter 9: Inverse Kinematics for 3D Movement

c = sqrt((d*d)+(Zoffset*Zoffset));

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);

}

servoOne.write(theta);

servoTwo.write(alphaFinal);

servoThree.write(180-alphaFinal);

servoFour.write(beta);

Serial.print("Theta: ");

Serial.print(theta);

Serial.print(" Alpha: ");

Serial.print(alphaFinal);

Serial.print(" Beta: ");

Serial.