Don't understand problem with my code

Question

I'm trying to get a robot car(elegoo one) to do the obstacle avoidance course. I am trying to write my own one from scratch to better myself and thus would really appreciate it if someone were to tell me what's wrong with my code and why it's not working

#include <Servo.h>  //servo library
Servo myservo;      // create servo object to control servo
Servo servosweep; 
int Echo = A4;  
int Trig = A5; 

#define ENA 5
#define ENB 6
#define IN1 7
#define IN2 8
#define IN3 9
#define IN4 11
#define carSpeedForward 100
#define carSpeedLeftandRight 150
int rightDistance = 0, leftDistance = 0, middleDistance = 0; 
int pos=0;


void forward(){ 
  analogWrite(ENA, carSpeedForward);
  analogWrite(ENB, carSpeedForward);
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
  Serial.println("Forward");
}

void back() {
  analogWrite(ENA, carSpeedForward);
  analogWrite(ENB, carSpeedForward);
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
  Serial.println("Back");
}

void left() {
  analogWrite(ENA, carSpeedLeftandRight);
  analogWrite(ENB, carSpeedLeftandRight);
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH); 
  Serial.println("Left");
}

void right() {
  analogWrite(ENA, carSpeedLeftandRight);
  analogWrite(ENB, carSpeedLeftandRight);
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
  Serial.println("Right");
}

void stop() {
  digitalWrite(ENA, LOW);
  digitalWrite(ENB, LOW);
  Serial.println("Stop!");
} 

//Ultrasonic distance measurement Sub function
int Distance_test() {
  digitalWrite(Trig, LOW);   
  delayMicroseconds(2);
  digitalWrite(Trig, HIGH);  
  delayMicroseconds(20);
  digitalWrite(Trig, LOW);   
  float Fdistance = pulseIn(Echo, HIGH);  
  Fdistance= Fdistance / 58;       
  return (int)Fdistance;
}  

void setup() { 
  myservo.write(pos);
  myservo.attach(3);  // attach servo on pin 3 to servo object
  Serial.begin(9600);     
  pinMode(Echo, INPUT);    
  pinMode(Trig, OUTPUT);  
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  pinMode(ENA, OUTPUT);
  pinMode(ENB, OUTPUT);
  //stop();
} 

void loop(){
  for (pos = 0; pos < 90; pos += 1.0) {
  servosweep.write(pos);
  delay(50);
  leftDistance=Distance_test();
  }
  
  myservo.write(90);
  middleDistance=Distance_test();
  
  for (pos=90;pos<=180;pos+=1.0){
    servosweep.write(pos);
    delay(50);
    rightDistance=Distance_test();
    
  }

  
  if(rightDistance > leftDistance) {
        right();
        delay(360);
      }
      else if(rightDistance < leftDistance) {
        left();
        delay(360);
      }
      else if((rightDistance <= 40) || (leftDistance <= 40)) {
        back();
        delay(180);
      }
      else {
        forward();
      }
}

I'm trying to get the ultrasonic sensor to read values from the left(0 to 90), the middle(90) and the right(90 to 180) and then use them for comparison to decide where to go. When I run this code my car goes in circles and I don't know what the problem is. Any help would be appreciated.

Answer 1

As jsotola writes, you are running through the 0-89 position and updating leftDistance every time. Then you find the middleDistance and finally you go through 91-180 and update rightDistance. After the two for loops, leftDistance will have the value for 89 and rightDistance will have the value for 180 because you overwrite your value without doing anything with it.

Start by skipping the for loops and measure at 0, 90 and 180 and use these values as you left, middle and right distance. When deciding if you want to back up, use the middle distance. Remember to use a delay between measurements that is large enough for the servo to get to the position for the next measurement

Answer 2

As a couple people already pointed out, you're using for loops that are unnecessary, and will cause you to get the wrong measurements. You're better off just using myservo.write(); to set your servo to positions 0 and 180.

Additionally you never check to see if there's an obstacle infront of your robot. Instead, your robot immediately tries turn right or left. That's what's causing it to spin.

Here's a fixed version of your void loop()

void loop(){

  myservo.write(0);
  delay(50);
  leftDistance=Distance_test();
  
  myservo.write(90);
  delay(50);
  middleDistance=Distance_test();
  
  myservo.write(180);
  delay(50);
  rightDistance=Distance_test();
   
  /* Start by checking if there is space to go forward */
  if(middleDistance > 40){ 
     forward();
     }
/* If there isn't space to go forward, turn */
  else if(rightDistance > leftDistance) {
        right();
        delay(360);
      }
  else if(rightDistance < leftDistance) {
        left();
        delay(360);
      }
  else if((rightDistance <= 40) || (leftDistance <= 40)) {
        back();
        delay(180);
      }
   else {
        forward();
      }
}

I hope this helps!