#include <Servo.h>

// Ultrasonic pins
#define TRIG_PIN 7
#define ECHO_PIN 8

// Servo objects
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;

// Servo pins
#define S1 9
#define S2 10
#define S3 11
#define S4 6

long duration;
float distance;

// Distance range (tune this!)
float minDist = 5;    // hand very close
float maxDist = 30;   // hand far

void setup() {
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  servo1.attach(S1);
  servo2.attach(S2);
  servo3.attach(S3);
  servo4.attach(S4);

  Serial.begin(9600);
}

float getDistance() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);

  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  duration = pulseIn(ECHO_PIN, HIGH);
  distance = duration * 0.034 / 2;

  return distance;
}

void loop() {
  float d = getDistance();
  Serial.println(d);

  // Constrain distance
  d = constrain(d, minDist, maxDist);

  // Map distance → angle (invert mapping)
  int angle = map(d, minDist, maxDist, 180, 0);

  // Apply to servos
  servo1.write(angle);
  servo2.write(angle);
  servo3.write(angle);
  servo4.write(angle);

  delay(50);
}