     adc_val[curr_adc_pin] += (analogVal >> 2) - (adc_val[curr_adc_pin] >> 2);
 
     // Update things that use ADC values
-    switch (curr_adc_pin) {
-        case TOPBOTTOM:
-            switch (state) {
-                case 0: // Moving right at top
-                    // TODO: TB ADC
-                break;
-                case 1: // Moving down
-                    // TODO: TB ADC
-                break;
-                case 2: // Moving left at bottom
-                    // TODO: TB ADC
-                break;
-                case 3: // Moving right at bottom
-                    // TODO: TB ADC
-                break;
-                case 4: // Moving up
-                    // TODO: TB ADC
-                break;
-                case 5: // Moving left at top
-                    // TODO: TB ADC
-                break;
-            }
-            break;
-        case LEFTRIGHT:
-            switch (state) {
-                case 0: // Moving right at top
-                    // TODO: LR ADC
-                break;
-                case 1: // Moving down
-                    // TODO: LR ADC
-                break;
-                case 2: // Moving left at bottom
-                    // TODO: LR ADC
-                break;
-                case 3: // Moving right at bottom
-                    // TODO: LR ADC
-                break;
-                case 4: // Moving up
-                    // TODO: LR ADC
-                break;
-                case 5: // Moving left at top
-                    // TODO: LR ADC
-                break;
-            }
-            break;
-        default:
-            if (state % 3 == 1) {
-                if (defect_array_index < MAX_DEFECT_POINTS) {
-                    defect_points[curr_adc_pin][defect_array_index] = analogVal;
-                }
-                if (curr_adc_pin == 3)
-                    defect_array_index++;
-            }
+    
+    // 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);
     }
 }
 

    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);


        distance += 1;
        if (distance > ((3 * WIDTH / 2) + (WIDTH * state))) {
            state += 1;
            digitalWrite(B_EN, LOW);
            analogWrite(T_SP, 0);
            analogWrite(B_SP, 0);
            if (state % 6 == 1) analogWrite(LR_SP, STALL_TORQUE - LR_OFFSET);
            else                analogWrite(LR_SP, STALL_TORQUE + LR_OFFSET);
        }
    }
}

ISR(TIMER0_COMPA_vect) {
    // TODO: Later. Add tilt correction.
    // TODO: Add height correction while moving along top and bottom.
    // TODO: make corrections use PID
}

ISR(ADC_vect)
{
    int analogVal = (ADCL | (ADCH << 8)) << 4;
    int curr_adc_pin = adc_pin;
    adc_pin = (adc_pin + 1) % 3;

    // Start the next ADC
    ADMUX &= B11110000; ADMUX |= adc_pin; ADCSRA |= B01000000;
