diff --git a/ard/pins.c b/ard/pins.c
index dc39841..3773495 100644
--- a/ard/pins.c
+++ b/ard/pins.c
@@ -28,11 +28,13 @@ void pins_init(){
 
 	// clear timer to compare match for PWM output
 	TCCR0A |= _BV(WGM01)|_BV(WGM00);
-	TCCR1A |= _BV(WGM01)|_BV(WGM00);
+	TCCR1A |= _BV(WGM11)|_BV(WGM10);
 	TCCR2A |= _BV(WGM21)|_BV(WGM20);
 
 	// enable interrupt on timer overflow for PWM output
 	TIMSK0 |= _BV(TOIE0);
+	TIMSK1 |= _BV(TOIE1);
+	TIMSK2 |= _BV(TOIE2);
 }
 
 void pin_mode(unsigned char pin, unsigned char mode){
@@ -167,21 +169,19 @@ void analog_write(unsigned char pin, unsigned char value){
 	// 1. clear timer on compare match (pins_init)
 	// 2. set interrupt on overflow (pins_init) and value to compare to
 	// 3. set timer scaling to 1, which starts the timer
-	if(pin == 3){
-		TCCR2A |= _BV(COM2B1);
-		OCR2B = value;
-		TCCR2B |= _BV(CS20)|_BV(CS21);
+
+	// timer 0
+	if(pin == 6){
+		TCCR0A |= _BV(COM0A1);
+		OCR0A = value;
+		TCCR0B |= _BV(CS00)|_BV(CS01);
 	}
 	else if(pin == 5){
 		TCCR0A |= _BV(COM0B1);
 		OCR0B = value;
-		TCCR0B |= _BV(CS00)|_BV(CS02);
-	}
-	else if(pin == 6){
-		TCCR0A |= _BV(COM0A1);
-		OCR0A = value;
-		TCCR0B |= _BV(CS00)|_BV(CS02);
+		TCCR0B |= _BV(CS00)|_BV(CS01);
 	}
+	// timer 1
 	else if(pin == 9){
 		TCCR1A |= _BV(COM1A1);
 		OCR1A = value;
@@ -192,11 +192,17 @@ void analog_write(unsigned char pin, unsigned char value){
 		OCR1B = value;
 		TCCR1B |= _BV(CS10)|_BV(CS11);
 	}
+	// timer 2
 	else if(pin == 11){
 		TCCR2A |= _BV(COM2A1);
 		OCR2A = value;
 		TCCR2B |= _BV(CS20)|_BV(CS21);
 	}
+	else if(pin == 3){
+		TCCR2A |= _BV(COM2B1);
+		OCR2B = value;
+		TCCR2B |= _BV(CS20)|_BV(CS21);
+	}
 }
 
 // maps A0-A7 to 0-7
diff --git a/compile b/compile
index a00c167..e1fc428 100755
--- a/compile
+++ b/compile
@@ -2,6 +2,8 @@
 
 mkdir build
 
+avr-gcc -Os -DF_CPU=16000000UL -mmcu=atmega328p -c -o build/servo.o -Wall servo/servo.c
+
 avr-gcc -Os -DF_CPU=16000000UL -mmcu=atmega328p -c -o build/rf24.o -Wall rf24/rf24.c
 
 avr-gcc -Os -DF_CPU=16000000UL -mmcu=atmega328p -c -o build/util.o -Wall ard/util.c
diff --git a/main.c b/main.c
index 3b1dabc..2be8f5d 100644
--- a/main.c
+++ b/main.c
@@ -3,19 +3,12 @@
 #include "ard/serial.h"
 #include "ard/pins.h"
 #include "ard/util.h"
-#include "rf24/rf24.h"
-#include "rf24/nRF24L01.h"
+#include "servo/servo.h"
 
-//#define TX
-#define PAYLOAD 32 // byte
-
-unsigned char rx_addr[5] = {0xE7, 0xE7, 0xE7, 0xE7, 0xE7};
-unsigned char tx_addr[5] = {0xD7, 0xD7, 0xD7, 0xD7, 0xD7};
+Servo servo = {7};
 
 void prepare();
 void loop();
-void tx();
-void rx();
 
 int main(){
 	prepare();
@@ -28,63 +21,9 @@ int main(){
 void prepare(){
 	pins_init();
 	serial_init(9600);
-
-	pin_mode(A1, OUTPUT); // green
-	pin_mode(A2, OUTPUT); // red
-
-	rf24_init(7, 6, 5, 4, 3, 2);
-	rf24_config(2, PAYLOAD);
-
-#ifdef TX
-	rf24_rx_addr(rx_addr);
-	rf24_tx_addr(tx_addr);
-#else
-	rf24_rx_addr(tx_addr);
-	rf24_tx_addr(rx_addr);
-#endif
-
-	digital_write(A1, HIGH);
-	_delay_ms(300);
-	digital_write(A1, LOW);
+	servo_init(&servo);
 }
 
 void loop(){
-#ifdef TX
-	tx();
-#else
-	rx();
-#endif
-}
-
-void tx(){
-	_delay_ms(10);
-	digital_write(A1, LOW);
-	digital_write(A2, LOW);
-	unsigned char data[PAYLOAD] = "Hello, World! What's going on?";
-
-	rf24_send(data);
-	while(rf24_is_sending());
-	unsigned char status = rf24_status();
-
-	if(status&(1<<TX_DS)){
-		digital_write(A1, HIGH);
-	}
-	else{
-		digital_write(A2, HIGH);
-	}
-}
-
-void rx(){
-	digital_write(A1, LOW);
-	digital_write(A2, LOW);
-
-	if(rf24_data_ready()){
-		unsigned char data[PAYLOAD];
-
-		rf24_get_data(data);
-		digital_write(A1, HIGH);
-		serial_write((char*)data, PAYLOAD);
-
-		_delay_ms(100);
-	}
+
 }
diff --git a/servo/servo.c b/servo/servo.c
new file mode 100644
index 0000000..a45f993
--- /dev/null
+++ b/servo/servo.c
@@ -0,0 +1,53 @@
+#include "servo.h"
+#include "../ard/pins.h"
+#include <util/delay.h>
+#include <avr/io.h>
+#include <avr/interrupt.h>
+
+#define MAX_SERVOS 8
+#define SERVO_DELTA_MIN_US 1000
+#define SERVO_DELTA_US 1000
+
+Servo* servos[MAX_SERVOS];
+unsigned char servo_i = 0;
+
+void servo_init(Servo* servo){
+	if(servo_i >= MAX_SERVOS){
+		return;
+	}
+
+	servos[servo_i] = servo;
+	servo_i++;
+
+	sei();
+	pin_mode((*servo).pin, OUTPUT);
+	(*servo).val = 0;
+
+	TCCR1A = 0x00;
+	TCCR1B = (1<<WGM12);
+	TCCR1C = 0x00;
+
+	// 20ms, 1.5ms
+	OCR1A = 1250;
+	OCR1B = 3000;
+
+	// enable interrupt on timer match
+	TIMSK1 = (1<<OCIE1A)|(1<<OCIE1B);
+
+	// start interrupt, prescaler 8
+	TCCR1B |= 1<<CS12;
+}
+
+void servo_set(Servo* servo, unsigned char val){
+	(*servo).val = val;
+}
+
+ISR(TIMER1_COMPA_vect){
+	unsigned char i = 0;
+
+	for(i = 0; i < servo_i; i++){
+		digital_write(7, HIGH);
+		_delay_us(1000+(1000/255)*(*servos[i]).val);
+		digital_write(7, LOW);
+	}
+}
diff --git a/servo/servo.h b/servo/servo.h
new file mode 100644
index 0000000..247e1da
--- /dev/null
+++ b/servo/servo.h
@@ -0,0 +1,12 @@
+#ifndef SERVO_H_
+#define SERVO_H_
+
+typedef struct{
+	unsigned char pin;
+	unsigned char val; // 0-255
+} Servo;
+
+void servo_init(Servo*);
+void servo_set(Servo*, unsigned char);
+
+#endif