
184 Make: Robotic Arms
}
else if(valueSelector == 4){
claw-=5;
}
} else {
if(valueSelector == 1){
x+=5;
}
else if(valueSelector == 2){
y+=5;
}
else if(valueSelector == 3){
z+=5;
}
else if(valueSelector == 4){
claw+=5;
}
}
lastStateCLK = currentStateCLK;
d = sqrt((x*x)+(y*y));
theta = atan(y/x)*(180/PI);
Zoffset = (z - linkOne);
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 ...