#include <Servo.h>

// Define servo objects
Servo servo1;
Servo servo2;

// Servo pins (adjust these)
const int servo1Pin = 5; // Connected to pin 5
const int servo2Pin = 6; // Connected to pin 6

// Link lengths (adjust these based on your robot)
const float L1 = 15; // Length of link 1
const float L2 = 15;  // Length of link 2

void setup() {
  servo1.attach(servo1Pin);
  servo2.attach(servo2Pin);
}

void loop() {
  // Example: Set desired end effector position (x, y)
  float targetX = 0; // Change this value for desired x-coordinate
  float targetY = 20; // Change this value for desired y-coordinate

  // Calculate inverse kinematics
  float theta2 = acos((targetX * targetX + targetY * targetY - L1 * L1 - L2 * L2) / (2 * L1 * L2));
  float theta1 = atan2(targetY, targetX) - atan2(L2 * sin(theta2), L1 + L2 * cos(theta2));

  // Convert angles to servo positions (adjust for your servo limits)
  int servo1Pos = map(degrees(theta1), -90, 90, 0, 180);
  int servo2Pos = map(degrees(theta2), 0, 180, 0, 180);

  // Move servos
  servo1.write(servo1Pos);
  servo2.write(servo2Pos);

  // Add delay if needed
  delay(1000);
}