/*
 * This ESP32 code is created by esp32io.com
 *
 * This ESP32 code is released in the public domain
 *
 * For more detail (instruction and wiring diagram), visit https://esp32io.com/tutorials/esp32-ultrasonic-sensor
 */

#define TRIG_PIN 18 // ESP32 pin GIOP23 connected to Ultrasonic Sensor's TRIG pin
#define ECHO_PIN 5 // ESP32 pin GIOP22 connected to Ultrasonic Sensor's ECHO pin
#define LED 18
#define LED1 19
#define IN1 21
#define IN2 22
#define PWM 16 //RX2






#define K1 2
#define K2 4
#define K3 5
// setting PWM properties
const int freq = 5000;
const int ledChannel = 0;
const int resolution = 8;
 


int zavdania, poverh1=90, poverh2=60, poverh3=30, delta=10, dutyCycle = 128;


float duration_us, distance_cm;

void setup() {


  ledcSetup(ledChannel, freq, resolution);
  ledcAttachPin(PWM, ledChannel);
  // begin serial port
  Serial.begin (9600);
  pinMode(K1, INPUT);
  pinMode(K2, INPUT);
  pinMode(K3, INPUT);

  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);

 

  // configure the trigger pin to output mode
  pinMode(TRIG_PIN, OUTPUT);
  // configure the echo pin to input mode
  pinMode(ECHO_PIN, INPUT);
  pinMode(LED, OUTPUT);
  pinMode(LED1, OUTPUT);
}
void scan_button(){
  if(digitalRead(K1)) zavdania=poverh1;
  else if (digitalRead(K2))zavdania=poverh2;
  else if (digitalRead(K3))zavdania=poverh3;
  else return;
  }

void measure_distanse(){
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  duration_us = pulseIn(ECHO_PIN, HIGH);
  distance_cm = 0.017 * duration_us;
  // print the value to Serial Monitor
  Serial.print("distanceA: ");
  Serial.print(distance_cm);
  Serial.println(" cm      ");
}

void move_drive(){
  if(distance_cm>(zavdania-delta) && distance_cm<(zavdania+delta)){
  digitalWrite(IN1, LOW);
  digitalWrite(IN1, LOW);
    ledcWrite(ledChannel, 0);
  }//stop
  else if(distance_cm<(zavdania-delta)) {
  digitalWrite(IN1, HIGH);
  digitalWrite(IN1, LOW);
  ledcWrite(ledChannel, dutyCycle);
  }//up
  else if (distance_cm>(zavdania+delta)) {
  digitalWrite(IN1, LOW);
  digitalWrite(IN1, HIGH);
  ledcWrite(ledChannel, dutyCycle);
  }//down

  else {digitalWrite(IN1, LOW);
  digitalWrite(IN1, LOW);
    ledcWrite(ledChannel, 0);
    }//stop
}


void loop() {
  measure_distanse();
  zavdania = distance_cm;
  scan_button();
  move_drive();

  delay(50);
}