
108 Make: Robotic Arms
void loop()
{
c = sqrt((x*x)+(y*y));
e = sqrt(((d-x)*(d-x))+(y*y));
alphaOne = atan(y/x) * (180/PI);
alphaTwo = acos(((linkTwo*linkTwo)-(c*c)-(linkOne*linkOne))
/(-2*linkOne*c)) * (180/PI);
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 ...