Fixed some PWM pins and started servo library.

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