Need help with autopilot car code

[memind]

Hello everyone, how are you all.
I have built an RC car that has 2 modes: autopilot and manual mode.
It consists of 2 Arduino Nanos for the car, 1 that is for controlling the vehicle and receiving the RF signal (it uses an nrf24l01+), and the 2nd Arduino Nano gets a message from the Arduino Nano 1 via serial to start autopilot, it has 5 ultrasonic devices and it sends what the car should drive via serial too. The problem is almost all the time 1 - 3 sensors don't work. So I have added an OLED 0.96in screen. Firstly the left sensor always displays 999 cm, I tried switching it, using different pins but it stays the same. And sometimes the front sensor doesn't work, and sometimes the right-front sensor.
I use 2 l7805cv to give out 5v for the sensors,1 for the 3 front, 2 for the left and right.
This is the code for the Arduino Nano 2 (the one for autopilot):

const int trigPin = 2;
const int echoPin = 3;  // Front

const int trigPin1 = 12;
const int echoPin1 = 5;  // Left

const int trigPin2 = 6;
const int echoPin2 = 7;  // Right0

const int trigPin3 = 8;
const int echoPin3 = 9;  // Front Left

const int trigPin4 = 10;
const int echoPin4 = 11;  // Front Right
const int led = 5;
const long timeout = 20000;        // 20 milliseconds timeout for pulseIn
const int distanceThreshold = 30;  // Threshold distance in cm
const int distanceThreshold2 = 10;
int autodrive = 0;
float duration, distance;
int sent = 0;
#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#define SCREEN_WIDTH 128  // OLED display width, in pixels
#define SCREEN_HEIGHT 64  // OLED display height, in pixels
#define OLED_RESET -1     // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x3C
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

void setup() {
  Serial.begin(9600);
  while (!Serial) {
    // Wait for the serial connection to be established
  }
  if (!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;)
      ;  // Don't proceed, loop forever
  }
  Serial.setTimeout(50);  // Set a shorter timeout for Serial.readStringUntil

  pinMode(LED_BUILTIN, OUTPUT);

  pinMode(trigPin, OUTPUT);  // Sets the trigPin as an Output
  pinMode(echoPin, INPUT);   // Sets the echoPin as an Input

  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);  // Sets the echoPin as an Input

  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);  // Sets the echoPin as an Input

  pinMode(trigPin3, OUTPUT);
  pinMode(echoPin3, INPUT);  // Sets the echoPin as an Input

  pinMode(trigPin4, OUTPUT);
  pinMode(echoPin4, INPUT);  // Sets the echoPin as an Input
  pinMode(led, OUTPUT);
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(0, 28);
  display.println("waiting for response");
  display.display();
  display.clearDisplay();
}

void loop() {
  if (Serial.available() > 0) {
    String receivedString = Serial.readStringUntil('\n');
    int receivedNumber = receivedString.toInt();

    if (receivedNumber == 1) {
      digitalWrite(LED_BUILTIN, HIGH);
      autodrive = 1;
      /*
      display.clearDisplay();
      display.setTextSize(1);
      display.setTextColor(WHITE);
      display.setCursor(0, 1);
      display.println("Autodrive On");
      display.display();
      */
    } else if (receivedNumber == 0) {
      digitalWrite(LED_BUILTIN, LOW);
      autodrive = 0;
      /*
      display.clearDisplay();
      display.setTextSize(1);
      display.setTextColor(WHITE);
      display.setCursor(0, 1);
      display.println("Autodrive OFF");
      display.display();
      */
    }

    // Clear the serial buffer
    while (Serial.available() > 0) {
      Serial.read();
    }
  }


  if (autodrive == 1) {
    digitalWrite(led, HIGH);
    long frontDistance = getDistance(trigPin, echoPin);
    delay(5);
    long frontLeftDistance = getDistance(trigPin3, echoPin3);
    delay(5);
    long frontRightDistance = getDistance(trigPin4, echoPin4);
    delay(5);
    long leftDistance = getDistance(trigPin1, echoPin1);
    delay(5);
    long rightDistance = getDistance(trigPin2, echoPin2);
    delay(5);
    if (frontDistance >= 999 || frontDistance == 0) {
      display.setTextSize(1);
      display.setTextColor(WHITE);
      display.setCursor(0, 10);
      display.println("Front (mid)");
      display.display();
    } else {
      display.setCursor(0, 10);        // Move cursor back to the same position
      display.setTextColor(BLACK);     // Change back to white for the new text
      display.println("Front (mid)");  // New text
    }
    if (frontRightDistance >= 999 || frontRightDistance == 0) {
      display.setTextSize(1);
      display.setTextColor(WHITE);
      display.setCursor(0, 20);
      display.println("Front Right");
      display.display();
    } else {
      display.setCursor(0, 20);        // Move cursor back to the same position
      display.setTextColor(BLACK);     // Change back to white for the new text
      display.println("Front Right");  // New text
    }
    if (frontLeftDistance >= 999 || frontLeftDistance == 0) {
      display.setTextSize(1);
      display.setTextColor(WHITE);
      display.setCursor(0, 30);
      display.println("FrontLeft");
      display.display();
    } else {
      display.setCursor(0, 30);      // Move cursor back to the same position
      display.setTextColor(BLACK);   // Change back to white for the new text
      display.println("FrontLeft");  // New text
    }
    if (leftDistance >= 999 || leftDistance == 0) {
      display.setTextSize(1);
      display.setTextColor(WHITE);
      display.setCursor(0, 40);
      display.print("Left: ");        // First print the label
      display.println(leftDistance);  // Then print the distance value
      display.display();
    } else {
      display.setCursor(0, 40);               // Move cursor back to the same position
      display.setTextColor(BLACK);            // Change back to white for the new text
      display.println(leftDistance, "left");  // New text
    }
    if (rightDistance >= 999 || rightDistance == 0) {
      display.setTextSize(1);
      display.setTextColor(WHITE);
      display.setCursor(0, 50);
      display.println("Right");
      display.display();
    } else {
      display.setCursor(0, 50);     // Move cursor back to the same position
      display.setTextColor(BLACK);  // Change back to white for the new text
      display.println("Right");     // New text
    }
    if (sent == 0) {
      Serial.println(2);
      sent = 1;
    }
    if ((frontDistance > 0 && frontDistance < distanceThreshold) || (frontLeftDistance > 0 && frontLeftDistance < distanceThreshold2) || (frontRightDistance > 0 && frontRightDistance < distanceThreshold2)) {
      sent = 0;
      Serial.println(4);
      while (leftDistance < 15 && rightDistance < 15) {
        leftDistance = getDistance(trigPin1, echoPin1);

        delay(10);
        rightDistance = getDistance(trigPin2, echoPin2);

        delay(10);
        Serial.println(8);
        delay(60);
      }
      Serial.println(4);

      if (rightDistance < leftDistance) {
        Serial.println(6);  // Turn right
      } else {
        Serial.println(7);  // Turn left
      }

      delay(450);         // Small delay before sending the next signal
      Serial.println(2);  // Move forward

      if (frontDistance >= distanceThreshold) {
        Serial.println(2);  // Move forward
      }
      delay(5);  // Adjust the delay based on your needs
    }
  } else {
    digitalWrite(led, LOW);
  }
}

long getDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  duration = pulseIn(echoPin, HIGH, timeout);
  if (duration == 0) {
    return 999;  // Timeout - no echo
  }
  distance = (duration * 0.0343) / 2;
  return distance;
}

The code for the Arduino nano 1 (which is perfectly fine):

#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>

RF24 radio(7, 4);                      // CE, CSN
const uint64_t pipe = 0xE8E8F0F0E1LL;  // Pipe address for communication

// L298N 1
const int ena = 3;
const int in1 = 2;
const int in2 = 8;
const int in3 = A4;
const int in4 = A5;
const int enb = 9;

// L298N 2
const int in12 = A0;
const int in22 = A1;
const int in32 = A2;
const int in42 = A3;
const int ena2 = 5;
const int enb2 = 6;

// Joystick Thresholds
const int joystickMin = 35;
const int joystickMax = 970;

int currentState = 0;  // Variable to store the current state
int autodrive = 0;     // Variable to store the autodrive state

void setup() {
  Serial.begin(9600);
  while (!Serial) {
    // Wait for the serial connection to be established
  }
  radio.begin();
  radio.openReadingPipe(1, pipe);
  radio.setPALevel(RF24_PA_HIGH);
  radio.setDataRate(RF24_250KBPS);
  radio.startListening();

  // L298N 1
  pinMode(ena, OUTPUT);
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(enb, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);

  // L298N 2
  pinMode(in12, OUTPUT);
  pinMode(in22, OUTPUT);
  pinMode(in32, OUTPUT);
  pinMode(in42, OUTPUT);
  pinMode(ena2, OUTPUT);
  pinMode(enb2, OUTPUT);
}

void controlMotors(int xAxis, int yAxis, int potValue) {
  analogWrite(ena, potValue);
  analogWrite(enb, potValue);
  analogWrite(ena2, potValue);
  analogWrite(enb2, potValue);

  if (yAxis >= joystickMax) {
    digitalWrite(in1, HIGH);
    digitalWrite(in2, LOW);
    digitalWrite(in3, LOW);
    digitalWrite(in4, HIGH);
    digitalWrite(in12, HIGH);
    digitalWrite(in22, LOW);
    digitalWrite(in32, LOW);
    digitalWrite(in42, HIGH);
  } else if (yAxis <= joystickMin) {
    digitalWrite(in1, LOW);
    digitalWrite(in2, HIGH);
    digitalWrite(in3, HIGH);
    digitalWrite(in4, LOW);
    digitalWrite(in12, LOW);
    digitalWrite(in22, HIGH);
    digitalWrite(in32, HIGH);
    digitalWrite(in42, LOW);
  } else if (xAxis <= joystickMin) {
    //back
    digitalWrite(in1, LOW);
    digitalWrite(in2, HIGH);
    digitalWrite(in3, LOW);
    digitalWrite(in4, HIGH);
    digitalWrite(in12, LOW);
    digitalWrite(in22, HIGH);
    digitalWrite(in32, LOW);
    digitalWrite(in42, HIGH);
  } else if (xAxis >= joystickMax) {
    //straight
    digitalWrite(in1, HIGH);
    digitalWrite(in2, LOW);
    digitalWrite(in3, HIGH);
    digitalWrite(in4, LOW);
    digitalWrite(in12, HIGH);
    digitalWrite(in22, LOW);
    digitalWrite(in32, HIGH);
    digitalWrite(in42, LOW);
  } else {
    digitalWrite(in1, LOW);
    digitalWrite(in2, LOW);
    digitalWrite(in3, LOW);
    digitalWrite(in4, LOW);
    digitalWrite(in12, LOW);
    digitalWrite(in22, LOW);
    digitalWrite(in32, LOW);
    digitalWrite(in42, LOW);
  }
}

void loop() {
  if (radio.available()) {
    int16_t joystickData[4];
    radio.read(&joystickData, sizeof(joystickData));

    int xAxis = joystickData[0];
    int yAxis = joystickData[1];
    int potValue = joystickData[2];
    autodrive = joystickData[3];

    if (autodrive == 1 && currentState != 1) {
      Serial.println(1);
      currentState = 1;
    } else if (autodrive == 0 && currentState != 0) {
      Serial.println(0);
      currentState = 0;
    }

    if (currentState == 0) {
      controlMotors(xAxis, yAxis, potValue);
    }
  }

  if (Serial.available() > 0) {
    int receivedNumber = Serial.parseInt();

    if (receivedNumber == 2) {
      digitalWrite(in1, HIGH);
      digitalWrite(in2, LOW);
      digitalWrite(in3, HIGH);
      digitalWrite(in4, LOW);
      digitalWrite(in12, HIGH);
      digitalWrite(in22, LOW);
      digitalWrite(in32, HIGH);
      digitalWrite(in42, LOW);
    }
    if (receivedNumber == 4) {
      digitalWrite(in1, LOW);
      digitalWrite(in2, LOW);
      digitalWrite(in3, LOW);
      digitalWrite(in4, LOW);
      digitalWrite(in12, LOW);
      digitalWrite(in22, LOW);
      digitalWrite(in32, LOW);
      digitalWrite(in42, LOW);
    }
    if (receivedNumber == 6) {
      digitalWrite(in1, LOW);
      digitalWrite(in2, HIGH);
      digitalWrite(in3, HIGH);
      digitalWrite(in4, LOW);
      digitalWrite(in12, LOW);
      digitalWrite(in22, HIGH);
      digitalWrite(in32, HIGH);
      digitalWrite(in42, LOW);
    }
    if (receivedNumber == 8) {
      digitalWrite(in1, LOW);
      digitalWrite(in2, HIGH);
      digitalWrite(in3, LOW);
      digitalWrite(in4, HIGH);
      digitalWrite(in12, LOW);
      digitalWrite(in22, HIGH);
      digitalWrite(in32, LOW);
      digitalWrite(in42, HIGH);

    } else if (receivedNumber == 7) {
      digitalWrite(in1, HIGH);
      digitalWrite(in2, LOW);
      digitalWrite(in3, LOW);
      digitalWrite(in4, HIGH);
      digitalWrite(in12, HIGH);
      digitalWrite(in22, LOW);
      digitalWrite(in32, LOW);
      digitalWrite(in42, HIGH);
    }
  }
}

This project is really complex and took me well over 3 years
Any help will be appreciated, thanks.
The car:

[jremington]

@memind

The problem is almost all the time 1 - 3 sensors don't work.

Debug those with a small program written specifically to test each sensor individually.

Double check all the wiring. If you are using breadboards, keep in mind that they are unreliable and intended for temporary experiments only.

[memind]

They not always dont work. Only the left sensor just never works, I tried swapping the ultrasonic sensor, same thing still not working, swapped the cables, same thing, changed its pin for 13 to 5, same thing. So I'm sure the left sensor is a code problem. I even connected all the pins from the Arduino to the ultrasonic using the Arduino nano expansion board to ensure that every single wire is held tightly. So what could be the problem that makes like 2 of the front sensors work a little, then stop working, then work again by itself? like is there a piece of hardware I could get to fix the problem (like a capacitor, a regulator, or something like that), or is it a code problem? (I will make a program to test the left sensor especially)

[jremington]

@memind

So what could be the problem that makes like 2 of the front sensors work a little, then stop working, then work again by itself?

Intermittent wiring contacts, for example.

[memind]

what can i do to solve that?

[jremington]

Reposition or replace faulty wiring. Avoid breadboards and solder all connections used in long term projects, use gold-plated connectors.

Consider using Adafruit protoboards, like this 5-phase stepper driver, from one of my projects. All connections are soldered.

[memind]

I'm using an Arduino nano expansion board which looks like this: so no breadboard is being used:

[jremington]

Nevertheless, intermittent connections or broken wires would explain the problem. Check continuity of all connections using your multimeter.

Another possibility is that you are almost out of dynamic memory, as the screen on Nano1 takes half of it.

You can save dynamic memory by modifying all print statements, like this one

  display.println("waiting for response");

to use the F macro, as follows. This puts the character string in program memory.

  display.println(F("waiting for response"));

[memind]

Okay I will. But are you sure its not a code glitch? Because i have tried simulating it on wokwi (an arduino sim) and it had the same value for left "999" but when i connected it to the front one it worked.
(connected to the front one) working

connect to the left one:
displaying its value is 999

so now im convinced its a coding problem.

[jremington]

Try the F macro idea, added after you posted.

[memind]

Is there a way to measure the amount of dynamic memory left? or is it just a big deal. Anyways i am currently modifying the code to use the f macro.

[jremington]

Of course there is, search for "arduino free memory".

[memind]

Sadly, even after I use the F macro for all the diplay.println, the problem still resumes :(.
Also what is "Arduino free memory" are you for real? I searched for it and nothing came.

[memind]

I even removed the whole oled display thing from the code:

const int trigPin = 2;
const int echoPin = 3;  // Front

const int trigPin1 = 12;
const int echoPin1 = 5;  // Left

const int trigPin2 = 6;
const int echoPin2 = 7;  // Right0

const int trigPin3 = 8;
const int echoPin3 = 9;  // Front Left

const int trigPin4 = 10;
const int echoPin4 = 11;  // Front Right
const int led = 5;
const long timeout = 20000;        // 20 milliseconds timeout for pulseIn
const int distanceThreshold = 30;  // Threshold distance in cm
const int distanceThreshold2 = 10;
int autodrive = 1;
float duration, distance;
int sent = 0;
#include <SPI.h>
#include <Wire.h>

void setup() {
  Serial.begin(9600);
  while (!Serial) {
    // Wait for the serial connection to be established
  }


  pinMode(LED_BUILTIN, OUTPUT);

  pinMode(trigPin, OUTPUT);  // Sets the trigPin as an Output
  pinMode(echoPin, INPUT);   // Sets the echoPin as an Input

  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);  // Sets the echoPin as an Input

  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);  // Sets the echoPin as an Input

  pinMode(trigPin3, OUTPUT);
  pinMode(echoPin3, INPUT);  // Sets the echoPin as an Input

  pinMode(trigPin4, OUTPUT);
  pinMode(echoPin4, INPUT);  // Sets the echoPin as an Input
  pinMode(led, OUTPUT);
}

void loop() {
  if (Serial.available() > 0) {
    String receivedString = Serial.readStringUntil('\n');
    int receivedNumber = receivedString.toInt();

    if (receivedNumber == 1) {
      digitalWrite(LED_BUILTIN, HIGH);
      autodrive = 1;
      /*
      display.clearDisplay();
      display.setTextSize(1);
      display.setTextColor(WHITE);
      display.setCursor(0, 1);
      display.println("Autodrive On");
      display.display();
      */
    } else if (receivedNumber == 0) {
      digitalWrite(LED_BUILTIN, LOW);
      autodrive = 0;
      /*
      display.clearDisplay();
      display.setTextSize(1);
      display.setTextColor(WHITE);
      display.setCursor(0, 1);
      display.println("Autodrive OFF");
      display.display();
      */
    }

    // Clear the serial buffer
    while (Serial.available() > 0) {
      Serial.read();
    }
  }


  if (autodrive == 1) {
    digitalWrite(led, HIGH);
    long frontDistance = getDistance(trigPin, echoPin);
    delay(5);
    long frontLeftDistance = getDistance(trigPin3, echoPin3);
    delay(5);
    long frontRightDistance = getDistance(trigPin4, echoPin4);
    delay(5);
    long leftDistance = getDistance(trigPin1, echoPin1);
    delay(5);
    long rightDistance = getDistance(trigPin2, echoPin2);
    delay(5);
    Serial.println(leftDistance);

    if (sent == 0) {
      Serial.println(2);
      sent = 1;
    }
    if ((frontDistance > 0 && frontDistance < distanceThreshold) || (frontLeftDistance > 0 && frontLeftDistance < distanceThreshold2) || (frontRightDistance > 0 && frontRightDistance < distanceThreshold2)) {
      sent = 0;
      Serial.println(4);
      while (leftDistance < 15 && rightDistance < 15) {
        leftDistance = getDistance(trigPin1, echoPin1);

        delay(10);
        rightDistance = getDistance(trigPin2, echoPin2);

        delay(10);
        Serial.println(8);
        delay(60);
      }
      Serial.println(4);

      if (rightDistance < leftDistance) {
        Serial.println(6);  // Turn right
      } else {
        Serial.println(7);  // Turn left
      }

      delay(450);         // Small delay before sending the next signal
      Serial.println(2);  // Move forward

      if (frontDistance >= distanceThreshold) {
        Serial.println(2);  // Move forward
      }
      delay(5);  // Adjust the delay based on your needs
    }
  } else {
    digitalWrite(led, LOW);
  }
}

long getDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  duration = pulseIn(echoPin, HIGH, timeout);
  if (duration == 0) {
    return 999;  // Timeout - no echo
  }
  distance = (duration * 0.0343) / 2;
  return distance;
}

so it will just serial print the leftdistance value and it is still 999.

[jremington]

@memind

what is "Arduino free memory" are you for real? I searched for it and nothing came.

[xfpd]

@memind

const int echoPin1 = 5; // Left

@memind

const int led = 5;

Verify your hardware...

[memind]

Hello, I checked the memory and it was more than enough.

[memind]

Yes, thank you. Now the left sensor works. But i realized if any of the sensors read more than 335 cm it displays as 999 (which means the sensor isn't functioning properly)

[xfpd]

Remove all but the five SR04s. Make them return good values. Then introduce the other parts of your project. Here is an example of five SR04s, with an interval between each reading. You could change the intervals by making an array of interval values. You could also move the timer into its own function, outside loop().

SR04x5 - Wokwi ESP32, STM32, Arduino Simulator