diff --git a/include/debug.h b/include/debug.h
index aa7197a..1f6e677 100644
--- a/include/debug.h
+++ b/include/debug.h
@@ -3,14 +3,14 @@
 
 #include <avr/io.h>
 #include "/usr/include/simavr/avr/avr_mcu_section.h"
+
 AVR_MCU(F_CPU, "atmega328p");
 const struct avr_mmcu_vcd_trace_t _mytrace[]  _MMCU_ = {
     { AVR_MCU_VCD_SYMBOL("DDRB"), .what = (void *) &DDRB, },
-    { AVR_MCU_VCD_SYMBOL("DDRD"), .what = (void *) &DDRD, },
-    { AVR_MCU_VCD_SYMBOL("PORTD"), .what = (void *) &PORTD, },
-    { AVR_MCU_VCD_SYMBOL("TCCR0B"), .what = (void *) &TCCR0B, },
-    { AVR_MCU_VCD_SYMBOL("TCCR0A"), .what = (void *) &TCCR0A, },
-    { AVR_MCU_VCD_SYMBOL("OCR0A"), .what = (void*)&OCR0A, },
+    { AVR_MCU_VCD_SYMBOL("TCCR1A"), .what = (void *) &TCCR1A, },
+    { AVR_MCU_VCD_SYMBOL("TCCR1B"), .what = (void *) &TCCR1B, },
+    { AVR_MCU_VCD_SYMBOL("OCR1A"), .what = (void *) &OCR1A, },
+    { AVR_MCU_VCD_SYMBOL("OCR1B"), .what = (void *) &OCR1B, },
 };
 
 #endif /* DEBUG_H */
diff --git a/src/main.c b/src/main.c
index f9466e7..307fe21 100644
--- a/src/main.c
+++ b/src/main.c
@@ -11,6 +11,15 @@
 
 void initIO()
 {
+    /* Servos */
+    DDRB |= (1 << SERVO_L);
+    /* Phase Correct PWM, 9-bit; Inverting mode */
+    TCCR1A |= 1 << WGM11 | 1 << COM1A1 | 1 << COM1A0;
+    /*  "Clear Timer on Compare match" mode; Prescaler = 1 */
+    TCCR1B |= 1 << WGM13 | 1 << WGM12 | 1 << CS10;
+    ICR1 = 19999; // F_CPU / 50Hz - 1
+
+    /* LED */
     DDRD |= (1 << LED_1);
 }
 
@@ -30,10 +39,27 @@ void readTemp()
     }
 }
 
+void runServos()
+{
+    /* Stop */
+    OCR1A = ICR1 - 1500;
+    _delay_ms(1500);
+
+    /* Reverse */
+    OCR1A = ICR1 - 2000;
+    _delay_ms(1500);
+
+    /* Forwards */
+    OCR1A = ICR1 - 1000;
+    _delay_ms(1500);
+}
+
 int main(void)
 {
     initIO();
+    runServos();
     readTemp();
 
+
     return 0;
 }