 }
 
 // Debug: Show current commands on lcd
-*/
 void lcd_command() {
-  lcd.clear();
-  lcd.setCursor(0,0);
-  lcd.print(
-          "Y"+String(cmd.yaw_pwm)+" "+String(cmd.yaw_direction)
-          +" L"+String(cmd.lid_pwm)+" "+String(cmd.lid_direction)
-          );
-}
-*/
+  if (debug == 1){
+     lcd.clear();
+     lcd.setCursor(0,0);
+     lcd.print(
+             "Y"+String(cmd.yaw_pwm)+" "+String(cmd.yaw_direction)
+             +" L"+String(cmd.lid_pwm)+" "+String(cmd.lid_direction)
+             );
+        }
+} 
+
 
 // Send command to the motors
 void do_command() {

      stringComplete = true;
    } 
  }
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print(inputString);

}

  Serial.println(statusString);
}

/*
  SerialEvent occurs whenever a new data comes in the
 hardware serial RX.  This routine is run between each
 time loop() runs, so using delay inside loop can delay
 response.  Multiple bytes of data may be available.
 */
 
void serialEvent() {
  while (Serial.available()) {
    // get the new byte:
