Robotics & Mechatronics Lab

#include <Servo.h>

Servo steeringServo;
const int trigPin = 9;
const int echoPin = 10;
const int motorLeftPin = 5;   // PWM
const int motorRightPin = 6;  // PWM

void setup() {
  steeringServo.attach(3);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  Serial.begin(115200);
}

void loop() {
  long duration, distance;
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  
  duration = pulseIn(echoPin, HIGH);
  distance = (duration * 0.0343) / 2; // Current: 42 cm

  if (distance < 20) {
    // Collision imminent: Turn sharp right
    steeringServo.write(45);
    analogWrite(motorLeftPin, 200);
    analogWrite(motorRightPin, 50);
  } else {
    steeringServo.write(90);
    analogWrite(motorLeftPin, 180);
    analogWrite(motorRightPin, 180);
  }
  delay(50);
}