Make: Robotic Arms

123Chapter 6: Intermediate Inverse Kinematics

betaOne = atan(y / (d - x)) * (180 / PI);

betaTwo = acos(((linkTwo * linkTwo) - (e * e) -

(linkOne * linkOne)) / (-2 * linkOne * e)) * (180 / PI);

if (x < 0) {

alphaFinal = 180 + ((alphaOne) + (alphaTwo));

betaFinal = 180 - ((betaOne) + (betaTwo));

}

else if (x > d) {

alphaFinal = ((alphaOne) + (alphaTwo));

betaFinal = -1 * ((betaOne) + (betaTwo));

}

else {

alphaFinal = ((alphaOne) + (alphaTwo));

betaFinal = 180 - ((betaOne) + (betaTwo));

}

Serial.print("alpha One: ");

Serial.print(alphaOne);

Serial.print(" beta One:");

Serial.print(betaOne);

Serial.print(" alpha Two: ");

Serial.print(alphaTwo); ...