 #define DETECTION_THRESH 50 // Distance cutoff for passing
 #define MAX_DISTANCE 200 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.
 
-NewPing sonar_1(TRIGGER_PIN_1, ECHO_PIN_1, MAX_DISTANCE); // NewPing setup of pins and maximum distance.
-NewPing sonar_2(TRIGGER_PIN_2, ECHO_PIN_2, MAX_DISTANCE); // NewPing setup of pins and maximum distance.
+NewPing sonar_1(TRIGGER_PIN_1, ECHO_PIN_1, MAX_DISTANCE); // NewPing setup of closer sensor.
+NewPing sonar_2(TRIGGER_PIN_2, ECHO_PIN_2, MAX_DISTANCE); // NewPing setup of farther sensor.
 
 // RGB
 rgb_lcd lcd;
 
 // MADGEWICK
 Madgwick filter;
-unsigned long microsPerReading, microsPrevious;
+unsigned long microsPerReading, microsPrevious, microsBetweenReads, microsLastRead;
 float accelScale, gyroScale;
 
 // THRESHOLDS
 const int thresh = 60;       // < 60 cm to sensor is considered passing through.
-const float openAngle = 10;  // door is considered open at 10 degrees.
+const float openAngle = 20;  // door is considered open at 20 degrees.
+boolean doorOpen = false;
 
 // INDICATOR
 const int ledPin =  13;      // the number of the LED pin
 
 // POPULATION
 int population = 0;
+unsigned long closeTrigger = 0, farTrigger = 0;
 
 // SETUP
 void setup() {

  // initialize variables to pace updates to correct rate
  microsPerReading = 1000000 / 25;
  microsPrevious = micros();

  // get initial angle of the door
  // startAngle = measureYaw();

    float difference = measureYaw() - 180;
    doorAngle = abs(difference);

    if (doorAngle < openAngle) {
      closeDoor();
      Serial.println("Door not open");
    } else if (microsNow - microsLastRead < microsBetweenReads) {
      Serial.println("Movement Detected");
    } else {
      openDoor();
      ping_1 = sonar_1.ping_cm();
      ping_2 = sonar_2.ping_cm();
  
      if (detect(ping_2)) {
        if (closeTrigger > 0) {
          incrementPopulation();
        } else if (!detect(ping_1)) {
          farTrigger = microsNow;
        }
      } else if (detect(ping_1)) {
        if (farTrigger > 0) {
          decrementPopulation();
        } else if (!detect(ping_2)) {
          closeTrigger = microsNow;
        }
      } else {
        if (farTrigger > 0 && microsNow - farTrigger > 2 * microsBetweenReads) {
          farTrigger = 0;
        } else if (closeTrigger > 0 && microsNow - closeTrigger > 2 * microsBetweenReads) {
          closeTrigger = 0;
        }
      }
      printinfo(doorAngle, ping_1, ping_2);
    }

    // increment previous time, so we keep proper pace
    microsPrevious = microsPrevious + microsPerReading;
