#include "HCSR04.h"

#define Trigg 6
#define Echo  5
#define EnA   10
#define EnB   2
#define In1   9
#define In2   8
#define In3   7
#define In4   4
#define threshold 200;//200 cm

UltraSonicDistanceSensor distanceSensor(Trigg, Echo);

void setup() {
  Serial.begin(115200);

  // MOTOR
  pinMode(EnA, OUTPUT);
  pinMode(EnB, OUTPUT);
  pinMode(In1, OUTPUT);
  pinMode(In2, OUTPUT);
  pinMode(In3, OUTPUT);
  pinMode(In4, OUTPUT);

  // ULTRASONIC SENSOR
  // pinMode(Trigg, INPUT);
  // pinMode(Echo, OUTPUT);
}

void loop() {
if(distanceSensor.measureDistanceCm() > threshold){
forward();
}
else(){
motorStop();
backward();
delay(2000);
Left(); 
delay(1000);
motorStop();
delay(100);
forward();
}
}

void motorControlLeft(bool clockwise) {
  // Motor control logic using L298N driver
  digitalWrite(In3, clockwise ? HIGH : LOW);
  digitalWrite(In4, clockwise ? LOW : HIGH);
  
  // Adjust the motor speed as needed by setting the PWM values on EnA and EnB
   analogWrite(EnB, 255); // Adjust the value (0 to 255) for speed control
}

void motorControlRight(bool clockwise) {
  // Motor control logic using L298N driver
  digitalWrite(In1, clockwise ? HIGH : LOW);
  digitalWrite(In2, clockwise ? LOW : HIGH);
   // Adjust the motor speed as needed by setting the PWM values on EnA and EnB
  analogWrite(EnA, 255); // Adjust the value (0 to 255) for speed control
 }

void motorStop() {
   // Adjust the motor speed as needed by setting the PWM values on EnA and EnB
  // Adjust the value (0 to 255) for speed control
   analogWrite(EnA, 0);
   analogWrite(EnB, 0);
 }
void forward(){
motorControlLeft(true);
motorControlRight(true);
}
void backward(){
motorControlLeft(false);
motorControlRight(false);
}

void Left(){
motorControlLeft(true);
motorControlRight(false);
}

void Right(){
motorControlLeft(false);
motorControlRight(true);
}