Make: Robotic Arms

105Chapter 6: Intermediate Inverse Kinematics

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

betaTwo = acos(((linkTwo*link-

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

Serial.print(" beta Two:" ...