Make: Robotic Arms

179Chapter 9: Inverse Kinematics for 3D Movement

double d;

double Zoffset;

double theta;

double alphaOne;

double alphaTwo;

double alphaFinal;

double beta;

void setup() {

servoOne.attach(3);

servoTwo.attach(5);

servoThree.attach(6);

servoFour.attach(9);

servoFive.attach(10);

Serial.begin(9600);

}

void loop() {

int points[8][3] = {

{0, 80, 120},

{80, 80, 120},

{80, 160, 120},

{0, 160, 120},

{0, 80, 200},

{80, 80, 200},

{80, 160, 200},

{0, 160, 200}

};

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

x = points[i][0];

y = points[i][1];

z = points[i][2];

d = sqrt((x*x)+(y*y));

theta = atan(y/x)*(180/PI);

Zoffset = (z - linkOne);

c = sqrt((d*d)+(Zoffset*Zoffset)); ...