           closeLastTime = 0;
         }
       }
-      printinfo(doorAngle, ping_1, ping_2);
-    }
-
-    updateBleCharacteristic();
+      printInfoIfDebug(doorAngle, ping_1, ping_2);
+    }
+
+    // Only bother updating bluetooth data if a device is connected
+    if (connected) {
+      updateBleCharacteristic();
+    }
+
+    // CURRENT CONTROL SCHEME: left and right buttons make the entering direction in the orientation specified by the button.
+    // select buttons always toggles the orientation.
+    uint8_t buttons = lcd.readButtons();
+    if (buttons) {
+      if (buttons & BUTTON_LEFT) {
+        if (enterFromLeftToRight) {
+          swapSonars();
+        }
+      }
+      if (buttons & BUTTON_RIGHT) {
+        if (!enterFromLeftToRight) {
+          swapSonars();
+        }
+      }
+      if (buttons & BUTTON_SELECT) {
+        swapSonars();
+      }
+    }
 
     // increment previous time, so we keep proper pace
     microsPrevious = microsPrevious + microsPerReading;
   }
+}
+
+// Flip the orientation of the sensors
+void swapSonars() {
+  NewPing temp = closeSonar;
+  closeSonar = farSonar;
+  farSonar = temp;
+  enterFromLeftToRight = !enterFromLeftToRight;
+  String message = enterFromLeftToRight ? "Enter this way->" : "<-Enter this way";
+  displayMessage(message);
 }
 
 void updateBleCharacteristic() {

  microsLastRead = micros();
}

// from tutorial
float convertRawAcceleration(int aRaw) {
  // since we are using 2G range
  // -2g maps to a raw value of -32768
  // +2g maps to a raw value of 32767

  float a = (aRaw * 2.0) / 32768.0;
  return a;
}

// from tutorial
float convertRawGyro(int gRaw) {
  // since we are using 250 degrees/seconds range
  // -250 maps to a raw value of -32768
  // +250 maps to a raw value of 32767

  float g = (gRaw * 250.0) / 32768.0;
  return g;
}

// did we get a detection?  boolean logic goes here to avoid doing >0 checks (which indicate that the ping was too far away)

  }
}

// wrap all Serial.prints using this function for ease of debugging
void printIfDebug(String message) {
  if (debug) {
    Serial.print(message);
  }
}

// wrap all Serial.printlns using this function for ease of debugging
void printlnIfDebug(String message) {
  if (debug) {
    Serial.println(message);
  }
}

// from the tutorial -- don't worry too much about this.
float measureYaw() {
  int aix, aiy, aiz;
  int gix, giy, giz;
  float ax, ay, az;
  float gx, gy, gz;
  // read raw data from CurieIMU
  CurieIMU.readMotionSensor(aix, aiy, aiz, gix, giy, giz);

  // convert from raw data to gravity and degrees/second units
  ax = convertRawAcceleration(aix);
  ay = convertRawAcceleration(aiy);
  az = convertRawAcceleration(aiz);
  gx = convertRawGyro(gix);
  gy = convertRawGyro(giy);
  gz = convertRawGyro(giz);

  // update the filter, which computes orientation
  filter.updateIMU(gx, gy, gz, ax, ay, az);

  return filter.getYaw();
}

// from tutorial
float convertRawAcceleration(int aRaw) {
  // since we are using 2G range
  // -2g maps to a raw value of -32768
  // +2g maps to a raw value of 32767

  float a = (aRaw * 2.0) / 32768.0;
  return a;
}

// from tutorial
float convertRawGyro(int gRaw) {
  // since we are using 250 degrees/seconds range
  // -250 maps to a raw value of -32768
  // +250 maps to a raw value of 32767

  float g = (gRaw * 250.0) / 32768.0;
  return g;
}
