#include <DHT.h>
#include <Servo.h>
#define DHTPIN 13     
#define DHTTYPE DHT22  // DHT 22  (AM2302), AM2321

#define ECHO_PIN1 1
#define TRIG_PIN1 2

#define ECHO_PIN2 3
#define TRIG_PIN2 4

#define ECHO_PIN3 5
#define TRIG_PIN3 6

#define ECHO_PIN4 7
#define TRIG_PIN4 8

#define ECHO_PIN5 9
#define TRIG_PIN5 10

#define ECHO_PIN6 11
#define TRIG_PIN6 12

#define servoPin1 44
#define servoPin2 45
DHT dht(DHTPIN, DHTTYPE);
Servo Servo1;
Servo Servo2;
int pos1 = 90; 
int pos2 = 90; 
int TRIG_PIN = 0;
int ECHO_PIN = 0;
void setup() {
  Serial.begin(115200);
  Servo1.attach(servoPin1);
  Servo2.attach(servoPin2);
  pinMode(TRIG_PIN1, OUTPUT);
  pinMode(ECHO_PIN1, INPUT);
  pinMode(TRIG_PIN2, OUTPUT);
  pinMode(ECHO_PIN2, INPUT);
  pinMode(TRIG_PIN3, OUTPUT);
  pinMode(ECHO_PIN3, INPUT);
  pinMode(TRIG_PIN4, OUTPUT);
  pinMode(ECHO_PIN4, INPUT);
  pinMode(TRIG_PIN5, OUTPUT);
  pinMode(ECHO_PIN5, INPUT);
}
float readDistanceCM(int TRIG_PIN, int ECHO_PIN) {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  int duration = pulseIn(ECHO_PIN, HIGH);
  return duration * 0.034 / 2;
}
void loop() {
  Servo1.write(90);
  Servo2.write(90);
  delay(1000);
  float distance1 = readDistanceCM(TRIG_PIN1, ECHO_PIN1);
  float distance2 = readDistanceCM(TRIG_PIN2, ECHO_PIN2);
  float distance3 = readDistanceCM(TRIG_PIN3, ECHO_PIN3);
  
  float temperature = dht.readTemperature();
  float humidity = dht.readHumidity();
  
  if (distance1 <= 20){
    Serial.print("Terdapat sampah \n");
    delay(500);
    if (distance2 <= 5){
      Servo1.write(0);
      delay(1000);
      Serial.print("Sampah Logam \n");
    }
    else{
      Servo1.write(180);
      delay(1000);
      if (distance3 <= 5){
        Servo2.write(180);
        delay(500);
        Serial.print("sampah Organik \n");
      }
      else{
        Servo2.write(0);
        delay(500);
        Serial.print("Sampah Anorganik \n");
      }
    }
  }
  else{
      Serial.print("Tidak Terdeteksi Sampah \n");
      delay(500);
  }
  float distance4 = readDistanceCM(TRIG_PIN4, ECHO_PIN4);
  float distance5 = readDistanceCM(TRIG_PIN5, ECHO_PIN5);
  float distance6 = readDistanceCM(TRIG_PIN6, ECHO_PIN6);
  if (distance4 <= 10){
    Serial.print("Sampah Logam Penuh\n");
  }
  else{

  }
  if (distance5 <= 10){
    Serial.print("Sampah Organik Penuh\n");
  }
  else{
    
  }
  if (distance6 <= 10){
    Serial.print("Sampah Anorganik Penuh\n");
  }
  else{
    
  }
}