         pwm_out[TB2] = 0;
         
         switch (st) {
+            // Moving right.
             case 0: case 3: 
                 pwm_out[TB2] = 80;
             break;
+            
+            // Moving Left.
             case 2: case 5: 
                 pwm_out[TB1] = 60;
             break;
+            
+            // Moving down.
             case 1:
                 pwm_out[LR1] = 255;
                 pwm_out[L2] = 240;
                 pwm_out[R2] = 240;
-                break;
+            break;
+                
+            // Moving up.
             case 4:
                 pwm_out[LR1] = 100;
-                break;
+            break;
         }
         
+        // Write the speeds.
         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]);
         
+        // Set next_dist according to the current state and width
+        // Default moving left or right.
         next_dist = curr_width;
+        
+        // Moving down. 
         if (st == 1) next_dist = DOWNHEIGHT;
+        
+        // Moving up.
         if (st == 4) next_dist = UPHEIGHT;
+        
+        // Moving fast. Change states well before the destination.
         if (state % 3 == 0) next_dist -= BUFFER + BUFFER;
+        
+        // Moving slow. Starts looking for edges when it is close.
         if (state % 3 == 1) next_dist -= BUFFER;
+        
+        // Set the failsafe in case it misses the edge.
         if ((state % 9 == 5) && (state % 9 == 8)) next_dist += SLACK;
     }
     
-        delayMicroseconds(25000); 
-        digitalWrite(13, HIGH);
-        delayMicroseconds(25000);
-        digitalWrite(13, LOW);
-        
+    // delay 50 ms and blink the light.
+    delayMicroseconds(25000); 
+    digitalWrite(13, HIGH);
+    delayMicroseconds(25000);
+    digitalWrite(13, LOW);
+        
+    // Only print every 20 cycles
     printCtr = (printCtr + 1) % 20;
     
     if (printCtr == 0) {
         int xPos, yPos;
+        // Moving up or down.
         if ((st % 3) == 1) {
             xPos = curr_width / 100;
+            // moving down.
             if (st < 3) yPos = map(distance_LR, 0, DOWNHEIGHT, 28, 0);
+            // Moving up.
             else yPos = map(distance_LR, 0, UPHEIGHT, 0, 28);
         }
+        // Moving left or right.
         else {
+            // At the top.
             if ((st == 0) || (st == 5)) yPos = 28;
+            // At the bottom
             else yPos = 0;
+            // Moving left.
             if ((st % 3) == 0) xPos = distance_TB / 100;
+            // Moving right.
             else xPos = (curr_width - distance_TB) / 100;
         }
+        
+        // Print the status.
         Serial.print("\nState: ");
         Serial.print(state);
         Serial.print(" x: ");

#define DEBUG

// Analog pin defines
#define LEFTRIGHT 1
#define TOPBOTTOM 0
#define ACCEL 2

// Digital pin defines
#define RX 0 // Reserved
#define TX 1 // Reserved
#define L_E1 2 // Interrupt
#define T_E1 3 // Interrupt
#define TB1 5
#define TB2 6
#define L2 9 // 4
#define R2 10 // 7
#define LR1 11 // 8
#define NE 9 
#define SW 10 
#define NW 11
#define SE 12

#define LEFT 0
#define RIGHT 1
#define UP 2
#define DOWN 3

// Constant defines
#define COLOR_SCALAR 10

#define HIGH_THRESHOLD_LR 10000L
#define LOW_THRESHOLD_LR 6000L
#define HIGH_THRESHOLD_TB 22000L
#define LOW_THRESHOLD_TB 20000L

#define MAX_DEFECT_POINTS 1000

#define BUFFER 100
#define WIDTH 400
#define UPHEIGHT 4600
#define DOWNHEIGHT 3600
#define SLACK 100

#define PWMRES 15
#define PWMFREQ 2

volatile int state = 0;
volatile int curr_width;
volatile int last_state_LR = 0;
volatile int last_state_TB = 0;
volatile int distance_LR = 0;
volatile int distance_TB = 0;
volatile int defect_array_index = 0;
volatile int next_dist;

volatile int adc_pin = 0;
volatile unsigned int widths[6];
volatile unsigned long adc_val[6] = {0,0,0,0,0,0};

volatile int defect_points[14][MAX_DEFECT_POINTS];

volatile int pwm_out[14] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};

volatile int pwmCtr = 0;

volatile int printCtr = 0;

void en_TB() { distance_TB++; }

void en_LR() { distance_LR++; }

void setup () {
    cli();
    Serial.begin(9600);
    // Timer
    //TCNT1  = 0; OCR1A = (PWMFREQ * 1) - 1; TCCR1A = (1 << WGM12);
    //TCCR1B = (1 << CS10); TIMSK1 |= (1 << OCIE1A);
    //TCNT2  = 0; OCR2A = 6                     ; TCCR2A = (1 << WGM21);
    //TCCR2B = (1 << CS22); TIMSK2 |= (1 << OCIE2A);
    
    // ADC
    adc_pin = 0;
    ADMUX |= B01000000; ADMUX &= B11010000; ADCSRA |= B11101111;
    
    // Defect sensing pins
    // pinMode(NE, INPUT); pinMode(NW, INPUT);
    // pinMode(SE, INPUT); pinMode(SW, INPUT);

    // Motors
    pinMode(LR1, OUTPUT); pinMode(R2, OUTPUT); pinMode(L2, OUTPUT);
    pinMode(TB1, OUTPUT); pinMode(TB2, OUTPUT);
    
    pinMode(13, OUTPUT);
    
    pwm_out[L2] = 255;
    pwm_out[R2] = 255;
    pwm_out[LR1] = 240;
    pwm_out[TB1] = 0;
    pwm_out[TB2] = 80;
        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]);
    distance_LR = 0;
    distance_TB = 0;
    
    state = 0;
    
    widths[0] = 1*WIDTH/2;
    widths[1] = 3*WIDTH/2;
    widths[2] = 5*WIDTH/2;

    }
}

// 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;
}
