// программа управления створками шкафа (с) UA6EM

// https://wokwi.com/projects/333250377190212180

// VNH5019 - 
// https://www.pololu.com/file/0J740/dual-vnh5019-motor-driver-shield-schematic-diagram.pdf

// define DEBUG

// Подключаем библиотеки
#include <GyverTimer.h>
GTimer myTimerHC(MS, 70); // Таймер опроса УЗ датчиков
GTimer myTimerDC(MS, 10); // Таймер опроса работы актуаторов
GTimer myTimerAC(MS, 1);

// Определения
#define Echo01 5    
#define Trig01 6
#define Echo02 2
#define Trig02 3
#define DC1_in1 12    // VNH5019 M1INA
#define DC1_in2 11    // VNH5019 M1INB
#define DC2_in1 7     // VNH5019 M2INA
#define DC2_in2 8     // VNH5019 M2INB
#define DC1_pwm 9     // VNH5019 M1PWM
#define DC2_pwm 10    // VNH5019 M2PWM
#define M1CS A0       // VNH5019 M1CS (датчики тока)
#define M2CS A1       // VNH5019 M2CS
#define CW1_t 1000
#define CCW1_t 1000
#define CW2_t 1000
#define CCW2_t 1000
#define ON_t 3000
#define maxLevel 7
#define maxADC 100

uint8_t data_out[] = {Trig01,Trig02,DC1_in1,DC1_in2,DC2_in1,DC2_in2, DC1_pwm, DC2_pwm, LED_BUILTIN};
uint8_t data_in[] = {Echo01, Echo02};

//float maxLevel = 10.0; //расстояние от датчика в сантиметрах

volatile uint32_t dc1_millis;
volatile uint32_t dc2_millis;

volatile uint8_t dc1_mode = 0;
volatile uint8_t dc2_mode = 0;
volatile uint8_t dc1_pwm = 255;
volatile uint8_t dc2_pwm = 255;

volatile bool flag_mon = false;
volatile bool flag2_mon = false;


void setup()
{
  #ifdef DEBUG
  Serial.begin(115200); // используем серийный порт для отладки
  #endif

  for(auto p: data_out)pinMode(p, OUTPUT);
  for(auto p: data_in) pinMode(p, INPUT);
  myTimerHC.start();
  myTimerDC.start();
  myTimerAC.start();
}


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

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

void ServoIsOk() {
  if(dc1_mode == 0 && flag_mon){
  #ifdef DEBUG
  Serial.println("DC1");
  #endif
  digitalWrite(DC1_in1,HIGH);
  //delay(1);
  digitalWrite(DC1_in2, LOW);
    #ifdef DEBUG
  Serial.println("DC1 CW On");
  #endif
  dc1_pwm = 255;
  analogWrite(DC1_pwm, dc1_pwm);
  dc1_mode++;
  dc1_millis = millis();
  }
  if(dc1_mode == 1 && millis() - dc1_millis >= CW1_t){
  //delay(CW1_t);
  digitalWrite(DC1_in1,LOW);
    #ifdef DEBUG
  Serial.println("DC1 delay CW1_t ");
  #endif
  dc1_pwm = 0;
  analogWrite(DC1_pwm, dc1_pwm);

  dc1_mode++;
  dc1_millis = millis();
  }
  if(dc1_mode == 2 && millis()- dc1_millis >= ON_t){
  //delay(ON_t);
  digitalWrite(DC1_in2, HIGH);
    #ifdef DEBUG
   Serial.println("DC1 delay ON_t ");
   #endif
  dc1_pwm = 255;
  analogWrite(DC1_pwm, dc1_pwm); 
  dc1_mode++;
  dc1_millis = millis();
  }
  if(dc1_mode == 3 && millis()- dc1_millis >= CCW1_t){
  //delay(CCW1_t);
  digitalWrite(DC1_in2, LOW);
    #ifdef DEBUG
  Serial.println("DC1 delay CCW1_t ");
  #endif
  dc1_pwm = 0;
  analogWrite(DC1_pwm, dc1_pwm);
  //flag_mon = false;
  dc1_mode++;
  }
  if(dc1_mode == 4 && millis()- dc1_millis >= ON_t){
  flag_mon = false;
  dc1_mode = 0;
  }
}

void Servo2IsOk() {
  if(dc2_mode == 0 && flag2_mon){
      #ifdef DEBUG
  Serial.println("DC2");
  #endif
  digitalWrite(DC2_in1,HIGH);
  //delay(1);
  digitalWrite(DC2_in2, LOW);
    #ifdef DEBUG
  Serial.println("DC2 CW On");
  #endif
  dc2_pwm = 255;
  analogWrite(DC2_pwm, dc2_pwm); 
  dc2_mode++;
  dc2_millis = millis();
  }
  if(dc2_mode == 1 && millis() - dc2_millis >= CW2_t){
  //delay(CW1_t);
  digitalWrite(DC2_in1,LOW);
    #ifdef DEBUG
  Serial.println("DC2 delay CW2_t ");
  #endif
   dc2_pwm = 0;
  analogWrite(DC2_pwm, dc2_pwm); 
  dc2_mode++;
  dc2_millis = millis();
  }
  if(dc2_mode == 2 && millis()- dc2_millis >= ON_t){
  //delay(ON_t);
  digitalWrite(DC2_in2, HIGH);
    #ifdef DEBUG
   Serial.println("DC2 delay ON_t ");
   #endif
  dc2_pwm = 255;
  analogWrite(DC2_pwm, dc2_pwm); 
  dc2_mode++;
  dc2_millis = millis();
  }
  if(dc2_mode == 3 && millis()- dc2_millis >= CCW2_t){
  //delay(CCW1_t);
  digitalWrite(DC2_in2, LOW);
    #ifdef DEBUG
  Serial.println("DC2 delay CCW2_t ");
  #endif
  dc2_pwm = 0;
  analogWrite(DC2_pwm, dc2_pwm); 
  //flag2_mon = false;
  dc2_mode++;
  }
  if(dc2_mode == 4 && millis()- dc2_millis >= ON_t){ 
  flag2_mon = false;
  dc2_mode = 0;
    }
   }

void loop()
{
  uint16_t adc1;
  uint16_t adc2;
  uint16_t rast = 15;
  uint16_t rast2 = 15;

  if (myTimerAC.isReady()) {
      #ifdef DEBUG
    Serial.print("flag_mon = ");
    Serial.println(flag_mon);
    Serial.print("flag2_mon = ");
    Serial.println(flag2_mon);
    #endif
    
   if(flag_mon || flag2_mon){
     adc1 = analogRead(M1CS);
       #ifdef DEBUG
     Serial.print("ADC1 = ");
     Serial.println(adc1);
       #endif
     adc2 = analogRead(M2CS);
       #ifdef DEBUG
     Serial.print("ADC2 = ");
     Serial.println(adc2);
     #endif
  
     if(adc1 > maxADC){
       dc1_pwm = dc1_pwm - 25;
       analogWrite(DC1_pwm,dc1_pwm);
       digitalWrite(LED_BUILTIN,HIGH);
     }else{
      digitalWrite(LED_BUILTIN,LOW); 
     }

     if(adc2 > maxADC){
      dc2_pwm = dc2_pwm - 25;
      analogWrite(DC2_pwm,dc2_pwm);
      digitalWrite(LED_BUILTIN,HIGH);
     }  else {
      digitalWrite(LED_BUILTIN,LOW);  
     } 
   }else{
     digitalWrite(LED_BUILTIN,LOW);   
   }
 }

  if (myTimerHC.isReady()) {
  rast = readDistanceCM();
  rast2 = readDistance2CM();
    #ifdef DEBUG
  //Serial.println(rast);       // выводим растояние в монитор порта
  //Serial.println(rast2); 
  #endif
  }
  
  if (myTimerDC.isReady()) {
    if(flag_mon){
    ServoIsOk();
    }

    if(flag2_mon){
    Servo2IsOk();
    }
  }

  if (rast <= maxLevel && !flag_mon) {
    flag_mon = true;
    dc1_millis = millis();
    dc1_mode = 0;
    ServoIsOk();
  }

  if (rast2 <= maxLevel && !flag2_mon) {
    flag2_mon = true;
     dc2_millis = millis();
     dc2_mode = 0;
    Servo2IsOk(); 
  }

  //delay(100);
}