Problems with if clauses and loop

Hello everybody,
I was software leader in team during engineering class, and I had not had any experience with writing code before. The goal of each team was to make robot, and then demonstrate how robot works. My team and I decided to make robot that would follow object in range from 20 cm to 60 cm. In fact, robot consisted of two DC motors, which allowed robot to move, servo motor, which rotated ultrasonic sensors and allowed them to increase its view angle, and ultrasonic sensors, which allowed robot to calculate distance between it and object. While hardware staff were under control of hardware leader, I just needed to write code for the robot. To write code that can follow object, I declared pins and maximum distance, made functions for DC motors and ultrasonic sensor, which allow me to save space, and made loop where the logic is located. I tested DC motors, servo motors, ultrasonic sensors, and functions, which were made to control them, with hardware leader, and they worked well. However, when I applied loop with the logic of the robot that consists mainly of if clauses my robot did not move but it could rotate ultrasonic sensors, and it could calculate distances between robot and object. Lemme clarify how logic works, if front distance is equal to range, it moves forward, otherwise if the front distance is not located in the range, we ask our robot to look at right/left, and if right/left distance is equal to the range, our robot will move right/left, otherwise if right/left distance is more than 60 cm, it will stay, or if right/left distance is less than 20cm, the robot will move backwards. As I mentioned before, I think that problem is located in loop but I could not find it because all functions worked well separately. Hence, I would really appreciate if someone could help me with solving my problem.

Here is a code

#include <NewPing.h> // is used to work with ultrasonic sensor
#include <Servo.h> // is used for servo motor
#define TRIG_PIN 12
#define ECHO_PIN  11
#define MAX_DISTANCE 200 

Servo myservo;        //initialize a servo object for the connected servo            

int angle = 0;   // Declaring starting angle  

int in1 = 7; //Declaring the pins where in1 in2 from the driver are wired  

int in2 = 8; // in1 and in2 are used for motor A 

int in3 = 2; // Declaring the pins where in3 in4 from the driver are wired 

int in4 = 13; // in2 and in3 are used for motor B 

int ConA = 5;  // PWM signal for A (First DC motor) 

int ConB = 3; // PWM signal for B  (Second DC motor)    

// PWM signal is used to control the speed motor, and only pins with this symbol “~” can deliver PWM signal 

NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE);
int readPing() { 
  delay(70);
  int cm = sonar.ping_cm();
}
  
// The function below is used to turn on motor B, the wheel rotates counterclockwise in order to go forward 

void TurnMotorB(){  

  digitalWrite(in1, LOW); 

  digitalWrite(in2, HIGH); 

  analogWrite(ConB,100); //Speed range (0-255) 
}

// The function below is used to turn on motor A, the wheel rotates counterclockwise in order to go forward  

void TurnMotorA(){ 

  digitalWrite(in3, LOW); 

  digitalWrite(in4, HIGH); 

  analogWrite(ConA, 100); //Speed range (0-255) 
}

// The function below is used to turn off motor A, in other words, it used to stop motor A 

void TurnOFFA(){ 

  digitalWrite(in3, LOW); 

  digitalWrite(in4, LOW); 

  analogWrite(ConA, 0); 
} 

// The function below is used to turn off motor B, in other words, it used to stop motor B  

void TurnOFFB(){ 

  digitalWrite(in1, LOW); 

  digitalWrite(in2, LOW); 

  analogWrite(ConB,0); 
} 

// The function below is used to rotate motor A clockwise, in other words, it used to move backward  

void TurnBackA(){ 

  digitalWrite(in3, HIGH); 

  digitalWrite(in4, LOW); 

  analogWrite(ConA, 50); //Speed range (0-255) 
} 

// The function below is used to rotate motor B clockwise, in other words, it used to move backward 

void TurnBackB(){ 

  digitalWrite(in1, HIGH); 

  digitalWrite(in2, LOW); 

  analogWrite(ConB, 50); //Speed range (0-255)  
}



void setup() {
  myservo.attach(9);  // it makes ultrasonic sensors to look forward
  myservo.write(115); 
  pinMode(in1, OUTPUT);   
  pinMode(in2, OUTPUT); 
  pinMode(in3, OUTPUT); 
  pinMode(in4, OUTPUT); 
  pinMode(ConB, OUTPUT); 
  pinMode(ConA, OUTPUT);  
}
void loop() {
  // Before we declared that robot’s ultrasonic sensors would look forward in the beginning, that is why left, and right distance is equal to zero 
  int RightDistance = 0;
  int LeftDistance = 0;
  int FrontDistance = readPing(); //this line asks our robot to measure the distance in front of the robot 
    delay(100);
  if (FrontDistance > 20 && FrontDistance < 60) {
   TurnMotorA();
   TurnMotorB();
  }
  else if (FrontDistance < 20 ) {
    myservo.write(170); 
    delay(1000);
    int RightDistance = readPing();
    delay(100);
    myservo.write(115);
   if (RightDistance > 20 && RightDistance < 60){
    TurnOFFB();
    TurnMotorA();
   } else {
    myservo.write(60); 
    delay(1000);
    int LeftDistance = readPing();
    delay(100);
    myservo.write(115);
   } if (LeftDistance > 20 && LeftDistance < 60) {
    TurnOFFA();
    TurnMotorB();
    }else{
    TurnBackB();
    TurnBackA();
    }
    }

  else if (FrontDistance > 60){
     myservo.write(170); 
     delay(1000);
     int RightDistance = readPing();
     delay(100);
     myservo.write(115);
     if (RightDistance > 20 && RightDistance < 60){
     TurnOFFB();
     TurnMotorA();
   } else {
     myservo.write(60); 
     delay(1000);
     int LeftDistance = readPing();
     delay(100);
     myservo.write(115);
   } if (LeftDistance> 20 && LeftDistance < 60) {
    TurnOFFA();
    TurnMotorB();
    }else{
    TurnOFFA();
    TurnOFFB();
    }
    }
  }
  

Do the DC Motors get Powers or are the driver shields broken ? Is the sonar reading correctly? Maybe do some Serial.println statements in front of the condition to be at least sure the Sensor is reading correct values and then put some Serial.println in the functions that are supposed to be called so you can see if the function actually gets called and it's just a hardware error or if something is the wrong software wise

Use the IDE autoformat tool (ctrl-t or Tools, Auto format)to indent the code so that the logic shows up. You need to mind the placement of your curly brackets.

Here is the loop() function formatted (see the comments).

void loop()
{
   // Before we declared that robot’s ultrasonic sensors would look forward in the beginning, that is why left, and right distance is equal to zero
   int RightDistance = 0;
   int LeftDistance = 0;
   int FrontDistance = readPing(); //this line asks our robot to measure the distance in front of the robot
   delay(100);
   if (FrontDistance > 20 && FrontDistance < 60)
   {
      TurnMotorA();
      TurnMotorB();
   }
   else if (FrontDistance < 20 )
   {
      myservo.write(170);
      delay(1000);
      int RightDistance = readPing();
      delay(100);
      myservo.write(115);
      // the next if will never execute because FrontDistance
      // must be < 20 to get here
      if (RightDistance > 20 && RightDistance < 60)
      {
         TurnOFFB();
         TurnMotorA();
      }
      else
      {
         myservo.write(60);
         delay(1000);
         int LeftDistance = readPing();
         delay(100);
         myservo.write(115);
      } if (LeftDistance > 20 && LeftDistance < 60)
      {
         TurnOFFA();
         TurnMotorB();
      }
      else
      {
         TurnBackB();
         TurnBackA();
      }
   }

   else if (FrontDistance > 60)
   {
      myservo.write(170);
      delay(1000);
      int RightDistance = readPing();
      delay(100);
      myservo.write(115);
      // The next if will never execute because FrontDistance 
      // must be > than 60 to get here      
      if (RightDistance > 20 && RightDistance < 60)
      {
         TurnOFFB();
         TurnMotorA();
      }
      else
      {
         myservo.write(60);
         delay(1000);
         int LeftDistance = readPing();
         delay(100);
         myservo.write(115);
      } if (LeftDistance > 20 && LeftDistance < 60)
      {
         TurnOFFA();
         TurnMotorB();
      }
      else
      {
         TurnOFFA();
         TurnOFFB();
      }
   }
}

I don't think that is an accurate comment. FrontDistance will be greater than 60 but then the robot turn the untrasonic to the right and reads again. That reading (RightDistance) can be whatever depending on how much turning is involved and how wide the ping is.

Same of the other comment

I guess that you see something that I am missing. The distances are not read inside of the if blocks. How can the FrontDistance change from < 20 to >20.

I see now. I was looking at the wrong variable(s).

Bingo! Unfortunately, I do that all to often. FrontDistance != RightDistance :slight_smile:

Time to get more coffee for me and clean my glasses. :blush:

Hello
I guess this ammount of

Line 27:   delay(70);
	Line 114:     delay(100);
	Line 121:     delay(1000);
	Line 123:     delay(100);
	Line 130:     delay(1000);
	Line 132:     delay(100);
	Line 145:      delay(1000);
	Line 147:      delay(100);
	Line 154:      delay(1000);
	Line 156:      delay(100);

will inhibit the desired realtime behaviour of the sketch.

Thank you all, it actually works after switching that lines.