#include <Servo.h>

// Inisialisasi objek untuk setiap motor servo
Servo servoMotor1;  // Base
Servo servoMotor2;  // Shoulder
Servo servoMotor3;  // Elbow
Servo servoMotor4;  // Gripper

// Servo untuk kotak sampah
Servo servoKotakSampah;

// Penentuan pin untuk setiap motor servo
const int pinMotor1 = 2;  // Pin untuk base
const int pinMotor2 = 3;  // Pin untuk shoulder
const int pinMotor3 = 4;  // Pin untuk elbow
const int pinMotor4 = 5;  // Pin untuk gripper

// Sensor ultrasonik
const int trigPin = 6;
const int echoPin = 7;

// Sensor proximity (SN04-N)
const int proximityPin = 8;

void setup() {
  // Attach setiap motor servo ke pin yang sesuai
  servoMotor1.attach(pinMotor1);  // Base
  servoMotor2.attach(pinMotor2);  // Shoulder
  servoMotor3.attach(pinMotor3);  // Elbow
  servoMotor4.attach(pinMotor4);  // Gripper
  
  // Servo kotak sampah
  servoKotakSampah.attach(9); // Misalnya, servo kotak sampah terhubung ke pin 9
  
  // Set pin trigPin sebagai OUTPUT
  pinMode(trigPin, OUTPUT);
  // Set pin echoPin sebagai INPUT
  pinMode(echoPin, INPUT);
  
  // Set pin proximityPin sebagai INPUT
  pinMode(proximityPin, INPUT);
  
  Serial.begin(9600); // Inisialisasi komunikasi serial
  Serial.println("Robot aktif. Menunggu objek...");
}

void loop() {
  // Membaca jarak dari sensor ultrasonik
  long duration, distance;
  digitalWrite(trigPin, LOW);  
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance = (duration / 2) / 29.1;  // Konversi waktu ke jarak dalam sentimeter
  
  // Output nilai jarak ke Serial Monitor
  Serial.print("Jarak: ");
  Serial.print(distance);
  Serial.println(" cm");
  
  // Cek apakah objek berada dalam jarak tertentu
  if (distance < 10) { // Misalnya, objek berada dalam jarak 10 cm
    // Gerakan lengan robot untuk mengambil objek
    
    // Gerakan base lengan robot untuk mengarahkan objek ke kotak sampah
    // Misalnya, dengan memutar base sesuai dengan posisi objek yang terdeteksi
    
    // Lakukan perhitungan sudut untuk menggerakkan base
    // Anggap bahwa servo pada base dapat berputar 180 derajat
    int baseAngle = map(distance, 0, 10, 0, 180); // Melakukan pemetaan jarak menjadi sudut
    
    // Kendalikan servo pada base untuk menggerakkan objek ke arah kotak sampah
    servoMotor1.write(baseAngle);
    delay(1000); // Tunggu 1 detik
    
    // Gerakan lengan robot untuk mengambil objek
    // Anda dapat menambahkan logika gerakan yang sesuai untuk mengambil objek
    
    // Gerakan untuk memasukkan objek ke dalam kotak sampah
    // Misalnya, dengan menutup gripper dan menggerakkan lengan ke belakang
    
    // Motor 4 (gripper)
    servoMotor4.write(0); // Tutup gripper
    delay(1000); // Tunggu 1 detik
    // Motor 3 (elbow)
    servoMotor3.write(180); // Gerakkan ke belakang
    delay(1000); // Tunggu 1 detik
  
    // Membaca status objek dari sensor proximity (SN04-N)
    int proximityValue = digitalRead(proximityPin);
    if (proximityValue == HIGH) { // Objek logam terdeteksi
      // Gerakan motor servo pada kotak sampah ke kanan
      servoKotakSampah.write(180); // Contoh: gerakan ke kanan
      
    } else { // Objek non-logam terdeteksi
      // Gerakan motor servo pada kotak sampah ke kiri
      servoKotakSampah.write(0); // Contoh: gerakan ke kiri
      
    }
  
    // Tambahkan gerakan lain sesuai kebutuhan
  }
  else {
    // Kembali ke posisi awal
    servoMotor1.write(0); // Base kembali ke posisi awal
    servoMotor2.write(0); // Shoulder kembali ke posisi awal
    servoMotor3.write(0); // Elbow kembali ke posisi awal
    servoMotor4.write(0); // Gripper kembali ke posisi awal
  }
  
  // Tambahkan logika lain atau gerakan default jika tidak ada objek dalam jarak tertentu
}