--- a/firmware/Makefile	Fri Jan 20 11:55:51 2017 +0100
+++ b/firmware/Makefile	Fri Jan 20 17:47:37 2017 +0100
@@ -4,7 +4,7 @@
 OBJECTS = main.o
 
 FUSES = -U lfuse:w:0xE2:m -U hfuse:w:0xDF:m
-AVRDUDE = avrdude $(PROGRAMMER) -p $(DEVICE) -B 500
+AVRDUDE = avrdude $(PROGRAMMER) -p $(DEVICE) -B 100
 COMPILE = avr-gcc -std=gnu99 -Wall -Os -DF_CPU=$(CLOCK) -mmcu=$(DEVICE)
 
 all:	main.hex

--- a/firmware/main.c	Fri Jan 20 11:55:51 2017 +0100
+++ b/firmware/main.c	Fri Jan 20 17:47:37 2017 +0100
@@ -23,6 +23,9 @@
 volatile static uint8_t servo_mask_b[MAX_SERVO];
 volatile static uint8_t servo_mask_c[MAX_SERVO];
 volatile static uint8_t servo_mask_d[MAX_SERVO];
+volatile static uint8_t servo_enable_b;
+volatile static uint8_t servo_enable_c;
+volatile static uint8_t servo_enable_d;
 volatile static uint8_t servo_event;
 
 // Remap servo numbers to pins.
@@ -35,9 +38,11 @@
 
 ISR(TIMER1_COMPA_vect) {
     servo_event = 0;
-    PORTB = 0b11111111;
-    PORTC = 0b00001111; // leave out RESET, SDA, SCL, PC7
-    PORTD = 0b11111111;
+    PORTB = 0xff;
+    // leave out RESET, A, SCL, PC7
+    // pull up SDA and SCL
+    PORTC = 0xff & 0x3f;
+    PORTD = 0xff;
     TCNT1 = 0;
     OCR1B = servo_delay[0];
 }
@@ -45,7 +50,7 @@
 
 ISR(TIMER1_COMPB_vect) {
     PORTB &= servo_mask_b[servo_event];
-    PORTC &= servo_mask_c[servo_event];
+    PORTC &= servo_mask_c[servo_event] | 0x30;
     PORTD &= servo_mask_d[servo_event];
     servo_event += 1;
     OCR1B = servo_delay[servo_event];
@@ -65,19 +70,22 @@
         servo_delay[event] = smallest_pulse;
         servo_mask_b[event] = 0xff;
         for (uint8_t servo = 0; servo < 8; ++servo) {
-            if (servo_pulse[servo] <= smallest_pulse) {
+            if (servo_pulse[servo] &&
+                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[8 + servo] <= smallest_pulse) {
+            if (servo_pulse[8 + servo] &&
+                servo_pulse[8 + 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[12 + servo] <= smallest_pulse) {
+            if (servo_pulse[12 + servo] &&
+                servo_pulse[12 + servo] <= smallest_pulse) {
                 servo_mask_d[event] &= ~(1 << servo);
             }
         }
@@ -87,7 +95,7 @@
 
 
 uint16_t adc(uint8_t pin) {
-    ADMUX = 0<<ADLAR | 0<<REFS1 | 0<<REFS0 | (0x03 & pin);
+    ADMUX = 0<<ADLAR | 0<<REFS1 | 0<<REFS0 | 0x03 & pin;
     ADCSRA |= 1<<ADSC;
     while (ADCSRA & (1<<ADSC)) {
         // Pass.
@@ -97,19 +105,19 @@
 
 
 int main() {
+    uint8_t i2c_address = I2C_ADDRESS;
     // Init ADC.
-    ADCSRA = 1<<ADEN | 1<<ADPS2 | 1<<ADPS1 | 0<<ADPS0; // Pre-scaler 64.
-    uint8_t i2c_address = I2C_ADDRESS;
-    if (adc(6) > 511) {
-        i2c_address += 1;
-    }
-    if (adc(7) > 511) {
-        i2c_address += 2;
-    }
-    ADCSRA = 0<<ADEN;
+//    ADCSRA = 1<<ADEN | 1<<ADPS2 | 1<<ADPS1 | 0<<ADPS0; // Pre-scaler 64.
+//    if (adc(6) > 511) {
+//        i2c_address += 1;
+//    }
+//    if (adc(7) > 511) {
+//        i2c_address += 2;
+//    }
+//    ADCSRA = 0<<ADEN;
 
     // setup i2c slave
-    PORTC |= 1<<PC4 | 1<<PC5; // enable pullups
+    //PORTC |= 1<<PC4 | 1<<PC5; // enable pullups
     TWAR = i2c_address<<1;
 
     // setup servos
@@ -130,10 +138,17 @@
     sei();
     TCCR1C |= 1<<FOC1A; // trigger COMPA
 
+    uint8_t dirty = 0;
     // run i2c slave
     while (1) {
         TWCR = 1<<TWEN | 1<<TWINT | 1<<TWEA;
-        while (!(TWCR & (1<<TWINT))) {}
+        while (!(TWCR & (1<<TWINT))) {
+            // Only do the change if we are not in the middle of events.
+            if (dirty && servo_delay[servo_event] == 0xffff) {
+                servo_update();
+                dirty = 0;
+            }
+        }
         switch (TWSR & 0xF8) {
             case 0x60: // received address for write and acked
                 i2c_cursor = 0;
@@ -145,19 +160,20 @@
                 }
                 break;
             case 0xa0: // stop or repeated start received
-                if (i2c_cursor > 0) {
-                    uint8_t i2c_servo = i2c_buffer[0];
-                    for (uint8_t i = 1; i < i2c_cursor - 1; i += 2) {
-                        if (i2c_servo >= 20) {
-                            i2c_servo = 0;
-                        }
-                        bytes2int.bytes[0] = i2c_buffer[i];
-                        bytes2int.bytes[1] = i2c_buffer[i + 1];
-                        servo_pulse[servo_map[i2c_servo]] = bytes2int.value;
-                        i2c_servo += 1;
+                if (i2c_cursor == 0) {
+                    break;
+                }
+                uint8_t i2c_servo = i2c_buffer[0];
+                for (uint8_t i = 1; i < i2c_cursor - 1; i += 2) {
+                    if (i2c_servo >= MAX_SERVO) {
+                        i2c_servo = 0;
                     }
-                    servo_update();
+                    bytes2int.bytes[0] = i2c_buffer[i];
+                    bytes2int.bytes[1] = i2c_buffer[i + 1];
+                    servo_pulse[servo_map[i2c_servo]] = bytes2int.value;
+                    i2c_servo += 1;
                 }
+                dirty = 1;
                 break;
         }
     }