#include <Arduino.h>
#include <Servo.h>

// Pin konfigurasi
#define TRIG_PIN PA0
#define ECHO1_PIN PA1
#define ECHO2_PIN PA2
#define ECHO3_PIN PA3
#define ECHO4_PIN PA4
#define ECHO5_PIN PA5
#define ECHO6_PIN PC13

#define SOIL1_PIN PA7
#define SOIL2_PIN PB0
#define SOIL3_PIN PB1
#define SOIL4_PIN PA8
#define SOIL5_PIN PA9
#define SOIL6_PIN PA10

#define SERVO1_PIN PA6
#define SERVO2_PIN PB7
#define SERVO3_PIN PB8
#define SERVO4_PIN PB9
#define SERVO5_PIN PA11
#define SERVO6_PIN PA15

int threshold_moisture = 500;  
int threshold_level = 20;     

Servo servos[6];
Servo servo1;
int soilPins[6] = {SOIL1_PIN, SOIL2_PIN, SOIL3_PIN, SOIL4_PIN, SOIL5_PIN, SOIL6_PIN};
int echoPins[6] = {ECHO1_PIN, ECHO2_PIN, ECHO3_PIN, ECHO4_PIN, ECHO5_PIN, ECHO6_PIN};
int servoPins[6] = {SERVO1_PIN, SERVO2_PIN, SERVO3_PIN, SERVO4_PIN, SERVO5_PIN, SERVO6_PIN};

void setup() {
  pinMode(SERVO1_PIN, OUTPUT);
}

void loop() {
  // Gerakkan ke 90 derajat (pulse ~1500 µs)
  for (int i = 0; i < 50; i++) {  // 50 kali = ~1 detik (50 x 20ms)
    digitalWrite(SERVO1_PIN, HIGH);
    delayMicroseconds(1500); // 1.5 ms untuk 90°
    digitalWrite(SERVO1_PIN, LOW);
    delayMicroseconds(18500); // sisa waktu 20ms - 1.5ms
  }

  delay(1000); // Total 2 detik jeda (1 detik sinyal + 1 detik delay)

  // Gerakkan ke 0 derajat (pulse ~500 µs)
  for (int i = 0; i < 50; i++) {
    digitalWrite(SERVO1_PIN, HIGH);
    delayMicroseconds(500); // 0.5 ms untuk 0°
    digitalWrite(SERVO1_PIN, LOW);
    delayMicroseconds(19500); // sisa waktu
  }

  delay(1000);
}