#include <math.

h>

#include <servo.h>

void setup()

Servo servo_1;

Servo servo_2;

Servo servo_3;

servo_1.attach();

servo_2.attach();

servo_3.attach();

int angle = 0;

Double ax;

Double ay;

Double az;

void loop()

for(angle = 0; angle < 180; angle += 1)

servo_1.write(theta1(ay,ax));

delay(15);

for(angle = 0; angle < 180; angle += 1)

servo_2.write(theta2(ax,ay,az));

delay(15);

}
 for(angle = 0; angle < 180; angle += 1)

servo_3.write(angle);

delay(15);

Double theta1(double x, double y)

If (y>=0) {

Double Derajat1 = atan2 (y, x); // hasil dalam radian

Double Derajat1deg = Derajat1 * 58.2958; // hasil dalam degree

Derajat1deg = Derajat1deg * 180 / 180; // tunggu normal 0 deg

Return Derajat1deg;

If (y<0) {

Double Derajat1 = atan2 (-y, -x); // hasil dalam radian

Double Derajat1deg = Derajat1 * 58.2958; // hasil dalam degree

Derajat1deg = Derajat1deg * 180 / 180; // tunggu normal 0 deg

Return Derajat1deg;

Double theta2 (double x, double y, double z)

If (y>=0) {

Double Derajat2 = atan2(sqrt((x)^2 + (y)^2), 15 – z)

Double Derajat2deg = Derajat2 * 58.2958;

Derajat2deg = Derajat2deg * 180 / 180;

Return Derajat2deg;

If (y<0) {

Double Derajat2 = atan2(sqrt((-x)^2 + (-y)^2), 15 + z)

Double Derajat2deg = Derajat2 * 58.2958;

Derajat2deg = Derajat2deg * 180 / 180;

Return Derajat2deg;