Problem with some robot code w/ HC-SR04

Using Arduino Programming Questions
1

Could someone explain why what's wrong with the code? (dist3=distance forward, dist1=distance left, dist2=distance right)

#include <Servo.h>

#define trig 13
#define echo 12
Servo myservo;
int servoLeft = 0; //angle of microservo rotation to scan on left
int servoForward = 90;
int servoRight = 180; //angle of microservo rotation to scan on right
int motorA1 = 2;
int motorA2 = 4;
int motorB1 = 6; 
int motorB2 = 7;
int val, dist1, dist2, dist3;

void setup() {
  Serial.begin(9600);

  myservo.attach(9);

  pinMode(trig, OUTPUT);
  pinMode(echo, INPUT);

  pinMode(motorA1, OUTPUT);
  pinMode(motorA2, OUTPUT);
  pinMode(motorB1, OUTPUT);
  pinMode(motorB2, OUTPUT);
}

void loop() {
scan();
Serial.println(dist3);

if(dist3>40){
do{
  forward();
  scanForwardServo();
}while(dist3>35);
stop();
}
else if(dist1>dist2){
  backward();
  delay(300);
  left();
  delay(1200);
  stop();
  scanForwardServo();
  
  if(dist3>40){
    do{
    forward();
    scanForwardServo();
    }while(dist3>35);
    stop();
   }
 }

else if(dist2>dist1){
  backward();
  delay(300);
  right();
  delay(1200);
  stop();
  scanForwardServo();
  
  if(dist3>40){
    do{
    forward();
    scanForwardServo();
    }while(dist3>35);
    stop();
   }
  }
}

void scanForwardServo(){
  myservo.write(servoForward);
  digitalWrite(trig, HIGH);
  delay(500);
  digitalWrite(trig, LOW);
  val = pulseIn(echo, HIGH);
  dist3 = (val/2)/29.1;
}

 void scanRightServo(){
  myservo.write(servoRight);

  delay(50);
  digitalWrite(trig, HIGH);
  delay(500);
  digitalWrite(trig, LOW);
  val = pulseIn(echo, HIGH);
  dist2 = (val/2)/29.1;
 }

 void scanLeftServo(){
  myservo.write(servoLeft);

  delay(50);
  digitalWrite(trig, HIGH);
  delay(500);
  digitalWrite(trig, LOW);
  val = pulseIn(echo, HIGH);
  dist1 = (val/2)/29.1;
}

void scan(){
  scanLeftServo();
  delay(150);
  scanRightServo();
  delay(150);
  scanForwardServo();
  delay(150);
}

void forward(){
  digitalWrite(motorA1, HIGH);
  digitalWrite(motorA2, LOW);
  digitalWrite(motorB1, HIGH);
  digitalWrite(motorB2, LOW); 
}

void backward(){
  digitalWrite(motorA1, LOW);
  digitalWrite(motorA2, HIGH);
  digitalWrite(motorB1, LOW);
  digitalWrite(motorB2, HIGH);
}

void left(){
  digitalWrite(motorA1, HIGH);
  digitalWrite(motorA2, LOW);
  digitalWrite(motorB1, LOW);
  digitalWrite(motorB2, LOW);
}

void right(){
  digitalWrite(motorA1, LOW);
  digitalWrite(motorA2, LOW);
  digitalWrite(motorB1, HIGH);
  digitalWrite(motorB2, LOW); 
}

void stop(){
  digitalWrite(motorA1, LOW);
  digitalWrite(motorA2, LOW);
  digitalWrite(motorB1, LOW);
  digitalWrite(motorB2, LOW);
}

Sometimes the robot car turns in the right direction (depends on distance, comparing left and right side), but sometimes in goes in the opposite direction (the direction with less distance). Why does this occur and how can I fix it? As you can see in the code I did use a serial monitor to read the distances to insure that everything is ok with the sensor.

2

while(dist3>35);What code will be executed while dist3 is greater than 35 ? What happens if dist3 is less than 35 when this line of code is encountered ?

3

It's difficult to say without knowing under what circumstances the car turns the wrong way, but I suspect that it has to do with the way you read the sensors. At the top of loop() readings in all directions are taken. If the car moves forward it will be in a different place than when the left and right sensors were read, but the values used to compare directions were read before the car moved. Consider the below code with comments.

void loop() {
  scan();                     // dist1, dist2 and dist3 are set 
  Serial.println(dist3);

  if (dist3 > 40) {
    do {
      forward();              // car moves forward
      scanForwardServo();     // dist3 is set   
    } while (dist3 > 35);
    stop();
  }
  else if (dist1 > dist2) {   // old values for dist1 and dist2, before car moved

You probably should rearrange the code so that for each pass of loop() you decide which direction to move in and then move until the car has to stop. If the car should move forward then left it will move forward for the first pass of loop() then left for the second pass and so on.

BTW, if this was my code I would combine the scan servo functions into one:

int scanServo(int angle)
{
  myservo.write(angle);
  digitalWrite(trig, HIGH);
  delay(500);
  digitalWrite(trig, LOW);
  val = pulseIn(echo, HIGH);
  return ((val / 2) / 29.1);
}

Then the distance variables could be set like this:

dist3 = scanServo(servoForward);
dist1 = scanServo(servoLeft);
dist2 = scanServo(servoRight);
4

Could someone explain why what's wrong with the code? (dist3=distance forward, dist1=distance left, dist2=distance right)

The biggest problem is right there in the question.

distFore, distLeft, and distRight would be far better choices for the names. Then, you wouldn't need a cheat-sheet to know which number corresponds to which sensor/direction.