/*

Voltaat learn (https://www.voltaat.com)

The function of this code is to make the robot avoid obstacles in front of it. When an obstacle

is detected within 30 cm, the robot measures the distance to its right and left using an

ultrasonic sensor mounted on a servo motor. The robot then moves in the direction where

no obstacles are found.

Connections from the Arduino to the breadboard:

• Arduino 5v pin → breadboard 5v line

• Arduino GND pin → breadboard GND line

The connections from the L298N motor driver to the breadboard:

• 5V pin from the motor driver → breadboard 5v line

• GND pin from the motor driver → breadboard GND line

Connections from the servo motor:

• Servo GND pin → Arduino GND pin

• Servo VCC pin → Arduino VCC pin

• Servo signal pin → Arduino pin 12

Connections from the ultrasonic sensor:

• ultrasonic sensor VCC pin → Breadboard 5V line

• ultrasonic sensor GND pin → Breadboard ground line

• ultrasonic sensor Trig pin → Arduino pin 10

• ultrasonic sensor Echo pin → Arduino pin 11

Connections from the l298n motor driver :

• ENABLE A pin → Arduino pin 3

• Input1 pin → Arduino pin 4

• Input2 pin → Arduino pin 2

• Input3 pin → Arduino pin 1

• Input4 pin → Arduino pin 0

• ENABLE B pin → Arduino pin 5

The connections from the right motors:

• One terminal of the first right motor → One terminal of the second right motor → OUT1 port in the motor driver

• The other terminal of the first right motor → The other terminal of the second right motor → OUT2 port in the motor driver

The connections from the left motors:

• One terminal of the first left motor → One terminal of the second left motor → OUT3 port in the motor driver

• The other terminal of the first left motor → The other terminal of the second left motor → OUT4 port in the motor driver

The connections from the battery input to the motor driver:

• Positive terminal of the battery input → +12V port on the motor driver

• Ground terminal of the battery input → Ground (GND) port on the motor driver

*/

#include "Servo.h"         // Include library to control the servo motor

#include "NewPing.h"        // Include library to interface with the ultrasonic sensor

// Pin definitions

#define SERVO_PIN 12                // Pin connected to the servo motor

#define ULTRASONIC_SENSOR_TRIG 10    // Pin for the ultrasonic sensor trigger

#define ULTRASONIC_SENSOR_ECHO 11    // Pin for the ultrasonic sensor echo

#define MAX_REGULAR_MOTOR_SPEED 75   // Maximum speed for regular motor movement

#define MAX_MOTOR_ADJUST_SPEED 150   // Maximum speed when adjusting motor direction

#define DISTANCE_TO_CHECK 40         // Distance threshold (cm) to trigger object detection

// Right motor pin assignments

int enableRightMotor = 5;            // Pin to control the speed of the right motor

int rightMotorPin1 = 1;              // Pin 1 for controlling right motor direction

int rightMotorPin2 = 0;              // Pin 2 for controlling right motor direction

// Left motor pin assignments

int enableLeftMotor = 3;             // Pin to control the speed of the left motor

int leftMotorPin1 = 2;               // Pin 1 for controlling left motor direction

int leftMotorPin2 = 4;               // Pin 2 for controlling left motor direction

// Initialize ultrasonic sensor with trigger and echo pins, and a max distance of 400 cm

NewPing mySensor(ULTRASONIC_SENSOR_TRIG, ULTRASONIC_SENSOR_ECHO, 400);

Servo myServo;  // Create a servo object

void setup() {

  // Configure motor control pins as output

  pinMode(enableRightMotor, OUTPUT);

  pinMode(rightMotorPin1, OUTPUT);

  pinMode(rightMotorPin2, OUTPUT);

  pinMode(enableLeftMotor, OUTPUT);

  pinMode(leftMotorPin1, OUTPUT);

  pinMode(leftMotorPin2, OUTPUT);

  // Attach the servo to its designated pin and set its position to center (90 degrees)

  myServo.attach(SERVO_PIN);

  myServo.write(90);

  // Stop both motors initially

  rotateMotor(0, 0);  

}

void loop() {

  // Measure the distance to an object using the ultrasonic sensor (in cm)

  int distance = mySensor.ping_cm();

  // If an object is detected within 40 cm, adjust motor movement

  if (distance > 0 && distance < DISTANCE_TO_CHECK) {

    // Stop motors to avoid collision

    rotateMotor(0, 0);

    delay(500);  

    // Reverse motors to back away from the object

    rotateMotor(-MAX_MOTOR_ADJUST_SPEED, -MAX_MOTOR_ADJUST_SPEED);        

    delay(200);

    // Stop motors again after reversing

    rotateMotor(0, 0);

    delay(500);

    // Rotate servo to the left (180 degrees) to check distance on the left side

    myServo.write(180);

    delay(500);

    int distanceLeft = mySensor.ping_cm();    // Read distance from left side

    // Rotate servo to the right (0 degrees) to check distance on the right side

    myServo.write(0);    

    delay(500);    

    int distanceRight = mySensor.ping_cm();   // Read distance from right side

    // Return servo to center (90 degrees)

    myServo.write(90);

    delay(500);        

    // Adjust motor direction based on left and right side distances

    if (distanceLeft == 0) {

      // If no distance is measured on the left, rotate right

      rotateMotor(MAX_MOTOR_ADJUST_SPEED, -MAX_MOTOR_ADJUST_SPEED);

      delay(200);

    }

    else if (distanceRight == 0) {

      // If no distance is measured on the right, rotate left

      rotateMotor(-MAX_MOTOR_ADJUST_SPEED, MAX_MOTOR_ADJUST_SPEED);

      delay(200);

    }

    else if (distanceLeft >= distanceRight) {

      // If left side has more space or is equal, rotate right

      rotateMotor(MAX_MOTOR_ADJUST_SPEED, -MAX_MOTOR_ADJUST_SPEED);

      delay(200);

    }

    else {

      // If right side has more space, rotate left

      rotateMotor(-MAX_MOTOR_ADJUST_SPEED, MAX_MOTOR_ADJUST_SPEED);

      delay(200);      

    }

    // Stop motors after adjustment

    rotateMotor(0, 0);    

    delay(200);    

  }

  else {

    // If no object is detected within the threshold, continue moving forward

    rotateMotor(MAX_REGULAR_MOTOR_SPEED, MAX_REGULAR_MOTOR_SPEED);

  }

}

// Function to rotate motors at specified speeds

// Positive speed values make the motor move forward, negative values make it move backward

void rotateMotor(int rightMotorSpeed, int leftMotorSpeed) {

  // Control the right motor direction based on the speed sign

  if (rightMotorSpeed < 0) {

    digitalWrite(rightMotorPin1, LOW);

    digitalWrite(rightMotorPin2, HIGH);    

  }

  else if (rightMotorSpeed >= 0) {

    digitalWrite(rightMotorPin1, HIGH);

    digitalWrite(rightMotorPin2, LOW);      

  }

  // Control the left motor direction based on the speed sign

  if (leftMotorSpeed < 0) {

    digitalWrite(leftMotorPin1, LOW);

    digitalWrite(leftMotorPin2, HIGH);    

  }

  else if (leftMotorSpeed >= 0) {

    digitalWrite(leftMotorPin1, HIGH);

    digitalWrite(leftMotorPin2, LOW);      

  }

  // Set the speed of the motors using PWM (Pulse Width Modulation)

  analogWrite(enableRightMotor, abs(rightMotorSpeed));

  analogWrite(enableLeftMotor, abs(leftMotorSpeed));    

}

‍

‍