Please help me with my code: Sonar + LCD menu

If this is a 'general question' then please move.

Ok, I am new to Arduino but open to constructive criticism. I've been working on this for quite a bit. The overall goal for this project is a collision avoidance system for a golf cart. I am using three sonar sensors and an LCD with keypad for a display. I want the LCD to display in real time the speed of the cart and the current charge state of the golf cart. The LCD will also display a menu to change a few options: speed units, warning buzzer on and off, automatic brakes on and off (haven't made it that far into the project but went ahead and built this option into the menu).

At this point I'm still in the prototyping stage. I'm simulating the speed and voltage with two potentiometers.

I started off with two separate programs, one for the sensors and warning system and the other for the LCD. I'm to the point where I am putting the two programs together and now I'm having some issues. With the way I have the program written now, the LCD doesn't display in real time. I'm able to see the main display, and I'm able to go into the menu to change units of speed and turn the buzzer on and off, but the voltage reading isn't real time. It only updates when I leave the menu to go back to the display.

I think the issue is the display doesn't print in the loop, it prints in a separate function. Adding that function to the loop causes all kinds of issues. The LCD doesn't display correctly, I believe it is having issues keeping up with the loop. Adding a delay to the loop fixes this, but it throws off the sonar sensors. Adding the function to the loop also does not allow me to visit the menu to change settings, as it is always displaying the main display.

I'll try to post my code, may have to separate it into a couple posts due to length. Let me know if you see any simple fixes that do not require me to completely rewrite my code. If that's the case, I'll probably cry. It has taken weeks for me to get to this point.

#include <NewPing.h> //Library for sonar

// include the library for LCD:
#include <Wire.h>
#include <Adafruit_RGBLCDShield.h>
#include <utility/Adafruit_MCP23017.h>
Adafruit_RGBLCDShield lcd = Adafruit_RGBLCDShield();

#define SONAR_NUM      3  //number of sonar sensors
#define MAX_DISTANCE 300  //max distance needed to read
#define PING_INTERVAL 37  //ping interval in ms

//setting pins for the LEDs
#define redLed 10 
#define yellowLed 9
#define greenLed 8

//setting analog inputs for speed and voltage
#define speedInput A0
#define voltageInput A1

#define buzzPin 11 //pin for buzzer

int speedValue;  //variable for the "speed reading" from potentiometer

int voltageValue;  //declaring variable for the voltage

//set up display options
String spd = "Speed";
String voltage = "Voltage";
String yardage = "Yardage";
String mph = "MPH";
String kph = "KPH";
String fps = "FPS";
String mps = "MPS";
String volt = "Volts";
String yard = "Yards";
String on = "ON";
String off = "OFF";
  

//set up menu options
String menu_1 = "Menu: Units";
String menu_2 = "Menu: Buzzer";
String menu_3 = "Menu: Brakes";

//set defaults for LCD
String current_units = mph;
String current_brake = on;
String current_buzzer = on;
String line1 = " sp " + current_units + "  Voltage";
String line2 = "yrd " + yard + " " + voltageValue;

  
//Default options
int menu = 0;
int unit_select = 1;
int buzz_select = 1;
int brake_select = 1;

unsigned long pingTimer[SONAR_NUM];
uint8_t currentSensor = 0;

NewPing sonar[SONAR_NUM] = {
  NewPing(5, 4, MAX_DISTANCE),  //trigger pin, echo pin
  NewPing(7, 6, MAX_DISTANCE),
  NewPing(3, 2, MAX_DISTANCE)
};

void setup() {
  Serial.begin(115200);
  pinMode(redLed, OUTPUT);
  pinMode(yellowLed, OUTPUT);
  pinMode(greenLed, OUTPUT);
  pinMode(buzzPin, OUTPUT);
  pingTimer[0] = millis() + 75;
  for (uint8_t i = 1; i < SONAR_NUM; i++)
    pingTimer[i] = pingTimer[i - 1] + PING_INTERVAL;

  lcd.begin(16,2);        // '16,2' is 16 columns, 2 rows on the display

  //Welcome screen
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("Welcome, please");
  lcd.setCursor(3,1);
  lcd.print("drive safe");

  delay(2000);

  mainDisplay();
}

void loop() {
  for (uint8_t i = 0; i < SONAR_NUM; i++) {
    if (millis() >= pingTimer[i]) {
      pingTimer[i] += PING_INTERVAL * SONAR_NUM;
      sonar[currentSensor].timer_stop();
      currentSensor = i;
      sonar[currentSensor].ping_timer(echoCheck);
    }
  }

  //changes speed/voltage reading from 0-5V to 0-1023
  speedValue = analogRead(speedInput);
  voltageValue = analogRead(voltageInput);




  //Begin LCD************************************************************
  uint8_t buttons = lcd.readButtons();

  if (buttons) {

    
  
    screenRef();


    if (buttons & BUTTON_SELECT && menu < 1) {
      menu1Display();
      menu = 1;
    }

    else if (buttons & BUTTON_SELECT && menu > 0) {
      mainDisplay();
      menu = 0;
    }
  
    else if (buttons & BUTTON_RIGHT && menu == 1 && unit_select == 1) {
      unit_select = 2;
      current_units = kph;
      menu1Display();
    }

    else if (buttons & BUTTON_RIGHT && menu == 1 && unit_select == 2) {
      unit_select = 3;
      current_units = fps;
      menu1Display();
    }

    else if (buttons & BUTTON_RIGHT && menu == 1 && unit_select == 3) {
      unit_select = 4;
      current_units = mps;
      menu1Display();
    }

    else if (buttons & BUTTON_RIGHT && menu == 1 && unit_select == 4) {
      unit_select = 4;
      current_units = mps;
      menu1Display();
    }

    else if (buttons & BUTTON_LEFT && menu == 1 && unit_select == 4) {
      unit_select = unit_select - 1;
      current_units = fps;
      menu1Display();
    }

    else if (buttons & BUTTON_LEFT && menu == 1 && unit_select == 3) {
      unit_select = unit_select - 1;
      current_units = kph;
      menu1Display();
    }

    else if (buttons & BUTTON_LEFT && menu == 1 && unit_select == 2) {
      unit_select = unit_select - 1;
      current_units = mph;
      menu1Display();
    }

    else if (buttons & BUTTON_LEFT && menu == 1 && unit_select == 1) {
      unit_select = 1;
      current_units = mph;
      menu1Display();
    }

    else if (buttons & BUTTON_DOWN && menu == 1) {
      menu = 2;
      menu2Display();
    }

    else if (buttons & BUTTON_RIGHT && menu == 2 && buzz_select == 1) {
      buzz_select = 2;
      current_buzzer = off;
      menu2Display();
    }

    else if (buttons & BUTTON_RIGHT && menu == 2 && buzz_select == 2) {
      buzz_select = 2;
      current_buzzer = off;
      menu2Display();
    }

    else if (buttons & BUTTON_RIGHT && menu == 2 && buzz_select == 2) {
      buzz_select = 1;
      current_buzzer = on;
      menu2Display();
    }
 else if (buttons & BUTTON_RIGHT && menu == 2 && buzz_select == 1) {
      buzz_select = 1;
      current_buzzer = on;
      menu2Display();
    }

    else if (buttons & BUTTON_UP && menu == 2) {
      menu1Display();
      menu = 1;
    }

    else if (buttons & BUTTON_DOWN && menu == 2) {
      menu3Display();
      menu = 3;
    }

    else if (buttons & BUTTON_RIGHT && menu == 3 && brake_select == 1) {
      brake_select = 2;
      current_brake = off;
      menu3Display();
    }

    else if (buttons & BUTTON_RIGHT && menu == 3 && brake_select == 2) {
      brake_select = 2;
      current_brake = off;
      menu3Display();
    }

    else if (buttons & BUTTON_LEFT && menu == 3 && brake_select == 2) {
      brake_select = 1;
      current_brake = on;
      menu3Display();
    }

    else if (buttons & BUTTON_RIGHT && menu == 3 && brake_select == 1) {
      brake_select = 1;
      current_brake = on;
      menu3Display();
    }

    else if (buttons & BUTTON_UP && menu == 3) {
      menu2Display();
      menu = 2;
    }

    else if (buttons & BUTTON_DOWN && menu == 3) {
      menu1Display();
      menu = 1;
    }

    else if (buttons & BUTTON_UP && menu == 1) {
      menu3Display();
      menu = 3;
    }
  }
  //End LCD***************************************************************************
}

void echoCheck() {
  if (sonar[currentSensor].check_timer())
    pingResult(currentSensor, sonar[currentSensor].ping_result / US_ROUNDTRIP_CM);
}

void pingResult(uint8_t sensor, int cm) {

 //Turns on the green LED to insure system is on
  digitalWrite(greenLed, HIGH);

  //For low speeds
  if (speedValue < 341){
    if (cm < 30){
      digitalWrite(yellowLed, HIGH);
    }
    
    else {
      digitalWrite(yellowLed, LOW);
    }
    
    if (cm <15) {
      digitalWrite(redLed, HIGH);
      if (buzz_select == 1) {
        digitalWrite(buzzPin, HIGH);
      }
    }
    
    else {
      digitalWrite(redLed, LOW);
      digitalWrite(buzzPin, LOW);
    }
  }

  //For high speeds
   if (speedValue > 682){
    if(cm < 90){
      digitalWrite(yellowLed, HIGH);
    }
    
    else {
      digitalWrite(yellowLed, LOW);
    }
    
    if (cm <45) {
      digitalWrite(redLed, HIGH);
      if (buzz_select == 1) {
        digitalWrite(buzzPin, HIGH);
      }
    }
    
    else {
      digitalWrite(redLed, LOW);
      digitalWrite(buzzPin, LOW);
    }
  }

  //For medium speeds
  if ((speedValue > 341) && (speedValue < 682)){
    if (cm < 60){
      digitalWrite(yellowLed, HIGH);
    }
    
    else {
      digitalWrite(yellowLed, LOW);
    }
    
    if (cm <30) {
      digitalWrite(redLed, HIGH);
      if (buzz_select == 1) {
        digitalWrite(buzzPin, HIGH);
      }
    }
    
    else {
      digitalWrite(redLed, LOW);
      digitalWrite(buzzPin, LOW);
    }
  }
  
  Serial.print(sensor);
  Serial.print(" ");
  Serial.print(cm);
  Serial.print("cm ");
  Serial.print("speed ");
  Serial.print(speedValue);
  Serial.print(" voltage ");
  Serial.println(voltageValue);
  
}

void mainDisplay() {

  line1 = " sp " + current_units + "  Voltage";
  line2 = "yrd " + yard + " " + speedValue;
  
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print(line1);
  lcd.setCursor(0,1);
  lcd.print(line2);
}

void menu1Display() {
  
  line1 = menu_1;
  line2 = current_units;

  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print(line1);
  lcd.setCursor(0,1);
  lcd.print(line2);
}

void menu2Display() {
  
  line1 = menu_2;
  line2 = current_buzzer;

  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print(line1);
  lcd.setCursor(0,1);
  lcd.print(line2);
}

void menu3Display() {
  
  line1 = menu_3;
  line2 = current_brake;

  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print(line1);
  lcd.setCursor(0,1);
  lcd.print(line2);
}

void screenRef() {
  
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print(line1);
  lcd.setCursor(0,1);
  lcd.print(line2);
}

You have an awful lot of Strings that should be strings, wasting memory unnecessarily.

I think the issue is the display doesn't print in the loop, it prints in a separate function.

You need to describe the problem in terms of function names in your code. What I think this statement means is "I think the issue is that changes to the LCD only happen in mainDisplay(), which I only call from loop() when...", which, to me, means that you aren't calling mainDisplay() under ALL the right circumstances.

If you meant something else, feel free to correct my guess-work of what you meant.

PaulS: You have an awful lot of Strings that should be strings, wasting memory unnecessarily.

I'm new to this. From what I have coded so far I have pulled from other programs. I have them as strings because that's how the were in the other codes that I used for reference. What else can I do other than declare the strings and call them when needed?

PaulS: You need to describe the problem in terms of function names in your code. What I think this statement means is "I think the issue is that changes to the LCD only happen in mainDisplay(), which I only call from loop() when...", which, to me, means that you aren't calling mainDisplay() under ALL the right circumstances.

If you meant something else, feel free to correct my guess-work of what you meant.

You honestly lost me here. Not sure what you are meaning.