Make: Robotic Arms

119Chapter 6: Intermediate Inverse Kinematics

void loop()

{

int points[4][2] = {

{0, 80},

{80, 80},

{80, 160},

{0, 160}

};

for (int i = 0; i < 4; i++) {

x = points[i][0];

y = points[i][1];

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