Created a servo interface.

emb/servos.cpp

+#include "servos.h"
+
+#include <avr/io.h>
+#include <avr/interrupt.h>
+#include <stdint.h>
+
+#define NUM_SERVOS 3
+
+static void reorderServos();
+static void initTimer1();
+
+struct Servo
+{
+    volatile uint8_t * const port;  // the PORT register connected to the servo
+    volatile uint8_t * const ddr;   // the DDR register connected to the servo
+    const uint8_t pinMask;          // the pin mask for the servo
+    uint16_t tempValue;             // stores a temporary value before it's written to the servo
+    uint16_t value;                 // current pulse width value
+};
+
+static Servo servos[NUM_SERVOS] = {
+    { &PORTB, &DDRB, (1 << PB2), 0, 0 },
+    { &PORTB, &DDRB, (1 << PB3), 0, 0 },
+    { &PORTB, &DDRB, (1 << PB4), 0, 0 }
+};
+
+static uint16_t orderedValues[NUM_SERVOS];  // current servo values in ascending order
+static uint8_t orderedValueIndex = 0;       // index in orderedValues array
+static bool updateFlag = false;             // used to flag when the servo values have been updated
+
+void initServos()
+{
+    for (Servo &servo : servos)
+    {
+        *servo.ddr |= servo.pinMask;
+    }
+    
+    initTimer1();
+}
+
+void setServoAngle(uint8_t servo, int16_t arcMin)
+{
+    if (0 <= servo && servo < NUM_SERVOS)
+        servos[servo].tempValue = 2999 + (int32_t) arcMin * 1000 / (180 * 60);
+}
+
+void updateServos()
+{
+    updateFlag = true;
+}
+
+static void reorderServos()
+{
+    uint16_t minValue, maxValue = 0;
+    
+    for (Servo &servo : servos)
+        servo.value = servo.tempValue;
+    
+    for (uint16_t &value : orderedValues)
+    {
+        minValue = (uint16_t) -1;
+        
+        for (Servo servo : servos)
+        {
+            if (servo.value < minValue && servo.value > maxValue)
+                minValue = servo.value;
+        }
+        
+        maxValue = minValue;
+        value = maxValue;
+    }
+}
+
+static void initTimer1()
+{
+    cli();
+    
+    TCCR1A = 0;
+    TCCR1B = (1 << WGM12)   // CTC mode, TOP = OCR1A
+           | (1 << CS11);   // 8th prescaler 
+    TCCR1C = 0;
+    
+    OCR1A = 39999;                          // set TOP for 50 Hz 
+    TIMSK1 = (1 << OCIE1A) | (1 << OCIE1B); // enable interrupts
+    
+    sei();
+}
+
+ISR(TIMER1_COMPA_vect)
+{
+    if (updateFlag)
+    {
+        updateFlag = false;
+        reorderServos();
+    }
+    
+    for (Servo &servo : servos)
+        *servo.port |= (servo.pinMask); // set servo pin HIGH
+    
+    orderedValueIndex = 0;
+    OCR1B = orderedValues[orderedValueIndex];
+}
+
+ISR(TIMER1_COMPB_vect)
+{
+    uint16_t currValue = orderedValues[orderedValueIndex];
+    
+    for (Servo &servo : servos)
+    {
+        if (servo.value == currValue)
+            *servo.port &= ~(servo.pinMask); // set servo pin LOW
+    }
+    
+    orderedValueIndex++;
+    
+    if (orderedValueIndex < NUM_SERVOS)
+        OCR1B = orderedValues[orderedValueIndex];
+}

emb/servos.hpp

+#ifndef SERVOS_H
+#define SERVOS_H
+
+#include <stdint.h>
+
+/**
+ * Initialises the servos.
+ */
+void initServos();
+
+/**
+ * Sets the angle of a servo.
+ * 
+ * @param servo     the index of the servo to control
+ * @param arcMin    the angle of the servo in arc-minutes
+ */
+void setServoAngle(uint8_t servo, int16_t arcMin);
+
+/**
+ * Writes the servo values out to the servos.
+ */
+void updateServos();
+
+#endif /* SERVOS_H */