Obstacle Avoiding Car Using Arduino

Published  October 30, 2023   0
Arduino Obstacle Avoiding Car

Avoiding obstacles is a key job in robotics, where autonomous robots aim to reach their destination without bumping into things. One type of autonomous robot that's pretty clever is the real-time Arduino obstacle avoiding car. It can spot obstacles and figure out new paths to move without running into them. Previously, we have build many robots using arduino including an Obstacle Avoiding Robot using Arduino Uno and Ultrasonic Sensor, you can also check them out if you are interested.

In this comprehensive guide, I will explain the process of constructing your very own obstacle avoiding car using Arduino. This innovative vehicle employs a servo mounted ultrasonic sensor to detect objects situated both in front and on either side of the car. Additionally, it employs an L298N DC motor driver, which serves as the driving force for four geared motors. you also need a Arduino obstacle avoiding car kit for this project.

Components Needed for Obstacle Avoiding Car Project

  • Arduino Uno 
  • MG 90 metal gear Servo motors (1x)
  • HC-SR04 Ultrasonic Sensor
  • RGB Led(3x)
  • L298N motor driver
  • 2WD Arduino car kit Laser cut Chassis 
  • 12V Li-ion battery
  • Lm2596 DC-DC buck convertor
  •  Switch
  • Jumper wires
  • Breadboard or PCB (Printed Circuit Board)
  • Screws, nuts, spacers

Arduino Obstacle Avoiding Car Circuit Diagram 

The complete circuit diagram for this 2wd obstacle avoiding robot is given below. As you can see, it uses an Ultrasonic sensor to take measurements in all three directions with the help of a servo motor. Based on the distance measurements, the motor takes actions to move forward or turn left and right.

Arduino Obstacle Avoiding Car Circuit Diagram

The sensor plays a crucial role in this project. It sends out 40kHz ultrasonic signals that bounce back when they hit an object. By analyzing the returning echoes, the sensor calculates distances between the car and obstacles in real-time. When an obstacle is detected, the car autonomously changes direction. Once our arduino car for obstacle avoidance is built it should look like something as shown in image below. 

circuit connection of Obstacle Avoiding Car Using Arduino

We have used an off the shelf Robotic Car chassis kit to build the body of our robot. There many versions of Arduino robot kit available in the market you can purchase the now of your choice. But, we personally selected this kit because it looks more like a car with a front bumper, spoiler etc. Previously we have also used the same kit to build other projects like Human Following Robot and Pick and Place Arduino Robot Car, you can also check them out if you are interested. 

Arduino Obstacle Avoiding Car Wiring Connection

Build the arduino based obstacle avoiding robot car is pretty simple, to make things easy to understand, in the following paragraphs we have explained how each component is connected to arduino nano. You can also follow the circuit diagram for better understanding. 

Servo Motor Connection with Arduino

To connect the servo motor to the Arduino, simply attach the signal wire to any PWM control pin, connect the power wire to the 5V output from a DC-DC converter, and link the ground wire to one of the Arduino's ground pins.

Connecting Arduino Car to Ultrasonic Sensor

Connect the sensor to the Arduino like this: VCC to 5V, GND to GND, TRIG to a digital pin 2, and ECHO to another digital pin 4 of. This setup allows the Arduino to interact with the sensor, further we will also be writing an arduino car ultrasonic sensor code to enable our robot to read the distance of object ahead of it and control the wheels accordingly. More details on that can be found in the code section, 

RGB LEDs Connection with Arduino

Connect all the RGB LEDs' ground (cathode) to a ground rail on the breadboard. Then, link each RGB LED's red terminal to an analog pin on the Arduino(A1,A2&A3) and the green terminal to a digital pin on the Arduino(5,6&13).This configuration allows the Arduino to control the RGB LEDs.

L298N Motor Driver Connection with Arduino

Arduino's GND to breadboard GND rail, Arduino's Vin to breadboard positive rail, LM2596 5V out to positive rail, LM2596 GND out to GND rail. For DC motor 1  L298N IN1 and IN2 to Arduino pins 7 and 8. For DC motor 2  L298N IN3 and IN4 to Arduino pins 12 and 11. Enable PWM motor control by connecting L298N ENA to Arduino PWM pin 9 and ENB to PWM pin 10, enabling independent control of two DC motors using Arduino.

Power Connections 

Connect the 12V Li-ion battery's positive terminal to the LM2596's Vin input and link the battery's negative terminal to the breadboard's ground rail. Also, connect the battery's positive terminal to the L298N motor driver's 12V input and connect the battery's negative terminal to the L298N's GND.

Important: Always turn off the Arduino power switch and remove external Vin supply when you upload the code.

power connection of Obstacle Avoiding Car Using Arduino

Secure all components to the car frame using screws, nuts, and spacers as needed. Ensure that all connections are secure and free from loose wires or potential short circuits. Always double-check your connections before powering on the circuit.

Additionally, I incorporated several enhancements to the car's design. These include the addition of a power on/off switch, a 12V DC charging socket, and a Li-ion battery level indicator located on the back side of the vehicle. Furthermore, I included a slider switch near to the DC socket, which serves the purpose of monitoring the battery level when the primary power switch is  off. When it comes to charging the battery, you simply slide the switch upwards. Conversely, if you wish to check the charging status of the battery, you can easily slide the switch back down. Do note that, when switch is slide down it will not charge the battery.

power connection of Obstacle Avoiding Car Using Arduino

Arduino Obstacle Avoiding Car Code

Now that the obstacle avoiding car connection is complete, let’s have a look at the Arduino obstacle avoiding car code. The complete arduino code for obstacle avoiding car is given at the end of this page. The program will include setting up HC-SR04 module and outputting the signals to Motor Pins to move motor direction accordingly.

You can make some personal changes in the code like the threshold distance as per your requirement and motor speed control. Install the required libraries and upload the code given above.

#include <Servo.h>

This block includes the Servo library, which is necessary for controlling the servo motor.

// Ultrasonic sensor pins
#define trig 2
#define echo 4

// RGB led pins 
#define LR 9        // Led Right
#define LC 13        // Led Center 
#define LL 10       // Led Left
#define LRR A3     // Led Right Red      
#define LCR A2     // Led Center Red  
#define LLR A1     // Led Left Red

In this block, various pin constants are defined for components such as the Ultrasonic sensor and RGB LEDs. These constants are used to specify which pins on the Arduino are connected to each component.

// Motor control pins
#define LEFT_MOTOR_PIN1 8
#define LEFT_MOTOR_PIN2 7
#define RIGHT_MOTOR_PIN1 12
#define RIGHT_MOTOR_PIN2 11
#define ENA 6 // Enable A pin for motor speed control
#define ENB 3 // Enable B pin for motor speed control

These lines define digital pins of Arduino (pin7,pin8,pin12 and pin11) for the L298N motor driver inputs to control the DC motors. The pins in1 and in2 control one motor, and the pins in3 and in4 control the other motor. Ena and Enb control the speed of the motor.

// Distance thresholds for obstacle detection
#define MIN_DISTANCE_BACK 12

This block defines threshold values. MIN_DISTANCE_BACK represents the minimum distance required from an obstacle to stop.

void setup() {
  // Set motor control pins as outputs
  pinMode(LEFT_MOTOR_PIN1, OUTPUT);
  pinMode(LEFT_MOTOR_PIN2, OUTPUT);
  pinMode(RIGHT_MOTOR_PIN1, OUTPUT);
  pinMode(RIGHT_MOTOR_PIN2, OUTPUT);
  pinMode(ENA, OUTPUT);
  pinMode(ENB, OUTPUT);
  pinMode(LR, OUTPUT);
  pinMode(LC, OUTPUT);
  pinMode(LL, OUTPUT);
  pinMode(LRR, OUTPUT);
  pinMode(LCR, OUTPUT);
  pinMode(LLR, OUTPUT);
  digitalWrite(LR, LOW);
  digitalWrite(LC, LOW);
  digitalWrite(LL, LOW);
  analogWrite(LRR, 0);
  analogWrite(LCR, 0);
  analogWrite(LLR, 0);
  servoLook.attach(5);
  //Set the Trig pins as output pins
  pinMode(trig, OUTPUT);  
  //Set the Echo pins as input pins
  pinMode(echo, INPUT);
  analogWrite(ENA, 53.5); //  speed for motor A  0-LOW speed, 255-Full speed
  analogWrite(ENB, 45); //  speed for motor B
  // Initialize the serial communication for debugging
  Serial.begin(9600);
}

In the setup() function, various initializations take place. This includes configuring motor control pins, LED pins, and sensor pins as either OUTPUT or INPUT. It also sets the initial states for LEDs and RGB LEDs to LOW or 0. Additionally, the Servo motor is attached to pin 5, motor speed is adjusted using analogWrite() for ENA and ENB, and serial communication is initiated for debugging (though this part is optional).

void loop()
{
  servoLook.write(90);
  delay(750);
  int distance = getDistance();
  if (distance >= MIN_DISTANCE_BACK)
{ 
    moveForward(); 
    Serial.println("forward");  } 
   while(distance >= MIN_DISTANCE_BACK)   
{
    distance = getDistance();
    delay(50); 
 }
   Stop(); 
   int turnDir = checkDirection();
   Serial.println(turnDir);
   switch(turnDir)
   {
    case 0:                            //Turn left
    Serial.println("Left");
      turnLeft();
      delay(425);
      Stop();
      break;
    case 2:                            //Turn right
    Serial.println("Right");
      turnRight();
      delay(415);
      Stop();
      break;   
    case 3:                             //move forward
    Serial.println("Forward");
     moveForward();
      break; 
 }
}

This is the main loop of the code, executed continuously. The servo motor is set to look straight ahead, and there's a delay for it to settle. The getDistance() function is used to measure the distance using the Ultrasonic sensor. If the measured distance is greater than or equal to MIN_DISTANCE_BACK, the robot is instructed to move forward. While the distance is above the threshold, it continues moving forward. When the distance becomes less than MIN_DISTANCE_BACK, the Stop() function is called to stop the robot.The checkDirection() function is used to determine if there's an obstacle in the left, right, or front direction.Based on the detected direction, the robot will turn left, turn right, or move forward.Serial messages are printed to help with debugging.

int getDistance()                   //Measure the distance to an object{
  unsigned long pulseTime;    //Create a variable to store the pulse travel time
  int distance;              //Create a variable to store the calculated distance
  digitalWrite(trig, HIGH);                 //Generate a 10 microsecond pulse
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  pulseTime = pulseIn(echo, HIGH);    //Measure the time for the pulse to return
  distance  = pulseTime / 29 / 2;   //Calculate the object distance based on the                                                   
                                                   pulse time
  Serial.println(distance);
  return distance;

This function measures the distance to an object using the Ultrasonic sensor. It generates a trigger pulse and measures the time for the pulse to return. Calculate and returns the distance based on the measured time.

int checkDirection()  //Check the left,Front and right directions and decide which way to turn


  int distances [3] = {0,0,0};       //Left ,right and Front distances
  int turnDir;        //Direction to turn, 0 left, 1 turn around, 2 right, 3 forward
  servoLook.write(180);                         //Turn servo to look left
  delay(500);
  distances [0] = getDistance();            //Get the left object distance
  servoLook.write(90);                         //Turn servo to look front
  delay(500);
  distances [3] = getDistance();            //Get the front object distance
  servoLook.write(0);                        //Turn servo to look right
  delay(1000);
  distances [1] = getDistance();             //Get the right object distance
  if (distances[0]<=25 && distances[1]<=25)   //If both directions are blocked, move forward
    turnDir = 3;                                             
  else if (distances[0]>=distances[1])   //If left has more space, turn left
    turnDir = 0;  //left
  else if (distances[0]<distances[1])   //If right has more space, turn right
    turnDir = 2;  //right
   //else if (distances[0]<=50 && distances[1]<=50 && distances[3]<=50)    //If All directions are blocked, turn around
    //turnDir = 1;  
  return turnDir;
}

This section of the code handles the obstacle-avoidance strategy for the car's movement. It continually monitors obstacle distances in three directions left, right, and front by utilizing a servo motor. After each scan, it calculates the distances to nearby obstacles. If both the left and right directions have close obstacles, the car proceeds forward. If there's more space on the left side, it steers the car left, and if the right side is more open, it directs the car to the right. If all three directions (front, left, and right) have obstructions nearby, the car is instructed to perform a U-turn. These directions are communicated by returning specific values: 0 for left, 2 for right, 3 for forward, and 1 for turning around.

// Motor control functions
void moveForward() { 
  analogWrite(ENA, 60.5); //  speed for motor A
  analogWrite(ENB, 52); //   speed for motor B
  digitalWrite(LR, HIGH);
  digitalWrite(LC, HIGH);      // All Green light turn on
  digitalWrite(LL, HIGH);
  analogWrite(LRR, 0);
  analogWrite(LCR, 0);       // All Red light turn off
  analogWrite(LLR, 0); 
  digitalWrite(LEFT_MOTOR_PIN1, LOW);
  digitalWrite(LEFT_MOTOR_PIN2, HIGH);
  digitalWrite(RIGHT_MOTOR_PIN1, LOW);
  digitalWrite(RIGHT_MOTOR_PIN2, HIGH);
}

void moveBackward() {
  digitalWrite(LR, LOW);
  digitalWrite(LC, LOW);        // All Green light turn off
  digitalWrite(LL, LOW);
  analogWrite(LRR, 255);
  analogWrite(LCR, 255);       // All Red light turn on
  analogWrite(LLR, 255); 
  digitalWrite(LEFT_MOTOR_PIN1, HIGH);
  digitalWrite(LEFT_MOTOR_PIN2, LOW);
  digitalWrite(RIGHT_MOTOR_PIN1, HIGH);
  digitalWrite(RIGHT_MOTOR_PIN2, LOW);
}

void turnRight() {
  analogWrite(ENA, 100); //  speed for motor A
  analogWrite(ENB, 90); //  speed for motor B  
  digitalWrite(LR, HIGH);
  digitalWrite(LC, LOW);            // right side Green light turn on
  digitalWrite(LL, LOW);
  analogWrite(LRR, 0);
  analogWrite(LCR, 255);         // Left and Middle Red light turn on
  analogWrite(LLR, 255);  
  digitalWrite(LEFT_MOTOR_PIN1, HIGH);
  digitalWrite(LEFT_MOTOR_PIN2, LOW);
  digitalWrite(RIGHT_MOTOR_PIN1, LOW);
  digitalWrite(RIGHT_MOTOR_PIN2, HIGH);
}

void turnLeft() {
  analogWrite(ENA, 100); //  speed for motor A
  analogWrite(ENB, 90); //  speed for motor B
  digitalWrite(LR, LOW);
  digitalWrite(LC, LOW);         // left side Green light turn on
  digitalWrite(LL, HIGH);
  analogWrite(LRR, 255);
  analogWrite(LCR, 255);         // right and Middle Red light turn on
  analogWrite(LLR, 0);
  digitalWrite(LEFT_MOTOR_PIN1, LOW);
  digitalWrite(LEFT_MOTOR_PIN2, HIGH);
  digitalWrite(RIGHT_MOTOR_PIN1, HIGH);
  digitalWrite(RIGHT_MOTOR_PIN2, LOW);
}

void Stop() {
analogWrite(ENA, 0); //  speed for motor A
  analogWrite(ENB, 0); //  speed for motor B
  digitalWrite(LR, LOW);
  digitalWrite(LC, LOW);
  digitalWrite(LL, LOW);
  analogWrite(LRR, 255);
  analogWrite(LCR, 255);
  analogWrite(LLR, 255);
  analogWrite(ENA, 0); //  speed for motor A
  analogWrite(ENB, 0); //  speed for motor B
  digitalWrite(LEFT_MOTOR_PIN1, LOW);
  digitalWrite(LEFT_MOTOR_PIN2, LOW);
  digitalWrite(RIGHT_MOTOR_PIN1, LOW);
  digitalWrite(RIGHT_MOTOR_PIN2, LOW);
}

These blocks are used in the main Arduino obstacle avoiding car code 2WD to control the car's movement and direction based on obstacle detection and the desired action.

moveForward() This function makes the car move forward. It sets the motor speeds for both sides to move forward, turns on green lights (indicating forward movement), and turns off red lights (indicating stopping or backward movement).

moveBackward() This function makes the car move backward. It turns off the green lights and turns on the red lights to indicate reverse motion. It also sets the motor speeds for backward movement.

turnRight() This function makes the car turn right. It adjusts the motor speeds to execute a right turn, turns on the right-side green light, and turns on the left and middle red lights to indicate the right turn.

turnLeft() This function makes the car turn left. It adjusts the motor speeds to execute a left turn, turns on the left-side green light, and turns on the right and middle red lights to indicate the left turn.

Stop() This function brings the car to a complete stop. It turns off all lights (indicating a stop), sets both motor speeds to zero, and stops all motor movement.

Working of Obstacle Avoiding Car

The central component that makes the Arduino obstacle avoiding car work is the HC-SR04 Ultrasonic sensor. This sensor relies on sound waves to figure out how far away objects are in front of it. It's a handy sensor used in many projects that need to quickly and affordably measure distances. It's small and can be put anywhere on the car's body.

There are six main parts to this project an Arduino obstacle avoiding 2WD kit an Arduino Uno, the Ultrasonic Sensor, DC motors, a Servo motor, and an L293D motor driver. When you connect power to the Arduino, the Servo motor starts turning and checking the distance in three directions left, right, and front. Whichever direction has the most space, the car moves in that direction.

As soon as the car gets close to an obstacle, there's a set distance programmed into the Arduino. If the obstacle is closer than this distance, the ultrasonic sensor tells the Arduino. Once the Arduino gets this signal, it tells the motor driver to stop the motors from turning. Then, the servo motor starts checking distances again in all three directions. Based on what it finds, it signals the motor driver to turn the car either left or right, and the car turns accordingly. After that, the car goes forward again until it encounters another obstacle. This whole process keeps happening until you turn off the power.

components of Obstacle Avoiding Car Using Arduino

Additionally, I've added three RGB LEDs on the underside of the car to indicate its movement. When the car stops, all the LEDs turn red. When it moves forward, they all turn green. If the car takes a right turn, the right-side LED glows green while the other two stay red, and the same goes for a left turn.

Obstacle Avoiding Car Using Arduino

In this project, we successfully designed and built an obstacle avoiding car using Arduino project that can navigate its environment autonomously and Solve the Maze. The car utilizes an HC-SR04 ultrasonic sensor for distance measurement, a servo motor for scanning the surroundings, and an L298N motor driver for controlling four geared motors.

In summary, this obstacle avoiding car demonstrates the fundamental principles of autonomous robotics, making it an excellent educational tool and a fun project for those interested in robotics and electronics. Building upon this foundation, there are many advantages of obstacle avoiding car that can be expanded and adapted for more complex tasks like food delivery, warehouse management, telemetric robots etc, highlighting the exciting possibilities of robotics in various applications. If you want to build more such exciting robots you can check out other Robotics Projects that we built previously. 
 

Code
//Arduino Obstacle Avoiding Car Code
//by circuitdigest 


#include <Servo.h> 

// Ultrasonic sensor pins
#define trig 2
#define echo 4

// RGB led pins 
#define LR 9       // Led Right
#define LC 13     // Led Center
#define LL 10     // Led Left

#define LRR A3   // Led Right Red
#define LCR A2   // Led Center Red
#define LLR A1  // Led Left Red

// Motor control pins
#define LEFT_MOTOR_PIN1 8
#define LEFT_MOTOR_PIN2 7
#define RIGHT_MOTOR_PIN1 12
#define RIGHT_MOTOR_PIN2 11

#define ENA 6 // Enable A pin for motor speed control
#define ENB 3 // Enable B pin for motor speed control

// Distance thresholds for obstacle detection
//#define MAX_DISTANCE 80
#define MIN_DISTANCE_BACK 12

//float timeOut = 2*(MAX_DISTANCE+10)/100/340*1000000;
// Maximum and minimum motor speeds
#define MAX_SPEED 100
#define MIN_SPEED 75

Servo servoLook;

void setup() {
  // Set motor control pins as outputs
  pinMode(LEFT_MOTOR_PIN1, OUTPUT);
  pinMode(LEFT_MOTOR_PIN2, OUTPUT);
  pinMode(RIGHT_MOTOR_PIN1, OUTPUT);
  pinMode(RIGHT_MOTOR_PIN2, OUTPUT);

  pinMode(ENA, OUTPUT);
  pinMode(ENB, OUTPUT);

  pinMode(LR, OUTPUT);
  pinMode(LC, OUTPUT);
  pinMode(LL, OUTPUT);

  pinMode(LRR, OUTPUT);
  pinMode(LCR, OUTPUT);
  pinMode(LLR, OUTPUT);

  digitalWrite(LR, LOW);
  digitalWrite(LC, LOW);
  digitalWrite(LL, LOW);

  analogWrite(LRR, 0);
  analogWrite(LCR, 0);
  analogWrite(LLR, 0);

  servoLook.attach(5);
  //Set the Trig pins as output pins
  pinMode(trig, OUTPUT);
  
  //Set the Echo pins as input pins
  pinMode(echo, INPUT);

  analogWrite(ENA, 53.5); // speed for motor A  0-LOW speed, 255-Full speed
  analogWrite(ENB, 45); // speed for motor B
  
  // Initialize the serial communication for debugging
  Serial.begin(9600);
}

void loop() {

  servoLook.write(90);
  delay(750);
  int distance = getDistance();
  if (distance >= MIN_DISTANCE_BACK) {
    
    moveForward();
   
    Serial.println("forward");
  } 
   while(distance >= MIN_DISTANCE_BACK)
   {
    distance = getDistance();
    delay(50);
   }
   Stop(); 
   int turnDir = checkDirection();
   Serial.println(turnDir);
   switch(turnDir)
   {
    case 0:                                       //Turn left
    Serial.println("Left");
      turnLeft();
      delay(425);
      Stop();
      break;
    
    case 2:                                       //Turn right
    Serial.println("Right");
      turnRight();
      delay(415);
      Stop();
      break;
      
    case 3:                                       //move forward
    Serial.println("Forward");
     moveForward();
      break;
    }
}

int getDistance()                                   //Measure the distance to an object
{
  unsigned long pulseTime;                          //Create a variable to store the pulse travel time
  int distance;                                     //Create a variable to store the calculated distance
  digitalWrite(trig, HIGH);                         //Generate a 10 microsecond pulse
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  pulseTime = pulseIn(echo, HIGH);         //Measure the time for the pulse to return
  distance  = pulseTime / 29 / 2;         //Calculate the object distance based on the pulse time
  Serial.println(distance);
  return distance;
}

int checkDirection()                                            //Check the left and right directions and decide which way to turn
{
  int distances [3] = {0,0,0};                                    //Left ,right and Front distances
  int turnDir;                                               //Direction to turn, 0 left, 1 turn around, 2 right, 3 forward
  servoLook.write(180);                                         //Turn servo to look left
  delay(500);
  distances [0] = getDistance();                                //Get the left object distance

  servoLook.write(90);                                         //Turn servo to look left
  delay(500);
  distances [3] = getDistance();
  servoLook.write(0);                                           //Turn servo to look right
  delay(1000);
  distances [1] = getDistance();                                //Get the right object distance
  if (distances[0]<=25 && distances[1]<=25)                   //If both directions are blocked, move forward
    turnDir = 3;                                             
  else if (distances[0]>=distances[1])                          //If left has more space, turn left
    turnDir = 0;  //left
  else if (distances[0]<distances[1])                           //If right has more space, turn right
    turnDir = 2;  //right
   //else if (distances[0]<=50 && distances[1]<=50 && distances[3]<=50)    //If All directions are blocked, turn around
    //turnDir = 1;  
  return turnDir;
}

// Motor control functions
void moveForward() {
 
  analogWrite(ENA, 60.5); //  speed for motor A
  analogWrite(ENB, 52); //   speed for motor B

  digitalWrite(LR, HIGH);
  digitalWrite(LC, HIGH);      // All Green light turn on
  digitalWrite(LL, HIGH);

  analogWrite(LRR, 0);
  analogWrite(LCR, 0);       // All Red light turn off
  analogWrite(LLR, 0);
  
  digitalWrite(LEFT_MOTOR_PIN1, LOW);
  digitalWrite(LEFT_MOTOR_PIN2, HIGH);
  digitalWrite(RIGHT_MOTOR_PIN1, LOW);
  digitalWrite(RIGHT_MOTOR_PIN2, HIGH);
}

void moveBackward() {
  digitalWrite(LR, LOW);
  digitalWrite(LC, LOW);        // All Green light turn off
  digitalWrite(LL, LOW);

  analogWrite(LRR, 255);
  analogWrite(LCR, 255);       // All Red light turn on
  analogWrite(LLR, 255);
  
 
  digitalWrite(LEFT_MOTOR_PIN1, HIGH);
  digitalWrite(LEFT_MOTOR_PIN2, LOW);
  digitalWrite(RIGHT_MOTOR_PIN1, HIGH);
  digitalWrite(RIGHT_MOTOR_PIN2, LOW);
}

void turnRight() {
  analogWrite(ENA, 100); //  speed for motor A
  analogWrite(ENB, 90); //  speed for motor B
  
  digitalWrite(LR, HIGH);
  digitalWrite(LC, LOW);            // right side Green light turn on
  digitalWrite(LL, LOW);

  analogWrite(LRR, 0);
  analogWrite(LCR, 255);         // Left and Middel Red light turn on
  analogWrite(LLR, 255);
  
  
  digitalWrite(LEFT_MOTOR_PIN1, HIGH);
  digitalWrite(LEFT_MOTOR_PIN2, LOW);
  digitalWrite(RIGHT_MOTOR_PIN1, LOW);
  digitalWrite(RIGHT_MOTOR_PIN2, HIGH);
}

void turnLeft() {
  analogWrite(ENA, 100); //  speed for motor A
  analogWrite(ENB, 90); //  speed for motor B
  
  digitalWrite(LR, LOW);
  digitalWrite(LC, LOW);         // left side Green light turn on
  digitalWrite(LL, HIGH);

  analogWrite(LRR, 255);
  analogWrite(LCR, 255);         // right and Middel Red light turn on
  analogWrite(LLR, 0);
  
 
  digitalWrite(LEFT_MOTOR_PIN1, LOW);
  digitalWrite(LEFT_MOTOR_PIN2, HIGH);
  digitalWrite(RIGHT_MOTOR_PIN1, HIGH);
  digitalWrite(RIGHT_MOTOR_PIN2, LOW);
}

void Stop() {
  digitalWrite(LR, LOW);
  digitalWrite(LC, LOW);
  digitalWrite(LL, LOW);

  analogWrite(LRR, 255);
  analogWrite(LCR, 255);
  analogWrite(LLR, 255);
  
  analogWrite(ENA, 70.5); //  speed for motor A
  analogWrite(ENB, 60); //  speed for motor B
 
  digitalWrite(LEFT_MOTOR_PIN1, HIGH);
  digitalWrite(LEFT_MOTOR_PIN2, LOW);
  digitalWrite(RIGHT_MOTOR_PIN1, HIGH);
  digitalWrite(RIGHT_MOTOR_PIN2, LOW);
  delay(50);
  
  analogWrite(ENA, 0); //  speed for motor A
  analogWrite(ENB, 0); //  speed for motor B
  
  digitalWrite(LEFT_MOTOR_PIN1, LOW);
  digitalWrite(LEFT_MOTOR_PIN2, LOW);
  digitalWrite(RIGHT_MOTOR_PIN1, LOW);
  digitalWrite(RIGHT_MOTOR_PIN2, LOW);
}
Have any question realated to this Article?

Ask Our Community Members