如何在Arduino上改进我的代码以提高响应速度？

Question

    #include <LiquidCrystal.h>
#include <AFMotor.h> //Adafruit motor
#include <SoftwareSerial.h> //Serial Communication
#include <Keypad.h> //Keypad 


//Setup pins;
SoftwareSerial lcd(16, 17);
 int RelayPin = 38;
const int CurrentPin = 39;

//Keypad Setup
const byte numRows= 4; //number of rows on the keypad
const byte numCols= 4; //number of columns on the keypad

//keymap defines the key pressed according to the row and columns just as appears on the keypad
char keymap[numRows][numCols]= 
{
{'1', '2', '3', 'A'}, 
{'4', '5', '6', 'B'}, 
{'7', '8', '9', 'C'},
{'*', '0', '#', 'D'}
};

//Code that shows the the keypad connections to the arduino terminals
byte rowPins[numRows] = {46,47,48,49}; //Rows 0 to 3
byte colPins[numCols]= {50,51,52,53}; //Columns 0 to 3

//initializes an instance of the Keypad class
Keypad myKeypad= Keypad(makeKeymap(keymap), rowPins, colPins, numRows, numCols);

//Toggle
int TogglePosition = 1;
char keypressed = myKeypad.getKey();

boolean MainMenuSelected = true;

void setup() {
  boolean MainMenuSelected = true;
  pinMode(RelayPin, OUTPUT);
  StartupLCD();
  toggle('X', MainMenuSelected);
}

void loop() {
  toggle(getKeyPad(), MainMenuSelected);

    Serial.begin(115200);
        AF_Stepper stepper(48, 2);
    while (MainMenuSelected == false){
        if (getKeyPad()=='A'){
    switch(digitalRead(RelayPin)){
      case HIGH: 
        digitalWrite(RelayPin, LOW);
      case LOW: 
        digitalWrite(RelayPin, HIGH);
    }}
      if(getKeyPad()=='2'){
        stepper.setSpeed(200);
        stepper.step(200, FORWARD, DOUBLE);
      }
            if(getKeyPad()=='8'){
        stepper.setSpeed(200);
        stepper.step(200, BACKWARD, DOUBLE);
      }
            if(getKeyPad()=='1'){
              MainMenuSelected = true;
      }

    }
      }


void StartupLCD(){
  lcd.begin(9600);  // Start the LCD at 9600 baud
  delay(10);
  clearDisplay();
  lcd.print("We are up and running!");
  delay(10);
  clearDisplay();
}

char getKeyPad(){
  char keypressed = myKeypad.getKey();
      if (keypressed != NO_KEY)
      {
        return keypressed;
      }
}

void toggle(char keypressed, boolean MainMenuSelected){
    if ((keypressed == 'X') && (MainMenuSelected == true)){
      PrintMenuLCD(TogglePosition);
    }
  if ((keypressed == '8') && (MainMenuSelected == true)){
      if(TogglePosition<5){
      TogglePosition = TogglePosition + 1;
      PrintMenuLCD(TogglePosition);
      }
  }
  if ((keypressed == '2') && (MainMenuSelected == true)){
      if(TogglePosition>2){
        TogglePosition = TogglePosition - 1;
        PrintMenuLCD(TogglePosition);
      }
  }
  if (keypressed == '5'){
    MainMenuSelected = false;
    PrintSubMenuLCD(TogglePosition);  
  }


  if (keypressed == '1'){
    MainMenuSelected = true;
    //PrintMenuLCD(TogglePosition);
    toggle('X',MainMenuSelected);
    digitalWrite(RelayPin, LOW);

  }

}

void PrintSubMenuLCD(int TogglePosition){
  if (TogglePosition == 2){
    lcd.print(" Manual Control ");
    lcd.print("                ");
    MainMenuSelected = false;
}

    if (TogglePosition == 3){
    lcd.print("> Outside Grind ");
    lcd.print("                ");
    MainMenuSelected = false;
}

    if (TogglePosition == 4){
    lcd.print("  Inside Grind  ");
    lcd.print("                ");
    MainMenuSelected = false;
}
  if (TogglePosition == 5){
    lcd.print("      Setup     ");
    lcd.print("                ");
    MainMenuSelected = false;
}
}



void PrintMenuLCD(int TogglePosition) {
  MainMenuSelected = true;
  if (TogglePosition == 1){
    lcd.print(">   Main Menu   ");
    lcd.print("1.Manual Control");
}
  if (TogglePosition == 2){
    lcd.print("    Main Menu   ");
    lcd.print("> Manual Control");
}

    if (TogglePosition == 3){
    lcd.print("> Outside Grind ");
    lcd.print("3.Inside Grind  ");
}

    if (TogglePosition == 4){
    lcd.print("2.Outside Grind ");
    lcd.print("> Inside Grind  ");
}
  if (TogglePosition == 5){
    lcd.print("> Setup         ");
    lcd.print("----------------");
}}


void clearDisplay(){
  lcd.write(0xFE);  // send the special command
  lcd.write(0x01);  // send the clear screen command
}

void setLCDCursor(byte cursor_position){
  lcd.write(0xFE);  // send the special command
  lcd.write(0x80);  // send the set cursor command
  lcd.write(cursor_position);  // send the cursor position
}

void setBacklight(byte brightness){
  lcd.write(0x80);  // send the backlight command
  lcd.write(brightness);  // send the brightness value
}

这是完整的代码，我怎样才能让它更快地运行...它目前是在一个不应该那么反应迟钝的arduino上。

任何帮助将不胜感激！

它不会让我发布我的代码而没有更多细节...所以我走了。我正在使用三个主要库。一个是Adafruit司机。

使用4x4键盘时建议的键盘库

MainMenuSelected旨在帮助区分主目录和子目录