         distance += 1;
         if (distance > ((3 * WIDTH / 2) + (WIDTH * state))) {
             state += 1;
-            // TODO: make motors go in the right direction
+            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: periodic tasks go here. Called at 1kHz.
+    // 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) % 6;
+    adc_pin = (adc_pin + 1) % 3;
 
     // Start the next ADC
     ADMUX &= B11110000; ADMUX |= adc_pin; ADCSRA |= B01000000;

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


 *
 */

#define DEBUG

// Analog pin defines
#define RIGHTLEFT 0
#define TOPBOTTOM 1
#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 NE 4
#define NW 5
#define SE 6
#define SW 7
// NOT_DEFINED 8
#define LR_SP 9  // pwm
#define B_SP 10 // pwm
#define T_SP 11 // pwm
#define TB_DIR 12
#define B_EN 13

// Constant defines
#define HIGH_THRESHOLD 16000
#define LOW_THRESHOLD 14000
#define MAX_DEFECT_POINTS 1000
#define SPEED 192
#define STALL_TORQUE 128
#define LR_OFFSET 64
#define OFFSET 10

#define LEFT LOW
#define RIGHT HIGH

volatile int state = 0;
volatile int distance = 0;
volatile int defect_array_index;

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

volatile int defect_points[8][MAX_DEFECT_POINTS];

void setup () {
    // PWM
