 void writeOutput()
 {
     if (DEBUG_MODE) 
-    {
-        if (VERBOSE_DEBUG) 
-        {
-            Serial.print("Round: ");
-            Serial.print(roundCount);
-            Serial.print(" micros: ");
-            Serial.print(micros());
-            Serial.print(", micros since last: ");
-            Serial.print(micros() - timeSinceLastRound);
-            timeSinceLastRound = micros();
-            Serial.print(", SigReceived: ");
-            Serial.print(SignalsReceived);
-            Serial.print(", isr1: ");
-            Serial.print(isr1Count);
-            Serial.print(", isr2: ");
-            Serial.print(isr2Count);
-            Serial.print(", isr3: ");
-            Serial.println(isr3Count);
-            Serial.print(" Vals: ");
-        }
-
+    { 
         for (int index = 1; index < RANGE_DATA_SIZE; index++)
         {
-            //Prints out only the sonars for one board (9, 10, 11, 12, 1, 2, 3)
-            if (boardCount == 1 && (index == 4 || index == 5 || index == 7 || index == 8)) { continue; }
-
-            if (index < 10) { Serial.print("[ "); }
-            else { Serial.print('['); }
+            if (VERBOSE_DEBUG)
+            {
+                //Prints out only the sonars for one board (9, 10, 11, 12, 1, 2, 3)
+                if (index == 4 || index == 5 || index == 7 || index == 8) { continue; }
+
+                Serial.print(F("Round: "));     Serial.print(roundCount);
+                Serial.print(F(" micros: "));   Serial.print(micros());
+               
+                Serial.print(F(", micros since last: "));
+                Serial.print(micros() - timeSinceLastRound);
+                timeSinceLastRound = micros();
                 
-            Serial.print(index);
-            Serial.print("] ");
-            Serial.println(range[index]);
+                Serial.print(F(", SigReceived: ")); Serial.print(SignalsReceived);
+                Serial.print(F(", isr1: "));        Serial.print(isr1Count);
+                Serial.print(F(", isr2: "));        Serial.print(isr2Count);
+                Serial.print(F(", isr3: "));        Serial.println(isr3Count);
+
+                if (index < 10) { Serial.print(F("[ ")); }
+                else { Serial.print(F("[")); }
+
+                Serial.print(index);
+                Serial.print(F("]\t"));
+
+                for (int position = 0; position < SAMPLE_DATA_SIZE; position++)
+                {
+                    Serial.print(range[index][position]);
+                    Serial.print(F("\t"));
+                }
+                Serial.print(F("\t"));
+            }
+           
+            // sortArray(range, index, SAMPLE_DATA_SIZE);   //Bubble Sort
+            quicksort(range, index, 0, SAMPLE_DATA_SIZE);   //Quicksort
+
+            Serial.print(findMode(range, index, SAMPLE_DATA_SIZE));
+            
+            if (VERBOSE_DEBUG) { Serial.println(); }
+            else { Serial.print(F(" ")); }
+            
             // Serial.println(analogRange[index]);
         }
         Serial.println();
     }
+
+// =============================================================================
+// TODO -- Below is where data will be sent over SPI.
+// =============================================================================
     else 
     {
-        //Here is where data will be sent over SPI. Still needs work
         int dataPos = 0;
         processIt = false;
         while (dataPos < RANGE_DATA_SIZE) 


    Round(AB)   Output     Sonars
    00          Y0         none
    01          Y1         12, 3, 9
    11          Y2         1, 10     // original design included 6
    10          Y3         2, 11, 6  // 6 not in original design

    A round may fire up to 3 sonars. The sonar range may be reported as either an analog value
    or as a pulse width. A pulse width will cause an interrupt.



// -------------------------------------------------------------------------------------------------
void ISRLeft()
{  
    //ISR for end of pulse on sonar 9, 10 or 11    
    noInterrupts();

    isr1Count++;

    //if (digitalRead(PW_OR3) == HIGH && timeStartSet == false)
    if (ibh(PINE, PE6) && !timeStartSet)
    {
        timeStart = micros();        
        timeStartSet = true;
