Object Avoidance Code help

Hi, I’m new to the forum and Arduino in general.

When running my code only my two left wheels rotate forward and when I hold my hand in front of my ultrasonic sensor my two left wheels stop, goes backward and then the sensor looks left and right before the two left wheels continue to rotate forward.

What I want to happen is for all 4 wheels to rotate forward but when the ultrasonic sensor detects an object in front it all 4 wheels rotate backward and it turns in the appropriate direction after it looks for an opening. I’ve tested the left and right wheels with the same connections and pin setup and they’re both functioning but my right wheels don’t move at all when I run my code below. Any insight is welcome.

Thanks! Btw im using a l298n motor driver.

Object_Avoidance_Robot.ino (3.4 KB)

Please post your code

int trigPin = 12;
int echoPin = 11;
int enA = 9;
int leftmotor = 4; // in3
int leftbrake = 2; // in4
int enB = 5;
int rightmotor = 8; // in1
int rightbrake = 7; //in2

#include <NewPing.h> // to use new Ping library
#include <Servo.h>   // to use Servo library
Servo myservo;  
#define MAX_DISTANCE 200 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.
#define MAX_SPEED 200

NewPing sonar(trigPin, echoPin, MAX_DISTANCE); // NewPing setup of pins and maximum distance.
int distance = 0;
int speedSet = 0;

void setup() {  // put your setup code here, to run once:
Serial.begin(115200); // Open serial monitor at 115200 baud to see ping results.

 myservo.attach(10); 
 myservo.write(90); // servo pointing eyes straight 
  delay(2000);
                
  distance = readPing();
  delay(100);

pinMode(enA,OUTPUT);
pinMode(4,OUTPUT); // right motor forward
pinMode(2,OUTPUT); // right motor backward

pinMode(enB,OUTPUT);
pinMode(7,OUTPUT);// left motor forward
pinMode(8,OUTPUT);// left motor backward
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);

digitalWrite(rightbrake,LOW);
digitalWrite(leftbrake,LOW);//disengage brakes

 goUp();
}


void loop() {  // put your main code here, to run repeatedly
  int distanceR = 0;
  int distanceL = 0;
  distance = readPing();
 
 if(distance<=25)
 {
  moveStop();
  delay(1000);
  backup();
  delay(1000);
  moveStop();
  delay(1000);

  
  distanceR = lookRight();
  delay(500);
  distanceL = lookLeft();
  delay(500);

  if(distanceR>=distanceL)
  {
  turnRightandUp();
  delay(1000);
   moveStop();
  } 
  
  else
  
  {
    turnLeftandUp();
    delay(100);
    moveStop();
  }
 }
 
 else
 {
   digitalWrite(leftmotor,HIGH);    
   digitalWrite(leftbrake,LOW);   
   analogWrite(enA,200);
   digitalWrite(rightmotor,HIGH);   
   digitalWrite(rightbrake,LOW);   
    analogWrite(enB,200);
 
 }
  distance = readPing();
}




int lookRight()
{
    myservo.write(150); 
    delay(500);
    distance =readPing();
    delay(100);
    myservo.write(90); 
    return distance;
}

int lookLeft()
{
    myservo.write(15); 
    delay(500);
    distance = readPing();
    delay(100);
    myservo.write(90); 
    return distance;
    delay(100);
}

int readPing() { 
  delay(100);
  int cm = sonar.ping_cm();
  Serial.println(cm);
  if(cm==0)
  {
    cm = 250;
  }
  return cm;
 
}

void moveStop() {
digitalWrite(rightbrake,LOW);
digitalWrite(rightmotor,LOW);
analogWrite(enA,0);
digitalWrite(leftmotor,LOW);  
digitalWrite(leftbrake,LOW);
analogWrite(enB,0);
  } 
  

void backup(){
  delay(100);
   digitalWrite(leftmotor,LOW);    
   digitalWrite(leftbrake,HIGH); 
   analogWrite(enB,200);  
   digitalWrite(rightmotor,LOW);   
   digitalWrite(rightbrake,HIGH); 
   analogWrite(enA,200);
  
}

void turnRightandUp() {
  digitalWrite(leftmotor,HIGH);    
   digitalWrite(leftbrake,LOW);  
   analogWrite(enB,200);
   digitalWrite(rightmotor,LOW);   
   digitalWrite(rightbrake,LOW);   
   delay(1000);
   
   goUp();
   delay(500);
}

void  turnLeftandUp() {  
   digitalWrite(rightmotor,HIGH);   
   digitalWrite(rightbrake,LOW);  
   analogWrite(enA,200); 
   digitalWrite(leftmotor,LOW);    
   digitalWrite(leftbrake,LOW);
   delay(500);
   goUp();
   delay(500);
}

void goUp() {
   digitalWrite(leftmotor,HIGH);    
   digitalWrite(leftbrake,LOW);  
   analogWrite(enB,200);
   digitalWrite(rightmotor,HIGH);   
   digitalWrite(rightbrake,LOW);
   analogWrite(enA,200);
   
}

Have a close look at the Servo library reference page.
(Some comfort: You’re not the first to do this)