diff --git a/include/io.h b/include/io.h
index 013225e..a5ac84a 100644
--- a/include/io.h
+++ b/include/io.h
@@ -7,14 +7,9 @@
 #define LED_1 PD2
 
 /* Servo motors */
-#define SERVO_L PB1
-#define SERVO_R PD6
-
-/* Photo sensors */
-#define PH_1 PC1
-#define PH_2 PC2
-#define PH_3 PC3
-#define PH_4 PC4
-#define PH_5 PC5
+#define SERVO_L_PIN PB1
+#define SERVO_L OCR1A
+#define SERVO_R_PIN PB2
+#define SERVO_R OCR1B
 
 #endif
diff --git a/include/main.h b/include/main.h
index 792efdc..58be55a 100644
--- a/include/main.h
+++ b/include/main.h
@@ -1,15 +1,17 @@
 #ifndef MAIN_H
 #define MAIN_H
 
+#include <stdint.h>
+
 #define SET(PORT, PIN) (PORT |= (1 << PIN))
 #define CLR(PORT, PIN) (PORT &= (~(1 << PIN)))
 #define CHK(PORT, PIN) (PORT & (1 << PIN))
 #define TOG(PORT, PIN) (PORT ^= (1 << PIN))
 
-void initIO(void);
-void blinkLed(void);
-void readTemp(void);
-void readPhoto(void);
-void runServos(void);
+void init_led(void);
+void init_pwm(void);
+void read_temp(void);
+void run_servos(void);
+uint16_t read_adc(uint8_t channel);
 
 #endif
diff --git a/src/main.c b/src/main.c
index b5b8e14..d32bc87 100644
--- a/src/main.c
+++ b/src/main.c
@@ -10,21 +10,29 @@
 #include "debug.h"
 #endif /* DEBUG */
 
-void initIO()
+void init_pwm(void)
 {
-    /* Servos */
-    SET(DDRB, SERVO_L);
-    /* Phase Correct PWM, 9-bit; Inverting mode */
-    TCCR1A |= 1 << WGM11 | 1 << COM1A1 | 1 << COM1A0;
+    DDRB |= 1 << SERVO_L_PIN | 1 << SERVO_R_PIN;
+    /* Phase Correct PWM; Set/clear on Compare Match when down/up-counting */
+    TCCR1A |= 1 << WGM11 | 1 << COM1A1 | 1 << COM1B1;
     /*  "Clear Timer on Compare match" mode; Prescaler = 1 */
-    TCCR1B |= 1 << WGM13 | 1 << WGM12 | 1 << CS10;
-    ICR1 = 19999; // F_CPU / 50Hz - 1
-
-    /* LED */
-    SET(DDRD, LED_1);
+    TCCR1B |= 1 << WGM12 | 1 << WGM13 | 1 << CS10;
+    ICR1 = F_CPU / 50 - 1; // 50Hz required by servos
 }
 
-void readTemp()
+void init_led(void)
+{
+    DDRD |= 1 << LED_1;
+}
+
+void init_adc(void)
+{
+    ADCSRA |= 1 << ADPS0 | 1 << ADPS1; // Prescaler = 8 => ADC clock = 125Hz
+    ADMUX |= 1 << REFS0; // AVcc with external cap as reference voltage
+    ADCSRA |= 1 << ADEN | 1 << ADSC;
+}
+
+void read_temp(void)
 {
     double d;
 
@@ -40,27 +48,52 @@ void readTemp()
     }
 }
 
-void runServos()
+void run_servos(void)
 {
     /* Stop */
-    OCR1A = ICR1 - 1500;
+    SERVO_L = 1500;
+    SERVO_R = 1500;
     _delay_ms(1500);
 
     /* Reverse */
-    OCR1A = ICR1 - 2000;
+    SERVO_L = 2000;
+    SERVO_R = 2000;
     _delay_ms(1500);
 
     /* Forwards */
-    OCR1A = ICR1 - 1000;
+    SERVO_L = 1000;
+    SERVO_R = 1000;
     _delay_ms(1500);
 }
 
+uint16_t read_adc(uint8_t channel)
+{
+    ADMUX &= 0xF0;
+    ADMUX |= channel;
+    SET(ADCSRA, ADSC); // Start conversion
+    while (CHK(ADCSRA, ADSC)); // Wait for conversion to finish
+
+    return ADCW;
+}
+
 int main(void)
 {
-    initIO();
-    runServos();
-    readTemp();
+    uint16_t adc_val;
 
+    init_led();
+    init_pwm();
+    init_adc();
+
+    while (1) {
+        adc_val = read_adc(0);
+        if (adc_val <= 512) {
+            SERVO_R = 2000;
+            SERVO_L = 2000;
+        } else {
+            SERVO_R = 1000;
+            SERVO_L = 1000;
+        }
+    }
 
     return 0;
 }