diff --git a/include/io.h b/include/io.h
index ee1d3d6..a5ac84a 100644
--- a/include/io.h
+++ b/include/io.h
@@ -12,11 +12,4 @@
 #define SERVO_R_PIN PB2
 #define SERVO_R OCR1B
 
-/* Photo sensors */
-#define PH_1 PC1
-#define PH_2 PC2
-#define PH_3 PC3
-#define PH_4 PC4
-#define PH_5 PC5
-
 #endif
diff --git a/include/main.h b/include/main.h
index 326e8cc..58be55a 100644
--- a/include/main.h
+++ b/include/main.h
@@ -1,16 +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 initLED(void);
-void initPWM(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 912653f..5e7ba95 100644
--- a/src/main.c
+++ b/src/main.c
@@ -9,7 +9,7 @@
 #include "debug.h"
 #endif /* DEBUG */
 
-void initPWM(void)
+void init_pwm(void)
 {
     DDRB |= 1 << SERVO_L_PIN | 1 << SERVO_R_PIN;
     /* Phase Correct PWM; Set/clear on Compare Match when down/up-counting */
@@ -19,12 +19,19 @@ void initPWM(void)
     ICR1 = F_CPU / 50 - 1; // 50Hz required by servos
 }
 
-void initLED(void)
+void init_led(void)
 {
-    SET(DDRD, LED_1);
+    DDRD |= 1 << LED_1;
 }
 
-void readTemp(void)
+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,7 +47,7 @@ void readTemp(void)
     }
 }
 
-void runServos(void)
+void run_servos(void)
 {
     /* Stop */
     SERVO_L = 1500;
@@ -58,13 +65,34 @@ void runServos(void)
     _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)
 {
-    initLED();
-    initPWM();
+    uint16_t adc_val;
 
-    runServos();
-    readTemp();
+    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;
 }