#include <Servo.h>

// Deklarasi servo motor
Servo servo1;
Servo servo2;

// Panjang link
const float L1 = 10.0; // Panjang link 1 dalam cm
const float L2 = 10.0; // Panjang link 2 dalam cm

// Fungsi untuk menghitung sudut dari posisi (x, y) yang diinginkan
void calculateInverseKinematics(float x, float y, float &theta1, float &theta2) {
  // Menghitung cos(theta2) dan sin(theta2)
  float cosTheta2 = (x*x + y*y - L1*L1 - L2*L2) / (2 * L1 * L2);
  float sinTheta2 = sqrt(1 - cosTheta2*cosTheta2);
  
  // Menghitung theta2 dalam radian
  theta2 = atan2(sinTheta2, cosTheta2);
  
  // Menghitung theta1 dalam radian
  theta1 = atan2(y, x) - atan2(L2 * sinTheta2, L1 + L2 * cosTheta2);
  
  // Konversi dari radian ke derajat
  theta1 = theta1 * 180.0 / PI;
  theta2 = theta2 * 180.0 / PI;
}

void setup() {
  Serial.begin(9600);


  // Menetapkan pin servo
  servo1.attach(9);
  servo2.attach(10);
}

void loop() {
  // Posisi yang diinginkan untuk ujung efektor
  float x = 10.0; // cm
  float y = 00.0; // cm

  // Variabel untuk menyimpan sudut sendi
  float theta1, theta2;
  
  // Menghitung sudut sendi
  calculateInverseKinematics(x, y, theta1, theta2);

  // Cetak hasil
  Serial.println();
  Serial.println("Hasil Akhir");
  Serial.print("Theta1: ");
  Serial.println(theta1); // Konversi ke derajat
  Serial.print("Theta2: ");
  Serial.println(theta2); // Konversi ke derajat

  // Menggerakkan servo ke sudut yang dihitung
  servo1.write(theta1);
  servo2.write(theta2);
  
  // Menunggu sejenak untuk melihat pergerakan
  delay(2000);
}