// программа управления створками шкафа (с) UA6EM
// https://wokwi.com/projects/332173707919229522

#include <Servo.h>
Servo servo;
Servo servo2;

#define Echo01_pin 2
#define Trig01_pin 3
#define Echo02_pin 5
#define Trig02_pin 6

#define Servo01_Pin 8
#define Servo02_Pin 9

float maxLevel = 10.00; //расстояние в сантиметрах
uint32_t old_millis;
int pos = 0;
bool flag_mon = false;
bool flag2_mon = false;


void setup()
{
  Serial.begin(115200); // используем серийный порт для отладки
  pinMode(LED_BUILTIN, OUTPUT);

  pinMode(Trig01_pin, OUTPUT);
  pinMode(Echo01_pin, INPUT);
  servo.attach(Servo01_Pin);

  pinMode(Trig02_pin, OUTPUT);
  pinMode(Echo02_pin, INPUT);
  servo2.attach(Servo02_Pin);

  servo.write(180);
  servo2.write(180);
  old_millis = millis();
  while(millis() - old_millis < 3000){
    //зависли на 3 секунды
  }
   servo.write(90);
   servo2.write(90);
}


float readDistanceCM() {
  digitalWrite(Trig01_pin, LOW);
  delayMicroseconds(2);
  digitalWrite(Trig01_pin, HIGH);
  delayMicroseconds(10);
  digitalWrite(Trig01_pin, LOW);
  int duration = pulseIn(Echo01_pin, HIGH);
  return duration * 0.034 / 2;
}

float readDistance2CM() {
  digitalWrite(Trig02_pin, LOW);
  delayMicroseconds(2);
  digitalWrite(Trig02_pin, HIGH);
  delayMicroseconds(10);
  digitalWrite(Trig02_pin, LOW);
  int duration = pulseIn(Echo02_pin, HIGH);
  return duration * 0.034 / 2;
}

void ServoIsOk() {
  servo.write(180);
  delay(1000);
  servo.write(90);
}

void Servo2IsOk() {
  servo2.write(180);
  delay(1000);
  servo2.write(90);
}

void loop()
{
  uint16_t rast = readDistanceCM();
  uint16_t rast2 = readDistance2CM();
  Serial.println(rast);       // выводим растояние до воды в монитор порта
  Serial.println(rast2); 

  if (rast <= 4 && !flag_mon) {
    ServoIsOk();
    flag_mon = true;
    old_millis = millis();
  }
  if (rast2 <= 4 && !flag2_mon) {
    Servo2IsOk();
    flag2_mon = true;
    old_millis = millis();
  }

  if (millis() - old_millis >= 1000) {
    flag_mon = false;
    flag2_mon = false;
    old_millis = millis();
  }
  delay(100);
}