#include <Servo.h>  // Library untuk mengendalikan servo

// Membuat objek servo untuk masing-masing sambungan robot arm
Servo servo 1;  // Servo untuk basis
Servo servo 2;  // Servo untuk siku pertama
Servo servo 3;  // Servo untuk siku kedua
Servo servo 4;  // Servo untuk gripper

// Pin untuk servo
const int servoPin1 = 9;
const int servoPin2 = 10;
const int servoPin3 = 11;
const int servoPin4 = 12;

void setup() {
  // Menghubungkan servo dengan pin yang sesuai
  servo1.attach(servoPin1);
  servo2.attach(servoPin2);
  servo3.attach(servoPin3);
  servo4.attach(servoPin4);

  // Inisialisasi posisi awal untuk servo
  servo1.write(90);  // Posisi dasar
  servo2.write(90);  // Posisi siku pertama
  servo3.write(90);  // Posisi siku kedua
  servo4.write(90);  // Posisi gripper (closed)
  delay(1000);  // Tunggu 1 detik untuk memastikan servo bergerak ke posisi awal
}

void loop() {
  // Contoh gerakan: Menggerakkan robot arm ke beberapa posisi berbeda

  // Posisi 1
  servo1.write(90);   // Basis ke 90 derajat
  servo2.write(45);   // Siku pertama ke 45 derajat
  servo3.write(90);   // Siku kedua ke 90 derajat
  servo4.write(0);    // Gripper terbuka (0 derajat)
  delay(2000);        // Tunggu 2 detik

  // Posisi 2
  servo1.write(45);   // Basis ke 45 derajat
  servo2.write(90);   // Siku pertama ke 90 derajat
  servo3.write(135);  // Siku kedua ke 135 derajat
  servo4.write(90);   // Gripper tertutup (90 derajat)
  delay(2000);        // Tunggu 2 detik

  // Posisi 3
  servo1.write(135);  // Basis ke 135 derajat
  servo2.write(0);    // Siku pertama ke 0 derajat
  servo3.write(45);   // Siku kedua ke 45 derajat
  servo4.write(180);  // Gripper tertutup rapat (180 derajat)
  delay(2000);        // Tunggu 2 detik

  // Posisi 4
  servo1.write(180);  // Basis ke 180 derajat
  servo2.write(180);  // Siku pertama ke 180 derajat
  servo3.write(180);  // Siku kedua ke 180 derajat
  servo4.write(0);    // Gripper terbuka lagi
  delay(2000);        // Tunggu 2 detik
}
#include <Servo.h>  // Library untuk mengendalikan servo

// Membuat objek servo untuk masing-masing sambungan robot arm
Servo servo 1;  // Servo untuk basis
Servo servo 2;  // Servo untuk siku pertama
Servo servo 3;  // Servo untuk siku kedua
Servo servo 4;  // Servo untuk gripper

// Pin untuk servo
const int  servoPin;= 9;
const int  servoPin;= 10;
const int  servoPin;= 11;
const int  servoPin;= 12;

// Pin untuk potensiometer
const int potPin1 = A0;  // Potensiometer untuk kontrol servo 1
const int potPin2 = A1;  // Potensiometer untuk kontrol servo 2
const int potPin3 = A2;  // Potensiometer untuk kontrol servo 3
const int potPin4 = A3;  // Potensiometer untuk kontrol servo 4

void setup() {
  // Menghubungkan servo dengan pin yang sesuai
  servo1.attach(servoPin1);
  servo2.attach(servoPin2);
  servo3.attach(servoPin3);
  servo4.attach(servoPin4);

  // Inisialisasi posisi awal untuk servo
  servo1.write(90);  // Posisi dasar
  servo2.write(90);  // Posisi siku pertama
  servo3.write(90);  // Posisi siku kedua
  servo4.write(90);  // Posisi gripper (closed)
  delay(1000);  // Tunggu 1 detik untuk memastikan servo bergerak ke posisi awal
}

void loop() {
  // Membaca nilai dari potensiometer
  int val1 = analogRead(potPin1);  // Membaca potensiometer untuk servo1
  int val2 = analogRead(potPin2);  // Membaca potensiometer untuk servo2
  int val3 = analogRead(potPin3);  // Membaca potensiometer untuk servo3
  int val4 = analogRead(potPin4);  // Membaca potensiometer untuk servo4
  
  // Mengubah nilai potensiometer menjadi sudut antara 0 dan 180
  int angle1 = map(val1, 0, 1023, 0, 180);  // Memetakan nilai 0-1023 ke 0-180 derajat
  int angle2 = map(val2, 0, 1023, 0, 180);  // Memetakan nilai 0-1023 ke 0-180 derajat
  int angle3 = map(val3, 0, 1023, 0, 180);  // Memetakan nilai 0-1023 ke 0-180 derajat
  int angle4 = map(val4, 0, 1023, 0, 180);  // Memetakan nilai 0-1023 ke 0-180 derajat
  
  // Menulis sudut ke servo
  servo1.write(angle1);
  servo2.write(angle2);
  servo3.write(angle3);
  servo4.write(angle4);

  // Delay sedikit untuk memberikan waktu agar servo bergerak
  delay(15);  // Waktu singkat agar servo bergerak halus
}