     widths[4] = 9*WIDTH/2;
     widths[5] = 11*WIDTH/2;
     
-    curr_width = widths[(state/18)%3];
+    // Set the width and distance of the first step.
+    curr_width = widths[0];
     next_dist = curr_width - BUFFER - BUFFER;
 
     // Encoders
     attachInterrupt(1, en_TB, RISING);
     attachInterrupt(0, en_LR, RISING);
 
+    // Enable interrupts
     sei();
 }
 
 void loop() {
     int sta = (state % 18);
     int st = sta / 3;
+    
+    // Choose the relevant correct encoder value
     int distance = distance_TB;
+    // Moving up or down
     if (st % 3 == 1) distance = distance_LR;
     
+    // If you are in the detecting state and an edge is detected, always go to the next state.
     if (    ((state % 9  ==  8) && (adc_val[LEFTRIGHT] > HIGH_THRESHOLD_LR)) ||
             ((state % 18 ==  5) && (adc_val[TOPBOTTOM] <  LOW_THRESHOLD_TB)) ||
-            ((state % 18 == 14) && (adc_val[TOPBOTTOM] > HIGH_THRESHOLD_TB)))
+            ((state % 18 == 14) && (adc_val[TOPBOTTOM] > HIGH_THRESHOLD_TB))) {
         next_dist = 0;
-        
+        Serial.print("EDGE DETECTED");
+    }
+    
+    // It is time to go to the next state
     if (distance > next_dist) {
         state++;
+        
         sta = (state % 18);
         st = sta / 3;
+        
+        // Set a new desired width when switching to a new pass.
         if (state % 9 == 0) curr_width = widths[(state/9)%6];
         
+        // Reset the encoder values when you change direction
         if (state % 3 == 0) { distance_TB = 0; distance_LR = 0; }
         
+        // Reset the motor speeds to stationary
         pwm_out[L2] = 255;
         pwm_out[R2] = 255;
         pwm_out[LR1] = 240;

        pwm_out[TB2] = 0;
        
        switch (st) {
            case 0: case 3: 
                pwm_out[TB2] = 80;
            break;
            case 2: case 5: 
                pwm_out[TB1] = 60;
            break;
            case 1:
                pwm_out[LR1] = 255;
                pwm_out[L2] = 240;
                pwm_out[R2] = 240;
                break;
            case 4:
                pwm_out[LR1] = 100;
                break;
        }
        
        analogWrite(TB1, pwm_out[TB1]);
        analogWrite(TB2, pwm_out[TB2]);
        analogWrite(R2, pwm_out[R2]);
        analogWrite(L2, pwm_out[L2]);
        analogWrite(LR1, pwm_out[LR1]);
        
        next_dist = curr_width;
        if (st == 1) next_dist = DOWNHEIGHT;
        if (st == 4) next_dist = UPHEIGHT;
        if (state % 3 == 0) next_dist -= BUFFER + BUFFER;
        if (state % 3 == 1) next_dist -= BUFFER;
        if ((state % 9 == 5) && (state % 9 == 8)) next_dist += SLACK;
    }
    
        delayMicroseconds(25000); 
        digitalWrite(13, HIGH);
        delayMicroseconds(25000);
        digitalWrite(13, LOW);
        
    printCtr = (printCtr + 1) % 20;
    
    if (printCtr == 0) {
        int xPos, yPos;
        if ((st % 3) == 1) {
            xPos = curr_width / 100;
            if (st < 3) yPos = map(distance_LR, 0, DOWNHEIGHT, 28, 0);
            else yPos = map(distance_LR, 0, UPHEIGHT, 0, 28);
        }
        else {
            if ((st == 0) || (st == 5)) yPos = 28;
            else yPos = 0;
            if ((st % 3) == 0) xPos = distance_TB / 100;
            else xPos = (curr_width - distance_TB) / 100;
        }
        Serial.print("\nState: ");
        Serial.print(state);
        Serial.print(" x: ");

    }
}

// Called when an analog-digital conversion completes
ISR(ADC_vect)
{
    // Record and amplify the value.
    unsigned long analogVal = (ADCL | (ADCH << 8)) << 4;
    
    // Add the new value to the rolling exponential buffer for that pin.
    adc_val[adc_pin] += (analogVal >> 6) - (adc_val[adc_pin] >> 6);
    
    // Set the next ADC conversion to happen.
    adc_pin = (adc_pin + 1) % 6;
    
    // Start the next analog-digital conversion
    ADMUX &= B11110000; ADMUX |= adc_pin; ADCSRA |= B01000000;
}
