This worked out pretty good, still working on making it into a function so I can have it move forward or backwards though the commands rather then resetting the board.
void loop() {
Serial.println("");// Clear the input field
Serial.println("ATI");//Send the 1st command to get things rolling
if (waitForString("Elm327 v1.3", 5000ul)) {Serial.println("AT AL"); }
//Using ANY part of this works, tested "Elm327 v1.3" -Works, "7 v.1" - Works, Elm326 calls the
//Else part seems to be working as intended
else {Serial.println("ATZ"); }//ATZ is reset of the shield
if (waitForString("OK", 5000ul)) {Serial.println("AT H1"); }
//Changed to AT H3(not a valid command) and it calls the Else block
else {Serial.println("ATZ"); }
if (waitForString("OK", 5000ul)) {Serial.println("AT MA"); }
else {Serial.println("ATZ"); }
delay(10000);
}