Programming a robot with HC-SR04 ultrasonic sensor (beginner)

Hi,

I am a beginner to robot programming and I wrote the following code for my obstacle avoidance robot.

#define trig 3
#define echo 2
#define thresholddistance 20.0

long duration;
float forwarddistance;
float distance[7];

#include <Servo.h>
#define pinServo 4

//----------------------------------------------------- Setting up Servo Angles
int servoArray[7] = {0, 30, 60, 90, 120, 150, 180};
Servo myServo;
//----------------------------------------------------- Motors
int motor_left[] = {9, 6};
int motor_right[] = {11, 10};
//----------------------------------------------------- Setup Module
void setup() {
  Serial.begin(9600);
  myServo.attach(pinServo);
  //Setup Motors
  for (int i = 0; i < 2; i++)
  {
    pinMode(motor_left[i], OUTPUT);
    pinMode(motor_right[i], OUTPUT);
  }
  //Setup Sensor
  pinMode(trig, OUTPUT);
  pinMode(echo, INPUT);

}
//----------------------------------------------------- Motor Module
// Following Module is for the Robot to brake
void brake()
{
  digitalWrite(motor_left[0], LOW);
  digitalWrite(motor_left[1], LOW);
  digitalWrite(motor_right[0], LOW);
  digitalWrite(motor_right[1], LOW);
}
// Following Module is for the Robot to go forward
void drive_forward()
{
  digitalWrite(motor_left[0], HIGH);
  digitalWrite(motor_left[1], LOW);
  digitalWrite(motor_right[0], LOW);
  digitalWrite(motor_right[1], HIGH);
}
// Following Module is for the Robot to slightly go towards left
void slight_left()
{
  digitalWrite(motor_left[0], HIGH);
  digitalWrite(motor_left[1], LOW);
  digitalWrite(motor_right[0], LOW);
  digitalWrite(motor_right[1], LOW);
  delay(300);
}
// Following Module is for the Robot to slightly go towards right
void slight_right()
{
  digitalWrite(motor_left[0], LOW);
  digitalWrite(motor_left[1], LOW);
  digitalWrite(motor_right[0], LOW);
  digitalWrite(motor_right[1], HIGH);
  delay(300);
}
  // Following Module is for the Robot to turn left
  void turn_left()
  {
    digitalWrite(motor_left[0], HIGH);
    digitalWrite(motor_left[1], LOW);
    digitalWrite(motor_right[0], LOW);
    digitalWrite(motor_right[1], LOW);
    delay(600);
  }
  // Following Module is for the Robot to turn right
  void turn_right()
  {
    digitalWrite(motor_left[0], LOW);
    digitalWrite(motor_left[1], LOW);
    digitalWrite(motor_right[0], LOW);
    digitalWrite(motor_right[1], HIGH);
    delay(600);
  }
  // Following Module is to make a uturn if there are obstacles to the right, to the left and in front
  void turn_around()
  {
    digitalWrite(motor_left[0], LOW);
    digitalWrite(motor_left[1], HIGH);
    digitalWrite(motor_right[0], HIGH);
    digitalWrite(motor_right[1], LOW);
    delay(600);
  }
  //----------------------------------------------------- Sensor Module
float distance_front() {
    myServo.write(90);
    digitalWrite(trig, HIGH);
    delayMicroseconds(10);
    digitalWrite(trig, LOW);

    duration = pulseIn(echo, HIGH);
    forwarddistance = duration / 58;
    delay(100);
    return forwarddistance;
  }
  //----------------------------------------------------- Sensor Array Module
float sensor_array() {
    for (int i = 0; i < 7; i++) {
      myServo.write(servoArray[i]);
      digitalWrite(trig, HIGH);
      delayMicroseconds(10);
      digitalWrite(trig, LOW);

      duration = pulseIn(echo, HIGH);
      distance[i] = duration / 58;

      delay(100);
      return distance[i];
    }
}
    //----------------------------------------------------- Loop Module
   
void loop() {
      distance_front();
      if (forwarddistance > thresholddistance)
      {
        drive_forward();
      }
      else {
        brake();
        sensor_array();
         if (distance[2] > thresholddistance && distance[1] > thresholddistance)
          {
        slight_left();
           }
         else if (distance[1] > thresholddistance && distance[0] > thresholddistance)
         {
        turn_left();
           }
         else if (distance[4] > thresholddistance && distance[5] > thresholddistance)
         {
        slight_right();
           }
         else if (distance[5] > thresholddistance && distance[6] > thresholddistance)
         {
        turn_right();
           }
         else
         {
        turn_around();
        brake();
        slight_left();
           }
     }
    delay(100);
  }

Basically, I want it to get input from the sensor and to decide which path to choose based on the loop, but it only performs the turn_around(); -> brake(); -> slight_left(); part of the loop.

I would appreciate it if you could tell me if I am calling the functions properly or if there is anything I need to fix.

Thanks.

I would appreciate it if you could tell me if I am calling the functions properly

No. The functions are manipulating global variables and returning them. They should NOT reference any global variables except pin numbers.

The functions should also NOT be delay()ing.

What do your Serial.print() statements tell you is happening? Why don't you?

I actually used Serial.print statements and tested for each portion of the loop. The loop works in the following segment up until the "sensor_array()" :

void loop() {
      distance_front();
      if (forwarddistance > thresholddistance)
      {
        drive_forward();
      }
      else {
        brake();

So, the portion that seems to cause the problem is:

sensor_array();
         if (distance[2] > thresholddistance && distance[1] > thresholddistance)
          {
        slight_left();
           }
         else if (distance[1] > thresholddistance && distance[0] > thresholddistance)
         {
        turn_left();
           }
         else if (distance[4] > thresholddistance && distance[5] > thresholddistance)
         {
        slight_right();
           }
         else if (distance[5] > thresholddistance && distance[6] > thresholddistance)
         {
        turn_right();
           }

I am guessing the loop doesn't recognize the "distance" array that I defined in the previous function. I want the "distance" array to store the 7 values given to it by the "for loop" in the sensor_array() function and I want to be able to use these 7 values in the "void loop" as it is shown. Did I define the "distance*" properly (will it actually store 7 values in this array?) and did I refer to it properly in the loop (for example, distance[5])?*
Sorry if it sounds confusing.
Thanks for your help.

So, the portion that seems to cause the problem is:

How? Is sensor_array() populating the array correctly, or not? Details matter.

I am guessing the loop doesn't recognize the "distance" array that I defined in the previous function.

If the "distance" array is local to a function, it is not visible in other functions. Why are there quotes around distance? The name of the array does not include quotes.

float distance_front() 
{
  myServo.write(90);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  duration = pulseIn(echo, HIGH);
  forwarddistance = duration / 58;
  delay(100);
  return forwarddistance;
}

You don't give the servo time to move to position 90 before starting the reading.
Same problem in "sensor_array".

Actually - "sensor_array" only makes one reading.

Hi, Thanks for the feedback. I changed the code a bit. Please find it below:

//----------------------------------------------------- Start

//----------------------------------------------------- Initialization
#define trig 3
#define echo 2
int thresholddistance = 30;

long duration;
float forwarddistance;
float distancefront;
float distanceArr[6];

#include <Servo.h>
#define pinServo 4

//----------------------------------------------------- Initialization: Setting up Servo Angles
int servoArray[6] = {0, 25, 50, 110, 145, 180};
Servo myServo;
//----------------------------------------------------- Initialization: Motors
int motor_left[] = {9, 6};
int motor_right[] = {11, 10};
//----------------------------------------------------- Initialization: Setup Module
void setup() {
  Serial.begin(9600);
  myServo.attach(pinServo);
  //Setup Motors
  for (int i = 0; i < 2; i++)
  {
    pinMode(motor_left[i], OUTPUT);
    pinMode(motor_right[i], OUTPUT);
  }
  //Setup Sensor
  pinMode(trig, OUTPUT);
  pinMode(echo, INPUT);

}
//----------------------------------------------------- Initialization: Motor Module
// Following Module is for the Robot to brake
void brake()
{
  digitalWrite(motor_left[0], LOW);
  digitalWrite(motor_left[1], LOW);
  digitalWrite(motor_right[0], LOW);
  digitalWrite(motor_right[1], LOW);
}
// Following Module is for the Robot to go forward
void drive_forward()
{
  digitalWrite(motor_left[0], HIGH);
  delayMicroseconds(400);
  digitalWrite(motor_left[1], LOW);
  delayMicroseconds(1000 - 400);
  digitalWrite(motor_right[0], LOW);
  delayMicroseconds(1000 - 400);
  digitalWrite(motor_right[1], HIGH);
  delayMicroseconds(400);
}
// Following Module is for the Robot to slightly go towards left
void slight_left()
{
  digitalWrite(motor_left[0], HIGH);
  digitalWrite(motor_left[1], LOW);
  digitalWrite(motor_right[0], LOW);
  digitalWrite(motor_right[1], LOW);
  delay(600);
}
// Following Module is for the Robot to slightly go towards right
void slight_right()
{
  digitalWrite(motor_left[0], LOW);
  digitalWrite(motor_left[1], LOW);
  digitalWrite(motor_right[0], LOW);
  digitalWrite(motor_right[1], HIGH);
  delay(600);
}
  // Following Module is for the Robot to turn left
  void turn_left()
  {
    digitalWrite(motor_left[0], HIGH);
    digitalWrite(motor_left[1], LOW);
    digitalWrite(motor_right[0], LOW);
    digitalWrite(motor_right[1], LOW);
    delay(900);
  }
  // Following Module is for the Robot to turn right
  void turn_right()
  {
    digitalWrite(motor_left[0], LOW);
    digitalWrite(motor_left[1], LOW);
    digitalWrite(motor_right[0], LOW);
    digitalWrite(motor_right[1], HIGH);
    delay(900);
  }
  // Following Module is to make a uturn if there are obstacles to the right, to the left and in front
  void turn_around()
  {
    digitalWrite(motor_left[0], LOW);
    digitalWrite(motor_left[1], HIGH);
    digitalWrite(motor_right[0], HIGH);
    digitalWrite(motor_right[1], LOW);
    delay(600);
  }
  //----------------------------------------------------- Sight Forward
float distance(int angle) {
    myServo.write(angle);
    delay(500);
    digitalWrite(trig, LOW);
    delayMicroseconds(20);
    digitalWrite(trig, HIGH);
    delayMicroseconds(20);
    digitalWrite(trig, LOW);

    duration = pulseIn(echo, HIGH);
    forwarddistance = duration / 58;
    return forwarddistance;
  }
  //----------------------------------------------------- Sensor Array Module
void scan_around() {
    for (int i = 0; i < 6; i++) {
      myServo.write(servoArray[i]);
      delay(500);
      digitalWrite(trig, LOW);
      delayMicroseconds(20);
      digitalWrite(trig, HIGH);
      delayMicroseconds(20);
      digitalWrite(trig, LOW);
      duration = pulseIn(echo, HIGH);
      distanceArr[i] = duration / 58;
      Serial.println(distanceArr[i]);
      delay(100);
    }
  }
    //----------------------------------------------------- Loop Module
   
void loop(){
  myServo.write(75);
  distancefront = distance(75);
 Serial.println(distancefront);
 // Serial.println(thresholddistance);
    if (distancefront > thresholddistance)
    {
      drive_forward();
      delay(1000);
 Serial.println("1");
    }
    else {
      brake();
      scan_around();
      delay(100);
      if (((distanceArr[1]) > 30.0) && ((distanceArr[2]) > 30.0))
      {
        slight_right();
        drive_forward();
        delay(1000);
        Serial.println("2");
      }
      else if (((distanceArr[0]) > 30.0) && ((distanceArr[1]) > 30.0))
      {
        turn_right();
        drive_forward();
        delay(1000);
        Serial.println("3");
      }
      else if (((distanceArr[3]) > 30.0) && ((distanceArr[4]) > 30.0))
      {
        slight_left();
        drive_forward();
        delay(1000);
        Serial.println("4");
      }
      else if (((distanceArr[4]) > 30.0) && ((distanceArr[5]) > 30.0))
      {
        turn_left();
        drive_forward();
        delay(1000);
        Serial.println("5");
      }
      else
      {
        turn_around();
        brake();
        delay(1000);
        slight_left();
        drive_forward();
        delay(1000);
        Serial.println("6");
      }
    }
//  delay(100);
}

First of all, now I'm getting values stored in the distanceArr array and I can see them when I use the "Serial.println(distanceArr*)". I also can see if the program correctly chooses the "if, else statements" that I have under "void loop" by using "Serial.println("1", etc.). But now, the problem is that whenever the sensor on the servo gets to the last position in the array (i.e. 180), it gives the actual value of the distance from the object but it also throws out a random value which is not supposed to be there and I think the if statements ignore the first value in the array and they count this random last value to be the last entry of the array. In other words, I get these values on my Serial Monitor:*
87.00 --> distance when Servo is centered (75 degrees seems to be the center for my servo).
1
88.00 --> distance when Servo is centered.
1
10.00 --> input 0
37.00 --> input 1
20.00 --> input 2
11.00 --> input 3
11.00 --> input 4
44.00 --> input 5
233.00 --> input 6 (random value - should not be there)
5 --> the loop that it decides to take after calculation

As it can be seen from the code and from the values in serial monitor above, the program shouldn't actually go into loop 5 because input 4 and input 5 are not both bigger than my thresholddistance value. So, I think the program believes the input 6 to be the fifth input and the input 5 to be the fourth input and so it makes a wrong decision.
Can you please tell me the problem and the source of the input 6?
Thanks.

Can you please tell me the problem and the source of the input 6?

No. There is nothing in your code that produces output anything like your annotated output.

Anonymous printing sucks. Stop doing it.

      Serial.print("distanceArr[");
      Serial.print(i);
      Serial.print("] = ");
      Serial.println(distanceArr[i]);

conveys so much more information.

Everywhere that you do anonymous printing, change to do something like this, so you KNOW what you are printing. Then, the bogus value will become real obvious.