 }
 
 void loop() {
-    // TODO: agregate defect data after up/down pass
+    // TODO: Later. Agregate defect data after up/down pass.
+#ifdef DEBUG
+    delay(500);
+    if (digitalRead(NE) == HIGH) printf("NE Defect\n");
+    if (digitalRead(NW) == HIGH) printf("NW Defect\n");
+    if (digitalRead(SE) == HIGH) printf("SE Defect\n");
+    if (digitalRead(SW) == HIGH) printf("SW Defect\n");
+#endif
 }
 
 void enUD() {
-    // TODO: Only needed as a sanity check, will implement later
+    // TODO: Later. Add sanity check.
+
+    // Take note of defects.
+    if ((defect_array_index < MAX_DEFECT_POINTS) && (stage % 3 == 1)) {
+        defect_points[NE][defect_array_index] = digitalRead(NE);
+        defect_points[NW][defect_array_index] = digitalRead(NW);
+        defect_points[SE][defect_array_index] = digitalRead(SE);
+        defect_points[SW][defect_array_index] = digitalRead(SW);
+    }
 }
 
 void enLR() {

    adc_pin = 0;
    ADMUX |= B01000000; ADMUX &= B11010000; ADCSRA |= B11101111;
    
    // Motors
    pinMode(L_EN, OUTPUT); pinMode(L_SP, OUTPUT);
    pinMode(R_EN, OUTPUT); pinMode(R_SP, OUTPUT);
    pinMode(T_EN, OUTPUT); pinMode(T_SP, OUTPUT);
    pinMode(B_EN, OUTPUT); pinMode(B_SP, OUTPUT);
    pinMode(L_E1, OUTPUT); pinMode(T_E1, OUTPUT);

    attachInterrupt(0, enUD, CHANGE);
    attachInterrupt(1, enLR, CHANGE);


    adc_val[curr_adc_pin] += (analogVal >> 2) - (adc_val[curr_adc_pin] >> 2);

    // Update things that use ADC values
    
    // Moving up/down
    if ((curr_adc_pin == TOPBOTTOM) && 
           (((state % 6 == 1) && (adc_val[curr_adc_pin] <  LOW_THRESHOLD)) ||
            ((state % 6 == 4) && (adc_val[curr_adc_pin] > HIGH_THRESHOLD)))) {
        state++;
        analogWrite(LR_SP, STALL_TORQUE);
        analogWrite(B_SP, SPEED);
        analogWrite(T_SP, SPEED);
        digitalWrite(TB_DIR, LEFT);
        digitalWrite(B_EN, HIGH);
    }
    // Moving Left
    else if (((curr_adc_pin == LEFTRIGHT) && (state % 3 == 2)) &&
             (adc_val[curr_adc_pin] < LOW_THRESHOLD)) {
        state++;
        digitalWrite(TB_DIR, RIGHT);
    }
}

