#include <Servo.h>

// Definisikan objek servo untuk setiap servo
Servo servoX;
Servo servoY;
Servo servoZ;

// Pin yang digunakan untuk masing-masing servo
#define SERVO_X_PIN PB8
#define SERVO_Y_PIN PB9
#define SERVO_Z_PIN PB11

// Fungsi untuk menggerakkan servo ke sudut tertentu
void moveServo(Servo servo, int angle) {
  servo.write(angle);
  delay(1000); // Tambahkan penundaan untuk memberikan waktu servo bergerak
}

void setup() {
  // Inisialisasi Serial Monitor
  Serial.begin(9600);

  // Inisialisasi servo
  servoX.attach(SERVO_X_PIN);
  servoY.attach(SERVO_Y_PIN);
  servoZ.attach(SERVO_Z_PIN);
}

void loop() {
  // Contoh gerakan kaki robot SAR
  Serial.println("Moving leg to position (0, 0, 0)");
  moveLeg(0, 0, 0);
  delay(1000);

  Serial.println("Moving leg to position (90, 90, 90)");
  moveLeg(90, 90, 90);
  delay(1000);

	Serial.println("Moving leg to position (180, 180, 180)");
  moveLeg(180, 180, 180);
  delay(1000);
}

// Fungsi untuk menggerakkan kaki robot ke koordinat x, y, z
void moveLeg(int x, int y, int z) {
  // Hitung sudut rotasi untuk setiap servo berdasarkan koordinat x, y, z
  int angleX = map(x, 0, 180, 0, 180);
  int angleY = map(y, 0, 180, 0, 180);
  int angleZ = map(z, 0, 180, 0, 180);

  // Menggerakkan servo ke sudut yang telah dihitung
  moveServo(servoX, angleX);
  moveServo(servoY, angleY);
  moveServo(servoZ, angleZ);
}