// ultrasonic sensor setup
#define trig A0  // example pin for trigger
#define echo A1  // example pin for echo

// 1st dc motor
#define in1 8
#define in2 7
#define enableA 9

// 2nd dc motor
#define in3 5
#define in4 4
#define enableB 3

// speed
#define speed 79

// pwm setup
void writepwm(int pin, int value) {
  analogWrite(pin, value);
}

void setup() {
  Serial.begin(9600);
  pinMode(trig, OUTPUT);
  pinMode(echo, INPUT);
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(enableA, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);
  pinMode(enableB, OUTPUT);
}

void moveforward() {
  digitalWrite(in1, HIGH);
  digitalWrite(in2, LOW);
  digitalWrite(in3, HIGH);
  digitalWrite(in4, LOW);
  writepwm(enableA, speed);
  writepwm(enableB, speed);
}

void stopmoving() {
  digitalWrite(in1, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in3, LOW);
  digitalWrite(in4, LOW);
  writepwm(enableA, 0);
  writepwm(enableB, 0);
}

void turnright() {
  digitalWrite(in1, LOW);
  digitalWrite(in2, HIGH);
  digitalWrite(in3, HIGH);
  digitalWrite(in4, LOW);
  writepwm(enableA, speed);
  writepwm(enableB, speed);
}

void turnleft() {
  digitalWrite(in1, HIGH);
  digitalWrite(in2, LOW);
  digitalWrite(in3, LOW);
  digitalWrite(in4, HIGH);
  writepwm(enableA, speed);
  writepwm(enableB, speed);
}

long getDistance() {
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  long duration = pulseIn(echo, HIGH);
  long distance = duration * 0.034 / 2; // cm
  return distance;
}

void loop() {
  long dist = getDistance();
  Serial.print("Distance: ");
  Serial.println(dist);

  if (dist <= 30) {
    moveforward();
  } else {
    stopmoving();
    delay(200);
    turnright();
    delay(500);
  }
  delay(100);
}