--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/firmware/Makefile	Sun Dec 18 23:51:12 2016 +0100
@@ -0,0 +1,43 @@
+DEVICE      = atmega328p
+CLOCK      = 16000000
+PROGRAMMER = -c usbasp
+OBJECTS    = main.o servo.o
+
+#FUSES       = -U lfuse:w:0xe2:m -U hfuse:w:0xdf:m -U efuse:w:0xff:m
+
+# Tune the lines below only if you know what you are doing:
+AVRDUDE = avrdude $(PROGRAMMER) -p $(DEVICE)
+COMPILE = avr-gcc -std=gnu99 -Wall -Os -DF_CPU=$(CLOCK) -mmcu=$(DEVICE)
+
+# symbolic targets:
+all:	main.hex
+
+.c.o:
+	$(COMPILE) -c $< -o $@
+
+.S.o:
+	$(COMPILE) -x assembler-with-cpp -c $< -o $@
+
+.c.s:
+	$(COMPILE) -S $< -o $@
+
+flash:	all
+	$(AVRDUDE) -U flash:w:main.hex:i
+
+fuse:
+	$(AVRDUDE) $(FUSES)
+
+# Xcode uses the Makefile targets "", "clean" and "install"
+install: flash fuse
+
+clean:
+	rm -f main.hex main.elf $(OBJECTS)
+
+# file targets:
+main.elf: $(OBJECTS)
+	$(COMPILE) -o main.elf $(OBJECTS)
+
+main.hex: main.elf
+	rm -f main.hex
+	avr-objcopy -j .text -j .data -O ihex main.elf main.hex
+	avr-size --format=avr --mcu=$(DEVICE) main.elf

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/firmware/main.c	Sun Dec 18 23:51:12 2016 +0100
@@ -0,0 +1,8 @@
+#include "servo.h"
+
+
+int main() {
+    servo_setup(20000);
+    while (1) {
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/firmware/servo.c	Sun Dec 18 23:51:12 2016 +0100
@@ -0,0 +1,107 @@
+#include <avr/interrupt.h>
+#include <avr/io.h>
+#include <inttypes.h>
+
+
+uint16_t servo_pulse[20];
+
+static uint16_t prescaler;
+volatile static uint16_t servo_delay[8];
+volatile static uint8_t servo_mask_b[8];
+volatile static uint8_t servo_mask_c[8];
+volatile static uint8_t servo_mask_d[8];
+volatile static uint8_t event;
+
+
+ISR(TIMER1_COMPA_vect) {
+    event = 0;
+    PORTB = 0b00111111; // leave out XTAL pins
+    PORTC = 0b00001111; // leave out RESET, SDA and SCL
+    PORTD = 0b11111111;
+    TCNT1 = 0;
+    OCR1B = servo_delay[event];
+}
+
+
+ISR(TIMER1_COMPB_vect) {
+    event += 1;
+    PORTB &= servo_mask_b[event];
+    PORTC &= servo_mask_c[event];
+    PORTD &= servo_mask_d[event];
+    OCR1B = servo_delay[event];
+}
+
+
+uint16_t servo_ticks(uint16_t us) {
+    return (((F_CPU / 1000000L) * us) / prescaler) & 0xfff0;
+}
+
+
+void servo_update() {
+    uint16_t last_pulse = 0;
+    for (uint8_t event = 0; event < 8; ++event) {
+        uint16_t smallest_pulse = 0xffff;
+        for (uint8_t servo = 0; servo < 20; ++servo) {
+            if (servo_pulse[servo] > last_pulse &&
+                servo_pulse[servo] < smallest_pulse) {
+                    smallest_pulse = servo_pulse[servo];
+            }
+        }
+        servo_delay[event] = servo_ticks(smallest_pulse);
+        servo_mask_b[event] = 0xff;
+        for (uint8_t servo = 0; servo < 6; ++servo) {
+            if (servo_pulse[servo] <= smallest_pulse) {
+                servo_mask_b[event] &= ~(1 << servo);
+            }
+        }
+        servo_mask_c[event] = 0xff;
+        for (uint8_t servo = 0; servo < 4; ++servo) {
+            if (servo_pulse[6 + servo] <= smallest_pulse) {
+                servo_mask_c[event] &= ~(1 << servo);
+            }
+        }
+        servo_mask_d[event] = 0xff;
+        for (uint8_t servo = 0; servo < 8; ++servo) {
+            if (servo_pulse[10 + servo] <= smallest_pulse) {
+                servo_mask_d[event] &= ~(1 << servo);
+            }
+        }
+        last_pulse = smallest_pulse;
+    }
+}
+
+
+void servo_setup(uint16_t period) {
+    uint8_t mask;
+    if (period < (1 * 65535L / (F_CPU / 1000000L))) {
+	prescaler = 1;
+	mask = 1<<CS10;
+    } else if (period < (8 * 65535L / (F_CPU / 1000000L))) {
+	prescaler = 8;
+	mask = 1<<CS11;
+    } else if (period < (64 * 65535L / (F_CPU / 1000000L))) {
+	prescaler = 64;
+	mask = 1<<CS11 | 1<<CS10;
+    } else if (period < (256 * 65535L / (F_CPU / 1000000L))) {
+	prescaler = 256;
+	mask = 1<<CS12;
+    } else if (period < (1024 * 65535L / (F_CPU / 1000000L))) {
+	prescaler = 1024;
+	mask = 1<<CS12 | 1<<CS10;
+    } else {
+	prescaler = 1024;
+	mask = 1<<CS12 | 1<<CS10;
+    }
+    TCCR1A = 0x00;
+    TCCR1B &= ~(_BV(CS12) | _BV(CS11) | _BV(CS10));
+    TCCR1B |= mask;
+    OCR1A = servo_ticks(period);
+    TIMSK1 = _BV(OCIE1A) | _BV(OCIE1B);
+    DDRD = 0xff;
+    for (int i = 0; i < 8; ++i) {
+        servo_pulse[i] = 1500;
+    }
+    servo_update();
+    sei();
+    TCCR1C |= 1<<FOC1A; // trigger COMPA
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/firmware/servo.h	Sun Dec 18 23:51:12 2016 +0100
@@ -0,0 +1,8 @@
+#include <inttypes.h>
+
+
+extern uint16_t servo_pulse[8];
+
+
+void servo_update();
+void servo_setup(uint16_t period);