const int trigPin = 6;
const int echoPin = 7;
const int motorPin1 = 2;
const int motorPin2 = 3;
const int motorPin3 = 4;
const int motorPin4 = 5;

const int objectD1 = 25;
const int objectH1 = 40;

const int objectD2 = 25;
const int objectH2 = 35;

const int objectD3 = 20;
const int objectH3 = 35;

const int objectD4 = 20;
const int objectH4 = 30;

const int tolerance = 2;

void setup() {
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(motorPin1, INPUT);
  pinMode(motorPin2, INPUT);
  pinMode(motorPin3, INPUT);
  pinMode(motorPin4, INPUT);
}

void loop() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH);

  int distance = duration * 0.034 / 2;

  int expectedDistance1 = objectH1 + objectD1 / 2;
  int expectedDistance2 = objectH2 + objectD2 / 2;
  int expectedDistance3 = objectH3 + objectD3 / 2;
  int expectedDistance4 = objectH4 + objectD4 / 2;

  if (distance > expectedDistance1 - tolerance && distance <= expectedDistance1 + tolerance) {
    //M1
    digitalWrite(motorPin1, HIGH);
    digitalWrite(motorPin2, LOW);
    digitalWrite(motorPin3, LOW);
    digitalWrite(motorPin4, LOW);
  } else if (distance > expectedDistance2 - tolerance && distance <= expectedDistance2 + tolerance) {
    //M2
    digitalWrite(motorPin1, LOW);
    digitalWrite(motorPin2, HIGH);
    digitalWrite(motorPin3, LOW);
    digitalWrite(motorPin4, LOW);
  } else if (distance > expectedDistance3 - tolerance && distance <= expectedDistance3 + tolerance) {
    //M3
    digitalWrite(motorPin1, LOW);
    digitalWrite(motorPin2, LOW);
    digitalWrite(motorPin3, HIGH);
    digitalWrite(motorPin4, LOW);
  } else if (distance > expectedDistance4 - tolerance && distance <= expectedDistance4 + tolerance) {
    //M4
    digitalWrite(motorPin1, LOW);
    digitalWrite(motorPin2, LOW);
    digitalWrite(motorPin3, LOW);
    digitalWrite(motorPin4, HIGH);
  } else {
    //NoMotor
    digitalWrite(motorPin1, LOW);
    digitalWrite(motorPin2, LOW);
    digitalWrite(motorPin3, LOW);
    digitalWrite(motorPin4, LOW);
  }

delay(100);
}