diff --git a/ATMegaCard/ISO7816-card.c b/ATMegaCard/ISO7816-card.c
index d82cf617a6eae3f444916639fa902a3c10a378cf..d3d8c37beef7c97507395fd1c5cb7f2198a74745 100644
--- a/ATMegaCard/ISO7816-card.c
+++ b/ATMegaCard/ISO7816-card.c
@@ -3,6 +3,16 @@
 #include <util/delay.h>
 #include <string.h>
 
+#define BAUD (F_CPU/372)
+#define USART_NAME UART
+#define PARITY EVEN
+#define STOPBITS 2
+#include "../common/serial.h"
+#define UART_recv_init UART_switch_mode_receive
+#define UART_transmit_init UART_switch_mode_transmit
+#define UART_receive_char_timeout UART_read_char_timeout
+#define UART_ERR_TIMEOUT ERR_TIMEOUT
+
 //ATMega Card
 //ATR: 3B8F014156522049534F3738313620302E31B9
 //Query: 00C101FE3E00000267016400C101FE3E00000267016400C101FE3E000002670164
@@ -32,20 +42,9 @@ uint16_t ISO7816_receive_buffer(uint8_t* buffer, uint8_t length);
 uint8_t  ISO7816_transmit_buffer(uint8_t* buffer, uint8_t length);
 uint8_t  ISO7816_transmit_t1_header(ISO7816_T1_Header* t1_header);
 
-void ISO7816_init(void){
-	UART_init();
-}
-/*
-void ISO7816_sendATR(void){
-	UART_transmit_init();
-	for (uint8_t i = 0; i < sizeof(atr); i++){
-		UART_transmit_char(atr[i]);
-	}
-	UART_recv_init();
-}*/
-
 void ISO7816_sendATR(void){
 	uint8_t checksum = 0;
+	UART_init();
 	UART_transmit_init();
 	UART_transmit_char(atr.initial_char);
 	checksum ^= atr.t0; UART_transmit_char(atr.t0);
diff --git a/ATMegaCard/ISO7816-card.h b/ATMegaCard/ISO7816-card.h
index 9728a1370a57b9c2ca9b9be61d0b31398a5285b8..896ddbf4f22f612f3bc20bd320b7d92dba4bd8d0 100644
--- a/ATMegaCard/ISO7816-card.h
+++ b/ATMegaCard/ISO7816-card.h
@@ -1,6 +1,5 @@
 #pragma once
 
-#include "software_serial.h"
 #include "../common/ISO7816-common.h"
 
 #define ISO7816_APDU_READ_TIMEOUT 3000 //300ms
diff --git a/ATMegaCard/Makefile b/ATMegaCard/Makefile
index 8e5e590a5d07800e71d2f69018e7b393ef05473f..1cf13f4f7309a44235248264e70da7bbf154d827 100644
--- a/ATMegaCard/Makefile
+++ b/ATMegaCard/Makefile
@@ -31,7 +31,7 @@
 
 # MCU name
 
-MCU = atmega163
+MCU = atmega16
 
 
 
@@ -41,6 +41,13 @@ ifeq ($(MCU), atmega163)
 	HEX_FILE_NAME = MEGA163
 endif
 
+#Fuse settings for ATmega16
+ifeq ($(MCU), atmega16)
+	#4Mhz internal, BOD, Quick Startup
+	FUSE_BITS = -u -U lfuse:w:0x83:m -U hfuse:w:0xd1:m
+	HEX_FILE_NAME = MEGA16
+endif
+
 # Output format. (can be srec, ihex, binary)
 FORMAT = ihex
 
diff --git a/ATMegaCard/common.h b/ATMegaCard/common.h
index 3c7f2176f8ddc22aaf14b6dcc314ec79d6607f7a..376f92743a67e403e2d52070b30ca1a729608cc3 100644
--- a/ATMegaCard/common.h
+++ b/ATMegaCard/common.h
@@ -1 +1,2 @@
-#define F_CPU 3579500
\ No newline at end of file
+//#define F_CPU 3579500
+#define F_CPU 4000000
\ No newline at end of file
diff --git a/ATMegaCard/main.c b/ATMegaCard/main.c
index 270701fc18c12999f93e699992481e7e1494f143..9d5900b00fe58d7bf8bf2d7e433901992d5bf876 100644
--- a/ATMegaCard/main.c
+++ b/ATMegaCard/main.c
@@ -1,6 +1,7 @@
 #include "common.h"
 #include <avr/io.h>
 #include <util/delay.h>
+#include <avr/interrupt.h>
 #include <string.h>
 #include "rng.h"
 
@@ -21,8 +22,6 @@ uint16_t main_handle_adpu(ISO7816_APDU_Header* header,
 uint8_t current_application = 0xFF;
 
 int main(void){
-	//DDRB = (1 << 5) | (1 << 7);
-// 	ISO7816_init();
 	ISO7816_set_apdu_handler(main_handle_adpu);
 	ISO7816_sendATR();
 	sei();
diff --git a/ATMegaCard/rng.c b/ATMegaCard/rng.c
index de843640f2286f66443b996d69cb7c1f205ccfea..d8a24f85c1eb278dbdb845ce0fd8effea2aac649 100644
--- a/ATMegaCard/rng.c
+++ b/ATMegaCard/rng.c
@@ -3,6 +3,10 @@
 
 #define ADC_RNG_CHANNEL 0
 
+#ifndef ADCSR
+	#define ADCSR ADCSRA
+#endif
+
 uint16_t seed;
 
 uint8_t rnd_adc_read(void);
diff --git a/ATMegaCard/software_serial.c b/ATMegaCard/software_serial.c
index bd9fb3946118f7ad442fc8d63f10c6f7c4a5fc23..934a923e60233e2bc93380409bdb7a14c5b87e64 100644
--- a/ATMegaCard/software_serial.c
+++ b/ATMegaCard/software_serial.c
@@ -11,21 +11,23 @@
 #endif
 
 
+void SoftUART_recv_init(void);
+	
 /**
  * Initializes the Software UART
  */
-void UART_init(void){
+void SoftUART_init(void){
 	#ifdef UART_USE_AS_STDOUT
 		stdout = &UART_stdout; //assign serial output stream as stdout.
 	#endif
 	
-	UART_recv_init();
+	SoftUART_recv_init();
 }
 
 /**
  * Switches the UART to transmit mode
  */
-void UART_transmit_init(void){
+void SoftUART_transmit_init(void){
 	cli(); //interrupts disable (messes up send timings otherwise)
 	SWSERIAL_PORT |= (1 << SWSERIAL_LINE); //data line to high level (otherwise we might trigger a start bit)
 	SWSERIAL_PORT_DR |= (1 << SWSERIAL_LINE); //output
@@ -35,9 +37,9 @@ void UART_transmit_init(void){
  * Sends a 0-terminated string
  * Note: The caller must to switch to transmit mode before calling this
  */
-void UART_transmit_buffer_sz(char* buffer){
+void SoftUART_transmit_buffer_sz(char* buffer){
 	while (buffer[0] != 0){
-		UART_transmit_char(buffer[0]);
+		SoftUART_transmit_char(buffer[0]);
 		buffer++;
 	}
 }
@@ -46,13 +48,13 @@ void UART_transmit_buffer_sz(char* buffer){
  * Sends a byte coverted to hex digits
  * Note: The caller must to switch to transmit mode before calling this
  */
-void UART_transmit_byte_hex(unsigned char b){
+void SoftUART_transmit_byte_hex(unsigned char b){
 
 	unsigned char n;
 	n = '0' + (b >> 4 & 0xF);
-	UART_transmit_char((n > '9') ? n + 7 : n);
+	SoftUART_transmit_char((n > '9') ? n + 7 : n);
 	n = '0' + (b & 0xF);
-	UART_transmit_char((n > '9') ? n + 7 : n);
+	SoftUART_transmit_char((n > '9') ? n + 7 : n);
 }
 
 
@@ -60,7 +62,7 @@ void UART_transmit_byte_hex(unsigned char b){
  * Writes a single character to the data line, bit-by-bit
  * Note: The caller must to switch to transmit mode before calling this
  */
-void UART_transmit_char(unsigned char x){
+void SoftUART_transmit_char(unsigned char x){
 
 	_delay_us(BIT_LENGTH); //guard time beween bytes
 	
@@ -111,7 +113,7 @@ volatile uint8_t recv_valid = 0;    //Did we receive 1 for the stop bits and val
 /**
  * Switch UART into receive mode
  */
-void UART_recv_init(void){
+void SoftUART_recv_init(void){
 	SWSERIAL_PORT_DR &= ~(1 << SWSERIAL_LINE); //set Serial pin to input
 	SWSERIAL_PORT |= (1 << SWSERIAL_LINE); //pullup
 	
@@ -120,15 +122,15 @@ void UART_recv_init(void){
 	TIFR = (1 << ICF1); //clear old interrupt flag.
 	TIMSK = 1 << TICIE1; //enable ICP interrupt, disable timer compare interrupt
 	TCCR1B =  (1 << CS10)   //Timer prescaler = 1
-// #ifdef CTC1
+#ifdef CTC1
 			| (1 << CTC1)   //Clear timer on compare match (important for exact timing, don't do it in software)
-// #else
-// 			| (1 << WGM12)
-// #endif
+#else
+			| (1 << WGM12)
+#endif
 			| (1 << ICNC1) //Capture interrupt noise canceler enable (filters out very short spikes  < 4 clks)
-// #ifdef ICES1
-// 			| (1 << ICES1)
-// #endif
+#ifdef ICES1
+			| (1 << ICES1)
+#endif
 			;
 	
 	OCR1A  = 372; //Default smartcard conversion factor 3571200Hz/372(clks/bit) = 9600bit/s
@@ -139,30 +141,30 @@ void UART_recv_init(void){
 /**
  * Is a byte available to be processed?
  */
-inline uint8_t UART_data_available(void){
+inline uint8_t SoftUART_data_available(void){
 	return recv_avail != 0;
 }
 /**
  * Reads the last byte that was received.
  * Resets the availability flag.
  */
-uint8_t UART_read_char(void){
+uint8_t SoftUART_read_char(void){
 	recv_avail = 0;
 	return recv_buffer2;
 }
 
-uint16_t UART_receive_char_timeout(uint16_t timeout){
+uint16_t SoftUART_receive_char_timeout(uint16_t timeout){
 	while ((timeout > 0) && (recv_avail == 0)) {_delay_us(100); timeout--;}
 	if (recv_avail == 0) return UART_ERR_TIMEOUT;
 	
-	return UART_read_char();
+	return SoftUART_read_char();
 }
 
 /**
  * Enables the timer (interrupt) that samples the data line to receive the bits.
  * Disables the Capture (ICP) interrupt that triggered on the start bit.
  */
-void UART_start_read(void){
+void SoftUART_start_read(void){
 	recv_bits = 0;
 	
 	#if SWSERIAL_PARITY == EVEN
@@ -183,7 +185,7 @@ void UART_start_read(void){
  */
 ISR(TIMER1_CAPT_vect){
 	PORTB |= (1 << 5); //Debug
-	UART_start_read();
+	SoftUART_start_read();
 	PORTB &= ~(1 << 5); //Debug
 }
 
@@ -215,7 +217,7 @@ ISR(TIMER1_COMPA_vect){
 			recv_avail = 1;
 		}
 		recv_bits = 0;
-		UART_recv_init();
+		SoftUART_recv_init();
 	}
 	PORTB &= ~(1 << 7); //Debug
 }
diff --git a/ATMegaCard/software_serial.h b/ATMegaCard/software_serial.h
index e7a423a9ac21d818acbf0493e2beafce84ae69df..ea8161083a0f36d350eca060c04d35f94687370e 100644
--- a/ATMegaCard/software_serial.h
+++ b/ATMegaCard/software_serial.h
@@ -41,18 +41,18 @@
 
 #define INST_LENGTH(count) ((1000000*count)/F_CPU)  //(µSec) Duration of count CPU Instructions
 
-void UART_transmit_char(unsigned char data);
+void SoftUART_transmit_char(unsigned char data);
 
-void UART_transmit_buffer_sz(char* buffer);
-void UART_transmit_byte_hex(unsigned char b);
+void SoftUARTtransmit_buffer_sz(char* buffer);
+void SoftUARTtransmit_byte_hex(unsigned char b);
 
-void UART_init(void);
+void SoftUARTinit(void);
 
-void UART_transmit_init(void);
-void UART_recv_init(void);
+void SoftUARTtransmit_init(void);
+void SoftUARTrecv_init(void);
 
-void UART_transmit_char(unsigned char x);
+void SoftUARTtransmit_char(unsigned char x);
 
-uint8_t UART_data_available(void);
-uint8_t UART_read_char(void);
-uint16_t UART_receive_char_timeout(uint16_t timeout);
\ No newline at end of file
+uint8_t SoftUARTdata_available(void);
+uint8_t SoftUARTread_char(void);
+uint16_t SoftUARTreceive_char_timeout(uint16_t timeout);
\ No newline at end of file
diff --git a/Control/cardreader_interface.c b/Control/cardreader_interface.c
index 242b0c59c4e9d3a7a3214e3f2d8edb172d86120c..423063f55489edd0e7c543f67e2d8c8aadd433d8 100644
--- a/Control/cardreader_interface.c
+++ b/Control/cardreader_interface.c
@@ -32,8 +32,7 @@
  *    Card -> Host:   other ISO7816 status code (-> abort)
 **/
 
-
-void unlock(void);
+void cardreader_process_message(void);
 uint8_t check_card_status(uint8_t* msg_buffer, uint8_t length, uint8_t expected_body_length);
 void cardreader_select_auth_applet(void);
 uint8_t cardreader_select_auth_applet_result(uint8_t* msg_buffer, uint8_t length);
@@ -59,11 +58,11 @@ uint8_t cardreader_sys_get_card_status(void);
 static uint8_t cardreader_power_state = CARDREADER_POWER_STATE_OFF;
 static volatile uint8_t cardreader_startup_event = 0;
 
-#define AUTH_STATE_NONE 1
-#define AUTH_STATE_CARD_INSERTED 2
-#define AUTH_STATE_SELECTING_AUTH_APPLET 3
-#define AUTH_STATE_AUTH_REQUESTED_SENT 4
-#define AUTH_STATE_AUTH_VERYFIED 5
+#define AUTH_STATE_NONE 1                   //System idle
+#define AUTH_STATE_CARD_INSERTED 2          //Card has been inserted into th reader, auth will be started.
+#define AUTH_STATE_SELECTING_AUTH_APPLET 3  //APDU to select the portal auth application has been sent.
+#define AUTH_STATE_AUTH_REQUESTED_SENT 4    //Authorisation challange has been sent.
+#define AUTH_STATE_AUTH_VERYFIED 5          //Authorisation key has been verified, new key has been sent.
 
 volatile uint8_t auth_state = AUTH_STATE_NONE;
 volatile uint16_t state_timeout;
@@ -145,8 +144,10 @@ void cardreader_process(void){
 		return;
 	}
 	
-	if (!transport_data_available()) return;
-	
+	if (transport_data_available()) cardreader_process_message();
+}
+
+void cardreader_process_message(void){
 	uint8_t msg_buffer[100];
 	
 	uint16_t ret = transport_receive_message(msg_buffer, 100, 400);
@@ -337,11 +338,13 @@ uint8_t cardreader_update_key_result(uint8_t* msg_buffer, uint8_t length){
 // 	cardreader_display_show_result(1);
 	
 	cardreader_display_clear();
-	if (toggle_lock_unlock(cardreader_display_write_sz_P)){
+// 	if (
+	toggle_lock_unlock(cardreader_display_write_sz_P);
+// 		){
 // 		cardreader_display_write_sz_P(PSTR("Auth Successful"));
-		cardreader_display_move(0,1);
-		cardreader_display_write_sz_P(PSTR("Remove Card"));
-	}
+// 		cardreader_display_move(0,1);
+// 		cardreader_display_write_sz_P(PSTR("Remove Card"));
+// 	}
 	
 	keystore_update_salt();
 	
@@ -448,7 +451,6 @@ void cardreader_display_show_result(uint8_t success){
 	}
 }
 
-
 void cardreader_display_set_backlight(uint8_t on){
 	cardreader_display_message_t msg = {CARDREADER_MSG_TYPE_DISPALY};
 	msg.command = CARDREADER_DISPLAY_MSG_SET_BACKLIGHT;
@@ -457,4 +459,18 @@ void cardreader_display_set_backlight(uint8_t on){
 	
 	uint8_t success;
 	transport_receive_message(&success, 1, 3000);
+}
+
+void cardreader_display_show_door_retry(void){
+	cardreader_display_move(10,0);
+	cardreader_display_write_sz_P(PSTR("Retry"));
+}
+
+void cardreader_display_show_door_result(uint8_t success){
+	cardreader_display_move(0,1);
+	if (success) {
+		cardreader_display_write_sz_P(PSTR("OK, remove card "));
+	} else {
+		cardreader_display_write_sz_P(PSTR("Mechanical Fail!"));
+	}
 }
\ No newline at end of file
diff --git a/Control/cardreader_interface.h b/Control/cardreader_interface.h
index 8843bdf5d341f2531639f2ee408d220099178243..eaf9e16d21997c517faf0646652bd055584fa641 100644
--- a/Control/cardreader_interface.h
+++ b/Control/cardreader_interface.h
@@ -23,4 +23,7 @@ void cardreader_process(void);
 void cardreader_tick(void);
 
 uint8_t cardreader_init_card_key(KEY key);
-uint8_t cardreader_clear_card_key(void);
\ No newline at end of file
+uint8_t cardreader_clear_card_key(void);
+
+void cardreader_display_show_door_retry(void);
+void cardreader_display_show_door_result(uint8_t success);
\ No newline at end of file
diff --git a/Control/door.c b/Control/door.c
index a309e06e9af8cbaaf0b104e22a470f3e62cbad3b..664eaf1374de5380f78be3236d102379fe8b2497 100644
--- a/Control/door.c
+++ b/Control/door.c
@@ -1,5 +1,6 @@
 #include "door.h"
 #include "log.h"
+#include "cardreader_interface.h" //for (un)lock messages
 
 uint8_t door_read_pin_status(void);
 uint8_t door_update_status(uint8_t sensor_status);
@@ -20,17 +21,21 @@ const char* const status_names[] PROGMEM = {status_unlocked,
                                             status_unlocking,
                                             status_locking};
 
-volatile uint8_t door_status;
+uint8_t door_status;
 
-volatile uint8_t sensor_candidate_status;
-volatile uint8_t sensor_stability;
+uint8_t sensor_candidate_status;
+uint8_t sensor_stability;
 #define DOOR_PIN_NEEDED_STABILITY 4
 
-#define DOOR_COMMAND_PULSE_TIME 35 //350ms
-volatile uint8_t door_command_pulse_time = 0;
+#define DOOR_COMMAND_PULSE_TIME 45 //450ms
+uint8_t door_command_pulse_time = 0;
+
+#define DOOR_COMMAND_WAIT_TIMEOUT 900 //9s
+uint16_t door_command_wait_timeout = 0;
+
+uint8_t door_command_retry_count = 0;
+#define DOOR_COMMAND_MAX_RETRY 1
 
-#define DOOR_COMMAND_WAIT_TIMEOUT 1000 //10s
-volatile uint16_t door_command_wait_timeout = 0;
 
 void door_init(void){
 	DOOR_PORT &= ~(COMMAND_UNLOCK_PIN | COMMAND_LOCK_PIN); //commnd pins off
@@ -52,7 +57,9 @@ void door_init(void){
 void door_tick(void){
 	if (door_command_pulse_time != 0) {
 		door_command_pulse_time--;
-		if (door_command_pulse_time == 0) DOOR_PORT &= (uint8_t)~(COMMAND_LOCK_PIN|COMMAND_UNLOCK_PIN);
+		if (door_command_pulse_time == 0) {
+			DOOR_PORT &= (uint8_t)~(COMMAND_LOCK_PIN|COMMAND_UNLOCK_PIN);
+		}
 	}
 	
 	if (   ((door_status & DOOR_STATUS_UNLOCKING) && !(door_status & DOOR_STATUS_UNLOCKED))
@@ -60,12 +67,30 @@ void door_tick(void){
 		door_command_wait_timeout--;
 		if (door_command_wait_timeout == 0){
 			
-			printf_P(PSTR("Error: Door command timeout\n"));
 			log_append(LOG_EVENT_DOOR_COMMAND_TIMEOUT, 0);
 			
-			//TODO: Retry (and turn keymatic power on if currently off)
+			printf_P(PSTR("Error: Door command timeout!"));
 			
-			door_status &= (uint8_t)~(DOOR_STATUS_UNLOCKING | DOOR_STATUS_LOCKING); //clear activity flags
+			if (door_command_retry_count < DOOR_COMMAND_MAX_RETRY){
+				printf_P(PSTR(" Retry...\n"));
+				cardreader_display_show_door_retry();
+				uint8_t retry = door_command_retry_count;
+				
+				//TODO: turn keymatic power on if currently off?
+				if (door_status & DOOR_STATUS_UNLOCKING) unlock();
+				if (door_status & DOOR_STATUS_LOCKING  ) lock();
+				//lock/unlock resets the counter, restore it.
+				door_command_retry_count = retry + 1;
+				
+			} else {
+				
+				log_append(LOG_EVENT_DOOR_COMMAND_RETRY_FAIL, 0);
+				
+				printf_P(" Giving Up :((\n");
+				cardreader_display_show_door_result(0);
+				
+				door_status &= (uint8_t)~(DOOR_STATUS_UNLOCKING | DOOR_STATUS_LOCKING); //clear activity flags
+			}
 		}
 	}
 	
@@ -95,8 +120,15 @@ uint8_t door_read_pin_status(void){
 uint8_t door_update_status(uint8_t sensor_status){
 	uint8_t new_door_status = (sensor_status & DOOR_STATUS_HW_MASK) | (door_status & DOOR_STATUS_SW_MASK);
 
-	if (new_door_status & DOOR_STATUS_LOCKED)   new_door_status &= (uint8_t)~DOOR_STATUS_LOCKING;
-	if (new_door_status & DOOR_STATUS_UNLOCKED) new_door_status &= (uint8_t)~DOOR_STATUS_UNLOCKING;
+	if ((new_door_status & DOOR_STATUS_LOCKED) && (door_status & DOOR_STATUS_LOCKING)){
+		new_door_status &= (uint8_t)~DOOR_STATUS_LOCKING;
+		cardreader_display_show_door_result(1);
+	}
+		
+	if ((new_door_status & DOOR_STATUS_UNLOCKED) && (door_status & DOOR_STATUS_UNLOCKING)){
+		new_door_status &= (uint8_t)~DOOR_STATUS_UNLOCKING;
+		cardreader_display_show_door_result(1);
+	}
 	
 	if (is_alarm_status(new_door_status)) new_door_status |= DOOR_STATUS_ALARM;
 	
@@ -114,7 +146,7 @@ uint8_t is_alarm_status(uint8_t door_status){
 	
 	//NOTE: This condition is supposed to trigger, if the door is slammed open.
 	//      We can't use the LOCKED sensor, because the connection between lock bolt and frame will be interrupted by this.
-	//      This connection is what the sensor actually detects. TODO: Can we improove this?
+	//      This connection is what the sensor actually detects. TODO: Can we improve this?
 	//      The UNLOCKED sensor, however will be uneffected, leaving the lock sensors in an
 	//      undefined (neither locked nor unlocked) state. We trigger an alarm if this occurs when the door is open.
 	//      Unfortunately the UNLOCKED has shown failures in the past, possibly causing a false alarm.
@@ -136,6 +168,7 @@ uint8_t door_get_status(void){
 void unlock(void){
 	door_command_pulse_time = DOOR_COMMAND_PULSE_TIME;
 	door_command_wait_timeout = DOOR_COMMAND_WAIT_TIMEOUT;
+	door_command_retry_count = 0;
 	
 	door_status = (door_status & (uint8_t)~DOOR_STATUS_LOCKING) | DOOR_STATUS_UNLOCKING;
 	DOOR_PORT   = (DOOR_PORT   & (uint8_t)~COMMAND_LOCK_PIN)    | COMMAND_UNLOCK_PIN;
@@ -144,6 +177,9 @@ void unlock(void){
 void lock(void){
 	door_command_pulse_time = DOOR_COMMAND_PULSE_TIME;
 	door_command_wait_timeout = DOOR_COMMAND_WAIT_TIMEOUT;
+	door_command_retry_count = 0;
+	
+// 	printf_P(PSTR("."));
 	
 	door_status = (door_status & (uint8_t)~DOOR_STATUS_UNLOCKING) | DOOR_STATUS_LOCKING;
 	DOOR_PORT   = (DOOR_PORT   & (uint8_t)~COMMAND_UNLOCK_PIN)    | COMMAND_LOCK_PIN;
@@ -158,34 +194,47 @@ uint8_t toggle_lock_unlock( void (*msg_target)(const char*) ){
 }
 
 uint8_t unlock_checked( void (*msg_target)(const char*) ){
+	uint8_t ok = 0;
+	const char* msg;
+	
 	if (door_status & DOOR_STATUS_UNLOCKED){
-		msg_target(PSTR("Already unlocked\n"));
+		msg = PSTR("Already unlocked\n");
 	} else if (door_status & DOOR_STATUS_LOCKING){
-		msg_target(PSTR("Can't unlock while locking\n"));
+		msg = PSTR("Can't unlock while locking\n");
 	} else {
 		unlock();
-		msg_target(PSTR("Unlocking...\n"));
-		return 1;
+		msg = PSTR("Unlocking...\n");
+		ok = 1;
 	}
-	return 0;
+	
+	printf_P(msg);
+	if (msg_target) msg_target(msg);
+	
+	return ok;
 }
 
 
 uint8_t lock_checked( void (*msg_target)(const char*) ){
+	uint8_t ok = 0;
+	const char* msg;
 	if (is_alarm_status(door_status)){
-		msg_target(PSTR("Can't lock in alarm state\n"));
+		msg = PSTR("Can't lock in alarm state\n");
 	} else if (door_status & DOOR_STATUS_UNLOCKING) {
-		msg_target(PSTR("Can't lock while unlocking\n"));
+		msg = PSTR("Can't lock while unlocking\n");
 	} else if (door_status & DOOR_STATUS_OPEN){
-		msg_target(PSTR("Can't lock while door open\n"));
+		msg = PSTR("Can't lock while door open\n");
 	} else if (door_status & DOOR_STATUS_LOCKED){
-		msg_target(PSTR("Already locked\n"));
+		msg = PSTR("Already locked\n");
 	} else {
 		lock();
-		msg_target(PSTR("Locking...\n"));
-		return 1;
+		msg = PSTR("Locking...\n");
+		ok = 1;
 	}
-	return 0;
+	
+	printf_P(msg);
+	if (msg_target) msg_target(msg);
+	
+	return ok;
 }
 
 uint8_t door_clear_alarm(void){
diff --git a/Control/log.c b/Control/log.c
index c2ce307ca0595b2253600904f1e760a05025c132..2158ae36a3f34f80c7d637fdd419dd42645924d9 100644
--- a/Control/log.c
+++ b/Control/log.c
@@ -5,16 +5,17 @@
 #include "door.h"
 #include "power_monitor.h"
 
-const char log_event_startup[]              PROGMEM = "== System Started ==";
-const char log_event_alarm_raised[]         PROGMEM = "Alarm Raised";
-const char log_event_alarm_changed[]        PROGMEM = "State change during Alarm";
-const char log_event_alarm_login[]          PROGMEM = "Login during Alarm";
-const char log_event_alarm_disabled[]       PROGMEM = "Alarm disabled";
-const char log_event_ac_fail[]              PROGMEM = "AC Power Fail";
-const char log_event_ac_return[]            PROGMEM = "AC Power Restored";
-const char log_event_main_battery_low[]     PROGMEM = "Main Battery Low";
-const char log_event_keymatic_battery_low[] PROGMEM = "Keymatic Battery Low";
-const char log_event_door_command_timeout[] PROGMEM = "Door command Timeout";
+const char log_event_startup[]                 PROGMEM = "== System Started ==";
+const char log_event_alarm_raised[]            PROGMEM = "Alarm Raised";
+const char log_event_alarm_changed[]           PROGMEM = "State change during Alarm";
+const char log_event_alarm_login[]             PROGMEM = "Login during Alarm";
+const char log_event_alarm_disabled[]          PROGMEM = "Alarm disabled";
+const char log_event_ac_fail[]                 PROGMEM = "AC Power Fail";
+const char log_event_ac_return[]               PROGMEM = "AC Power Restored";
+const char log_event_main_battery_low[]        PROGMEM = "Main Battery Low";
+const char log_event_keymatic_battery_low[]    PROGMEM = "Keymatic Battery Low";
+const char log_event_door_command_timeout[]    PROGMEM = "Door command Timeout";
+const char log_event_door_command_retry_fail[] PROGMEM = "Door command retry failed";
 
 const char* const event_names[] PROGMEM = {log_event_startup,
                                log_event_alarm_raised,
@@ -25,7 +26,8 @@ const char* const event_names[] PROGMEM = {log_event_startup,
                                log_event_ac_return,
                                log_event_main_battery_low,
                                log_event_keymatic_battery_low,
-                               log_event_door_command_timeout};
+                               log_event_door_command_timeout,
+                               log_event_door_command_retry_fail};
 
 uint8_t log_get_first_entry_index(void){
 	uint8_t l = eeprom_read_byte(LOG_START_POINTER_LOCATION);
diff --git a/Control/log.h b/Control/log.h
index 111d446ee20b1c978ef4fdb04d17f6019ee3b9a9..67825eb266c7b72df5abe86125eb9f87c58a82d4 100644
--- a/Control/log.h
+++ b/Control/log.h
@@ -20,16 +20,17 @@ struct log_entry_t{
 #define LOG_START_POINTER_LOCATION ((void*)(EEPROM_SIZE-1))
 #define LOG_END_POINTER_LOCATION   ((void*)(EEPROM_SIZE-2))
 
-#define LOG_EVENT_STARTUP              0x00
-#define LOG_EVENT_ALARM_RAISED         0x01
-#define LOG_EVENT_ALARM_CHANGED        0x02
-#define LOG_EVENT_ALARM_LOGIN          0x03
-#define LOG_EVENT_ALARM_DISABLED       0x04
-#define LOG_EVENT_AC_FAIL              0x05
-#define LOG_EVENT_AC_RETURN            0x06
-#define LOG_EVENT_MAIN_BATTERY_LOW     0x07
-#define LOG_EVENT_KEYMATIC_BATTERY_LOW 0x08
-#define LOG_EVENT_DOOR_COMMAND_TIMEOUT 0x09
+#define LOG_EVENT_STARTUP                 0x00
+#define LOG_EVENT_ALARM_RAISED            0x01
+#define LOG_EVENT_ALARM_CHANGED           0x02
+#define LOG_EVENT_ALARM_LOGIN             0x03
+#define LOG_EVENT_ALARM_DISABLED          0x04
+#define LOG_EVENT_AC_FAIL                 0x05
+#define LOG_EVENT_AC_RETURN               0x06
+#define LOG_EVENT_MAIN_BATTERY_LOW        0x07
+#define LOG_EVENT_KEYMATIC_BATTERY_LOW    0x08
+#define LOG_EVENT_DOOR_COMMAND_TIMEOUT    0x09
+#define LOG_EVENT_DOOR_COMMAND_RETRY_FAIL 0x0A
 
 #define LOG_EVENT_NONE                 0xFF
 
diff --git a/Control/shell/shell.c b/Control/shell/shell.c
index 15f840b439455cf3964be5181917156c9330c8cd..158f3540788fabc21e8fc91bec7d7d105b039127 100644
--- a/Control/shell/shell.c
+++ b/Control/shell/shell.c
@@ -302,14 +302,14 @@ void cmd_lock(readline_parsed_cmd_t* cmd){
 	if ((cmd->num_args > 0) && (strcmp(cmd->args[0], "-f") == 0)){
 		lock();
 	} else {
-		lock_checked(print_P);
+		lock_checked(0);
 	}
 }
 void cmd_unlock(readline_parsed_cmd_t* cmd){
 	if ((cmd->num_args > 0) && (strcmp(cmd->args[0], "-f") == 0)){
 		unlock();
 	} else {
-		unlock_checked(print_P);
+		unlock_checked(0);
 	}
 }
 
diff --git a/Main.sch b/Main.sch
index 1b57a6ecfc1fbec248546b8eec3c9aaee7ee1803..ba90b2189e40286d959cc8f4cbbcaa060dc26518 100644
--- a/Main.sch
+++ b/Main.sch
@@ -1,4 +1,4 @@
-EESchema Schematic File Version 2  date Mi 04 Mai 2011 00:11:36 CEST
+EESchema Schematic File Version 2  date Do 15 Mär 2012 14:16:25 CET
 LIBS:power
 LIBS:device
 LIBS:transistors
@@ -35,7 +35,7 @@ EELAYER END
 $Descr A4 11700 8267
 Sheet 1 1
 Title ""
-Date "3 may 2011"
+Date "15 mar 2012"
 Rev ""
 Comp ""
 Comment1 ""
@@ -43,521 +43,710 @@ Comment2 ""
 Comment3 ""
 Comment4 ""
 $EndDescr
-Connection ~ 5350 5650
 Wire Wire Line
-	5350 5350 5350 5650
+	6600 3300 6600 3100
 Wire Wire Line
-	3350 5650 2250 5650
+	6600 3100 6600 3000
 Wire Wire Line
-	2250 5650 2250 5000
+	8100 5750 8000 5750
 Wire Wire Line
-	2250 5000 2150 5000
+	8000 5750 7900 5750
+Connection ~ 7900 5150
 Wire Wire Line
-	550  5450 550  5400
+	7900 5250 7900 5150
+Connection ~ 2550 1400
 Wire Wire Line
-	550  5400 650  5400
+	2550 1150 2550 1400
 Wire Wire Line
-	650  5400 650  5500
+	2550 1400 2550 1500
+Connection ~ 3150 1400
 Wire Wire Line
-	650  5500 950  5500
+	2950 1400 3150 1400
+Connection ~ 3250 1400
 Wire Wire Line
-	950  5400 750  5400
+	3450 1400 3250 1400
 Wire Wire Line
-	750  5400 750  6350
+	3350 1150 3450 1150
 Wire Wire Line
-	750  6350 2400 6350
+	3350 1150 3350 1500
+Connection ~ 3150 5500
 Wire Wire Line
-	5650 6000 5650 5950
+	3000 5550 3000 5500
+Connection ~ 5250 5550
 Wire Wire Line
-	5650 5950 5550 5950
-Connection ~ 4450 6450
+	5250 5350 5250 5550
 Wire Wire Line
-	4450 6250 4450 6550
-Connection ~ 8450 2000
+	1850 5200 1850 5500
 Wire Wire Line
-	8450 2300 8450 1950
-Connection ~ 6200 2800
+	1850 5500 1750 5500
 Wire Wire Line
-	6200 2700 6200 3000
-Connection ~ 5200 3100
+	2400 6150 2050 6150
 Wire Wire Line
-	5450 3200 5450 3100
+	2050 6150 2050 5300
 Wire Wire Line
-	5450 3100 5000 3100
+	2050 5300 1750 5300
 Wire Wire Line
-	6150 7600 6150 5550
+	5800 6950 5800 6900
 Wire Wire Line
-	5550 5400 5550 5350
+	5800 6900 5650 6900
+Connection ~ 4550 7400
 Wire Wire Line
-	6400 1350 6400 2100
+	4550 7300 4550 7400
 Wire Wire Line
-	6400 2100 4200 2100
-Connection ~ 7550 6200
-Connection ~ 7350 6200
+	4550 7400 4550 7500
 Wire Wire Line
-	7700 6200 7350 6200
-Connection ~ 6050 6450
+	5700 7700 5700 7700
 Wire Wire Line
-	6050 6450 6650 6450
+	4200 4900 4500 4900
 Wire Wire Line
-	6200 1350 6100 1350
+	4500 4900 4500 6150
 Wire Wire Line
-	6100 1350 6100 1450
+	4500 6150 4400 6150
 Wire Wire Line
-	3250 5500 3000 5500
-Connection ~ 3150 5500
+	4400 6150 4400 6250
 Wire Wire Line
-	1900 550  3250 550 
-Connection ~ 2200 550 
+	4400 6250 4000 6250
+Connection ~ 8350 2400
 Wire Wire Line
-	6650 7050 6650 7150
+	8350 2400 8350 2250
 Wire Wire Line
-	7350 6650 7450 6650
+	8350 2250 8250 2250
 Wire Wire Line
-	8150 6200 8150 6450
+	8250 2250 8250 2300
 Wire Wire Line
-	8150 6450 7350 6450
+	8050 2400 8350 2400
 Wire Wire Line
-	6650 6650 6600 6650
-Connection ~ 7800 4500
+	8350 2400 8450 2400
 Wire Wire Line
-	7800 4500 7850 4500
+	4700 1900 5000 1900
+Connection ~ 6200 2900
 Wire Wire Line
-	7850 4500 7850 4550
+	5450 3400 5300 3400
 Wire Wire Line
-	7800 4250 7800 4650
+	5300 3400 5300 3500
 Wire Wire Line
-	7800 4650 8200 4650
+	5300 3500 4200 3500
 Wire Wire Line
-	7800 2300 7800 3750
+	4200 3500 4200 3400
 Wire Wire Line
-	7800 2300 4200 2300
+	6050 5650 6050 6450
 Wire Wire Line
-	5400 2800 5300 2800
+	6050 6450 6050 6550
 Wire Wire Line
-	5400 3000 5100 3000
+	6050 6550 6050 7500
 Wire Wire Line
-	7250 2600 7050 2600
+	5850 6350 5850 5850
 Wire Wire Line
-	7050 2600 7050 2400
-Connection ~ 6600 2500
+	5850 5850 4800 5850
 Wire Wire Line
-	6600 2600 6600 2500
-Connection ~ 6500 3100
+	4800 5850 4800 4600
 Wire Wire Line
-	6500 3300 6500 3000
+	4800 4600 4200 4600
+Connection ~ 6150 6350
 Wire Wire Line
-	6500 3000 6200 3000
+	6650 6350 6150 6350
 Wire Wire Line
-	4400 2900 4400 2600
+	4200 2200 6300 2200
 Wire Wire Line
-	4400 2900 4200 2900
+	6300 2200 6300 1350
 Wire Wire Line
-	5200 2800 5200 2900
+	2250 5800 2250 5950
 Wire Wire Line
-	5200 2800 4600 2800
+	2250 5800 3350 5800
 Wire Wire Line
-	5500 4550 5650 4550
+	2200 2350 2200 2600
 Wire Wire Line
-	1800 3000 1600 3000
+	8050 6650 8050 6550
 Wire Wire Line
-	1700 6900 1700 6850
+	8050 6550 7350 6550
 Wire Wire Line
-	7050 5700 7050 5800
+	7200 6200 7200 6100
 Wire Wire Line
-	7250 5700 7250 5800
+	7200 6100 7350 6100
 Wire Wire Line
-	7500 4950 7500 5800
+	7350 6100 7350 6200
 Wire Wire Line
-	7700 6000 6950 6000
-Connection ~ 8200 4950
+	7350 6200 7350 6350
 Wire Wire Line
-	7500 4950 8900 4950
+	8050 2000 8450 2000
 Wire Wire Line
-	6950 5000 6950 4950
+	8200 4550 7950 4550
 Wire Wire Line
-	7150 5000 7150 4950
+	7950 4550 7950 4250
 Wire Wire Line
-	7250 5000 7250 4700
+	5400 2600 4400 2600
 Wire Wire Line
-	7250 4700 6400 4700
-Connection ~ 6600 3100
-Connection ~ 3050 550 
+	5400 2900 5200 2900
+Connection ~ 7400 3100
 Wire Wire Line
-	2200 1000 2200 550 
+	7250 3100 7400 3100
 Wire Wire Line
-	2550 1400 2550 1500
-Connection ~ 3150 1400
+	4200 2400 7050 2400
+Connection ~ 6800 2500
 Wire Wire Line
-	2950 1400 3150 1400
-Connection ~ 3250 550 
-Connection ~ 5650 4350
-Connection ~ 5000 4350
+	6800 2500 6800 2600
 Wire Wire Line
-	5150 4350 5000 4350
+	7050 3100 6600 3100
 Wire Wire Line
-	1850 7450 1850 7250
+	6600 3100 6500 3100
 Wire Wire Line
-	6150 7600 5950 7600
+	6500 3100 6350 3100
 Wire Wire Line
-	6150 5550 5100 5550
+	6350 3100 6350 3250
 Wire Wire Line
-	5100 5550 5100 4200
+	8450 2600 7400 2600
 Wire Wire Line
-	5950 7500 6050 7500
+	5300 2700 5300 2800
 Wire Wire Line
-	5000 5650 6050 5650
+	5300 2700 5200 2700
 Wire Wire Line
-	5000 5650 5000 4300
+	5100 2900 5100 3000
 Wire Wire Line
-	5000 4300 4200 4300
-Connection ~ 5000 3300
+	5100 2900 4500 2900
 Wire Wire Line
-	5000 3000 5000 3400
+	5650 4200 5650 4350
 Wire Wire Line
-	3850 5800 4700 5800
+	5650 4350 5650 4650
 Wire Wire Line
-	4700 5800 4700 4700
+	3250 1500 3250 1400
 Wire Wire Line
-	4700 4700 4200 4700
+	3250 1400 3250 1250
 Wire Wire Line
-	1700 5950 2250 5950
+	4700 2500 6600 2500
 Wire Wire Line
-	1700 6250 2400 6250
+	6600 2500 6800 2500
 Wire Wire Line
-	5700 4100 5700 4850
+	6800 2500 8450 2500
 Wire Wire Line
-	5700 4100 4200 4100
+	2400 6850 2000 6850
 Wire Wire Line
-	6500 3900 4200 3900
+	2000 6850 1850 6850
 Wire Wire Line
-	6500 3700 4200 3700
+	1850 6850 1700 6850
+Connection ~ 7250 5800
 Wire Wire Line
-	6600 3100 6600 3000
+	7500 5800 7250 5800
 Wire Wire Line
-	7800 3750 7950 3750
+	7250 5800 7050 5800
 Wire Wire Line
-	5700 4850 5900 4850
-Connection ~ 6300 4550
-Connection ~ 6300 3800
+	6950 5700 6950 6000
+Connection ~ 6950 4950
 Wire Wire Line
-	6300 4150 6300 3800
-Connection ~ 6000 3600
+	6750 4950 6950 4950
 Wire Wire Line
-	6000 4150 6000 3600
+	6950 4950 7150 4950
 Wire Wire Line
-	7600 5900 7600 5050
+	6750 4950 6750 5050
 Wire Wire Line
-	7600 5900 7150 5900
+	7050 5000 7050 4850
 Wire Wire Line
-	7150 5900 7150 5700
+	7050 4850 6400 4850
 Wire Wire Line
-	7500 4250 7600 4250
+	6550 4400 6550 4550
 Wire Wire Line
-	7600 3750 7600 1600
+	3150 1500 3150 1400
 Wire Wire Line
-	5450 1600 5350 1600
+	3150 1400 3150 1150
+Connection ~ 5100 4200
 Wire Wire Line
-	5350 1600 5350 1650
+	5150 4200 5100 4200
 Wire Wire Line
-	2400 7450 1700 7450
+	5100 4200 4200 4200
+Connection ~ 1850 6850
+Connection ~ 2000 6850
+Connection ~ 1850 7450
 Wire Wire Line
-	2000 7450 2000 7050
+	7400 2600 7400 3100
 Wire Wire Line
-	2000 6850 2000 6550
+	7400 3100 7400 3750
 Wire Wire Line
-	4000 6050 4400 6050
+	7400 3750 7400 4750
+Connection ~ 6150 6650
 Wire Wire Line
-	4400 6050 4400 5000
+	6150 6650 5850 6650
+Connection ~ 6050 6550
 Wire Wire Line
-	4400 5000 4200 5000
+	5850 6550 6050 6550
 Wire Wire Line
-	4000 6150 4200 6150
+	5950 7300 5950 5750
 Wire Wire Line
-	4200 6150 4200 5200
+	5950 5750 4900 5750
 Wire Wire Line
-	3000 5500 3000 5550
+	4900 5750 4900 4500
 Wire Wire Line
-	1350 2350 1350 2300
-Connection ~ 1550 2300
+	4900 4500 4200 4500
+Connection ~ 4450 6350
+Connection ~ 5000 3200
 Wire Wire Line
-	1350 2300 1550 2300
+	1850 4750 1850 4800
 Wire Wire Line
-	1950 2600 2200 2600
+	4200 4800 4600 4800
 Wire Wire Line
-	1600 3000 1600 3100
+	4600 4800 4600 5650
 Wire Wire Line
-	4500 2800 4500 2900
+	4600 5650 3850 5650
 Wire Wire Line
-	4500 2800 4200 2800
+	1850 6450 1850 6050
 Wire Wire Line
-	4200 2700 4600 2700
+	1700 6150 1950 6150
 Wire Wire Line
-	4600 2700 4600 2800
+	5900 4000 5900 4700
 Wire Wire Line
-	2200 1500 2200 1800
+	5900 4000 4200 4000
 Wire Wire Line
-	1950 2000 2200 2000
+	6600 3800 6400 3800
 Wire Wire Line
-	2200 2000 2200 2200
+	6400 3800 4200 3800
 Wire Wire Line
-	1550 2000 1550 2600
+	6600 3600 6100 3600
 Wire Wire Line
-	2200 1800 2100 1800
+	6100 3600 4200 3600
 Wire Wire Line
-	4200 5100 4300 5100
+	5850 1600 7600 1600
+Connection ~ 8200 5050
 Wire Wire Line
-	4300 5100 4300 6350
+	8900 5050 8200 5050
 Wire Wire Line
-	4300 6350 4000 6350
+	8200 5050 8100 5050
 Wire Wire Line
-	1700 7450 1700 7350
-Connection ~ 2000 7450
+	8100 5050 7600 5050
+Connection ~ 6550 4550
 Wire Wire Line
-	1700 6050 1850 6050
+	6800 4450 6800 4400
 Wire Wire Line
-	2400 6050 1950 6050
+	6800 4400 6700 4400
 Wire Wire Line
-	1950 6050 1950 6150
-Connection ~ 5000 3100
+	6700 4400 6700 4550
 Wire Wire Line
-	5200 3150 5200 3100
+	6700 4550 6550 4550
 Wire Wire Line
-	4700 1800 4700 1300
+	6550 4550 6400 4550
 Wire Wire Line
-	4700 1300 5650 1300
+	6400 4550 6250 4550
 Wire Wire Line
-	7600 4250 7600 4450
+	6250 4550 6100 4550
 Wire Wire Line
-	7500 3750 7400 3750
-Connection ~ 7400 3750
+	5900 4700 5900 4700
+Connection ~ 6250 4550
+Connection ~ 6550 3900
 Wire Wire Line
-	7800 4750 8200 4750
+	6550 4000 6550 3900
+Connection ~ 6250 3700
+Wire Wire Line
+	6250 4000 6250 3700
 Wire Wire Line
-	8200 5150 7700 5150
+	6250 4400 6250 4550
 Wire Wire Line
 	7700 5150 7700 6000
 Wire Wire Line
-	6150 4400 6150 4550
+	7700 5150 7900 5150
 Wire Wire Line
-	6150 4000 6150 3700
-Connection ~ 6150 3700
+	7900 5150 8200 5150
 Wire Wire Line
-	6450 4000 6450 3900
-Connection ~ 6450 3900
-Connection ~ 6150 4550
+	7800 4750 8200 4750
+Connection ~ 7400 3750
 Wire Wire Line
-	5900 4700 5800 4700
+	7500 3750 7400 3750
 Wire Wire Line
-	6000 4550 6600 4550
+	7600 4450 7600 4250
 Wire Wire Line
-	6600 4550 6600 4400
+	5650 1300 4700 1300
 Wire Wire Line
-	6600 4400 6700 4400
+	4700 1300 4700 1800
 Wire Wire Line
-	6700 4400 6700 4450
-Connection ~ 6450 4550
+	5200 3150 5200 3100
+Connection ~ 5000 3100
 Wire Wire Line
-	7600 5050 8900 5050
-Connection ~ 8200 5050
+	1950 6150 1950 6050
 Wire Wire Line
-	7600 1600 5850 1600
+	1950 6050 2400 6050
 Wire Wire Line
-	6500 3600 4200 3600
+	1850 6050 1700 6050
+Connection ~ 2000 7450
 Wire Wire Line
-	6500 3800 4200 3800
+	1700 7350 1700 7450
 Wire Wire Line
-	4200 4000 5800 4000
+	4000 6350 4300 6350
 Wire Wire Line
-	5800 4000 5800 4700
+	4300 6350 4300 5100
 Wire Wire Line
-	1950 6150 1700 6150
+	4300 5100 4200 5100
 Wire Wire Line
-	1850 6050 1850 6450
+	2100 1800 2200 1800
 Wire Wire Line
-	3850 5650 4600 5650
+	1550 2000 1550 2300
 Wire Wire Line
-	4600 5650 4600 4800
+	1550 2300 1550 2600
 Wire Wire Line
-	4600 4800 4200 4800
+	2200 2200 2200 2000
 Wire Wire Line
-	1850 4750 1850 4800
-Connection ~ 5000 3200
-Connection ~ 4450 6350
+	2200 2000 1950 2000
 Wire Wire Line
-	4200 4500 4900 4500
+	2200 1800 2200 1500
 Wire Wire Line
-	4900 4500 4900 5750
+	4600 2700 4600 2800
 Wire Wire Line
-	4900 5750 5950 5750
+	4600 2700 4200 2700
 Wire Wire Line
-	5950 5750 5950 7300
+	4200 2800 4500 2800
 Wire Wire Line
-	5850 6550 6050 6550
-Connection ~ 6050 6550
+	4500 2800 4500 2900
 Wire Wire Line
-	5850 6650 6150 6650
-Connection ~ 6150 6650
+	1600 3100 1600 3000
 Wire Wire Line
-	7400 4750 7400 2600
-Connection ~ 1850 7450
-Connection ~ 2000 6850
-Connection ~ 1850 6850
+	2200 2600 1950 2600
 Wire Wire Line
-	5150 4200 4200 4200
-Connection ~ 5100 4200
+	1550 2300 1350 2300
+Connection ~ 1550 2300
 Wire Wire Line
-	3450 1400 3250 1400
+	1350 2300 1350 2350
 Wire Wire Line
-	3150 1500 3150 1150
+	4200 5200 4200 6150
 Wire Wire Line
-	3150 1150 3050 1150
+	4200 6150 4000 6150
 Wire Wire Line
-	3050 1150 3050 550 
+	4200 5000 4400 5000
 Wire Wire Line
-	6450 4400 6450 4550
+	4400 5000 4400 6050
 Wire Wire Line
-	6400 4850 7050 4850
+	4400 6050 4000 6050
 Wire Wire Line
-	7050 4850 7050 5000
+	2000 6850 2000 6550
 Wire Wire Line
-	6750 5050 6750 4950
+	2000 7450 2000 7050
 Wire Wire Line
-	6750 4950 7150 4950
-Connection ~ 6950 4950
+	1700 7450 1850 7450
 Wire Wire Line
-	6950 6000 6950 5700
+	1850 7450 2000 7450
 Wire Wire Line
-	7500 5800 7050 5800
-Connection ~ 7250 5800
+	2000 7450 2400 7450
 Wire Wire Line
-	1700 6850 2400 6850
+	5350 1650 5350 1600
 Wire Wire Line
-	4700 2500 8450 2500
+	5350 1600 5450 1600
 Wire Wire Line
-	3250 1500 3250 1250
-Connection ~ 3250 1400
+	7600 1600 7600 3750
 Wire Wire Line
-	5650 4550 5650 4200
+	7600 4250 7500 4250
 Wire Wire Line
-	4500 2900 5100 2900
+	7150 5700 7150 5900
 Wire Wire Line
-	5100 2900 5100 3000
+	7150 5900 7600 5900
 Wire Wire Line
-	5200 2700 5300 2700
+	7600 5900 7600 5050
 Wire Wire Line
-	5300 2700 5300 2800
+	6100 4150 6100 3600
+Connection ~ 6100 3600
 Wire Wire Line
-	7400 2600 8450 2600
+	6400 4150 6400 3800
+Connection ~ 6400 3800
+Connection ~ 6400 4550
 Wire Wire Line
-	6350 3250 6350 3100
+	5900 4850 5800 4850
 Wire Wire Line
-	6350 3100 7050 3100
+	7950 3750 7800 3750
 Wire Wire Line
-	6800 2500 6800 2600
-Connection ~ 6800 2500
+	6600 3700 6250 3700
 Wire Wire Line
-	7050 2400 4200 2400
+	6250 3700 4200 3700
 Wire Wire Line
-	7250 3100 7400 3100
-Connection ~ 7400 3100
+	6600 3900 6550 3900
 Wire Wire Line
-	5200 2900 5400 2900
+	6550 3900 4200 3900
 Wire Wire Line
-	4400 2600 5400 2600
+	4200 4100 5800 4100
 Wire Wire Line
-	7950 4250 7950 4550
+	5800 4100 5800 4850
 Wire Wire Line
-	7950 4550 8200 4550
+	1700 6250 2400 6250
 Wire Wire Line
-	8050 2000 8450 2000
+	2250 5950 1700 5950
 Wire Wire Line
-	7350 6350 7350 6100
+	4200 4700 4700 4700
 Wire Wire Line
-	7350 6100 7200 6100
+	4700 4700 4700 5800
 Wire Wire Line
-	7200 6100 7200 6200
+	4700 5800 3850 5800
 Wire Wire Line
-	7350 6550 8050 6550
+	5000 3000 5000 3100
 Wire Wire Line
-	8050 6550 8050 6650
+	5000 3100 5000 3200
 Wire Wire Line
-	3850 1400 3850 1500
+	5000 3200 5000 3300
 Wire Wire Line
-	2200 2600 2200 2350
+	5000 3300 5000 3400
+Connection ~ 5000 3300
 Wire Wire Line
-	3350 5800 2250 5800
+	4200 4300 5000 4300
 Wire Wire Line
-	2250 5800 2250 5950
+	5000 4300 5000 4350
 Wire Wire Line
-	6300 1350 6300 2200
+	5000 4350 5000 5650
 Wire Wire Line
-	6300 2200 4200 2200
+	5000 5650 5350 5650
 Wire Wire Line
-	6150 6350 6650 6350
-Connection ~ 6150 6350
+	5350 5650 6050 5650
+Wire Wire Line
+	6050 7500 5950 7500
 Wire Wire Line
-	4200 4600 4800 4600
+	5100 4200 5100 5550
 Wire Wire Line
-	4800 4600 4800 5850
+	5100 5550 5250 5550
 Wire Wire Line
-	4800 5850 5850 5850
+	5250 5550 6150 5550
 Wire Wire Line
-	5850 5850 5850 6350
+	5950 7600 6150 7600
 Wire Wire Line
-	6050 5650 6050 7500
+	1850 7450 1850 7250
 Wire Wire Line
-	4200 3400 4200 3500
+	5150 4350 5000 4350
+Connection ~ 5000 4350
+Connection ~ 5650 4350
 Wire Wire Line
-	4200 3500 5300 3500
+	2200 1000 2200 550 
+Connection ~ 3050 550 
+Connection ~ 6600 3100
 Wire Wire Line
-	5300 3500 5300 3400
+	6400 4700 7250 4700
 Wire Wire Line
-	5300 3400 5450 3400
-Connection ~ 6200 2900
+	7250 4700 7250 5000
 Wire Wire Line
-	4700 1900 5000 1900
+	7150 4950 7150 5000
 Wire Wire Line
-	8050 2400 8450 2400
+	6950 5000 6950 4950
 Wire Wire Line
-	8250 2300 8250 2250
+	8900 4950 8200 4950
 Wire Wire Line
-	8250 2250 8350 2250
+	8200 4950 7500 4950
+Connection ~ 8200 4950
 Wire Wire Line
-	8350 2250 8350 2400
-Connection ~ 8350 2400
+	7700 6000 6950 6000
 Wire Wire Line
-	4000 6250 4400 6250
+	7500 4950 7500 5800
 Wire Wire Line
-	4400 6250 4400 6150
+	7250 5700 7250 5800
 Wire Wire Line
-	4400 6150 4500 6150
+	7050 5800 7050 5700
 Wire Wire Line
-	4500 6150 4500 4900
+	1700 6850 1700 6900
 Wire Wire Line
-	4500 4900 4200 4900
+	1600 3000 1800 3000
 Wire Wire Line
-	5700 7700 5700 7700
+	5650 4650 5500 4650
 Wire Wire Line
-	4550 7300 4550 7500
-Connection ~ 4550 7400
+	4600 2800 5200 2800
 Wire Wire Line
-	5650 6900 5800 6900
+	5200 2800 5200 2900
+Wire Wire Line
+	4200 2900 4400 2900
 Wire Wire Line
-	5800 6900 5800 6950
+	4400 2900 4400 2600
 Wire Wire Line
-	1750 5300 2050 5300
+	6500 3000 6200 3000
 Wire Wire Line
-	2050 5300 2050 6150
+	6500 3000 6500 3100
+Connection ~ 6500 3100
 Wire Wire Line
-	2050 6150 2400 6150
+	6600 2500 6600 2600
+Connection ~ 6600 2500
 Wire Wire Line
-	1750 5500 1850 5500
+	7050 2400 7050 2600
 Wire Wire Line
-	1850 5500 1850 5200
+	7050 2600 7250 2600
 Wire Wire Line
-	5250 5350 5250 5550
-Connection ~ 5250 5550
+	5100 3000 5400 3000
+Wire Wire Line
+	5300 2800 5400 2800
+Wire Wire Line
+	4200 2300 7800 2300
+Wire Wire Line
+	7800 2300 7800 3750
+Wire Wire Line
+	8200 4650 7800 4650
+Wire Wire Line
+	7800 4650 7800 4500
+Wire Wire Line
+	7800 4500 7800 4250
+Wire Wire Line
+	7850 4550 7850 4500
+Wire Wire Line
+	7850 4500 7800 4500
+Connection ~ 7800 4500
+Wire Wire Line
+	6650 6650 6600 6650
+Wire Wire Line
+	7350 6450 8150 6450
+Wire Wire Line
+	8150 6450 8150 6200
+Wire Wire Line
+	7350 6650 7450 6650
+Wire Wire Line
+	6650 7050 6650 7150
+Connection ~ 2200 550 
+Wire Wire Line
+	6100 1450 6100 1350
+Wire Wire Line
+	6100 1350 6200 1350
+Wire Wire Line
+	6050 6450 6650 6450
+Connection ~ 6050 6450
+Wire Wire Line
+	7350 6200 7550 6200
+Wire Wire Line
+	7550 6200 7700 6200
+Connection ~ 7350 6200
+Connection ~ 7550 6200
+Wire Wire Line
+	4200 2100 6400 2100
+Wire Wire Line
+	6400 2100 6400 1350
+Wire Wire Line
+	5550 5400 5550 5350
+Wire Wire Line
+	6150 7600 6150 6650
+Wire Wire Line
+	6150 6650 6150 6350
+Wire Wire Line
+	6150 6350 6150 5550
+Wire Wire Line
+	5000 3100 5200 3100
+Wire Wire Line
+	5200 3100 5450 3100
+Wire Wire Line
+	5450 3100 5450 3200
+Connection ~ 5200 3100
+Wire Wire Line
+	6200 3000 6200 2900
+Wire Wire Line
+	6200 2900 6200 2800
+Wire Wire Line
+	6200 2800 6200 2700
+Connection ~ 6200 2800
+Wire Wire Line
+	8450 2300 8450 2000
+Wire Wire Line
+	8450 2000 8450 1950
+Connection ~ 8450 2000
+Wire Wire Line
+	4450 6250 4450 6350
+Wire Wire Line
+	4450 6350 4450 6450
+Wire Wire Line
+	4450 6450 4450 6550
+Connection ~ 4450 6450
+Wire Wire Line
+	5550 5950 5650 5950
+Wire Wire Line
+	5650 5950 5650 6000
+Wire Wire Line
+	2400 6350 750  6350
+Wire Wire Line
+	750  6350 750  5400
+Wire Wire Line
+	750  5400 950  5400
+Wire Wire Line
+	950  5500 650  5500
+Wire Wire Line
+	650  5500 650  5400
+Wire Wire Line
+	650  5400 550  5400
+Wire Wire Line
+	550  5400 550  5450
+Wire Wire Line
+	2150 5000 2250 5000
+Wire Wire Line
+	2250 5000 2250 5650
+Wire Wire Line
+	2250 5650 3350 5650
+Wire Wire Line
+	5350 5350 5350 5650
+Connection ~ 5350 5650
+Connection ~ 3050 5500
+Wire Wire Line
+	3000 5500 3050 5500
+Wire Wire Line
+	3050 5500 3150 5500
+Wire Wire Line
+	3150 5500 3250 5500
+Wire Wire Line
+	3250 5500 3350 5500
+Connection ~ 3250 5500
+Wire Wire Line
+	1900 550  2200 550 
+Wire Wire Line
+	2200 550  3050 550 
+Wire Wire Line
+	3050 550  3250 550 
+Wire Wire Line
+	3250 550  3450 550 
+Wire Wire Line
+	3450 550  3450 1150
+Connection ~ 3250 550 
+Wire Wire Line
+	3150 1150 3050 1150
+Wire Wire Line
+	3050 1150 2950 1150
+Connection ~ 3050 1150
+Wire Wire Line
+	3050 1500 3050 1150
+Wire Wire Line
+	3050 1150 3050 550 
+Wire Wire Line
+	3850 1500 3850 1400
+Wire Wire Line
+	3850 1400 3850 1150
+Connection ~ 3850 1400
+Wire Wire Line
+	8100 5250 8100 5050
+Connection ~ 8100 5050
+Wire Wire Line
+	8000 5850 8000 5750
+Connection ~ 8000 5750
+$Comp
+L GND #PWR?
+U 1 1 4F61E51B
+P 8000 5850
+F 0 "#PWR?" H 8000 5850 30  0001 C CNN
+F 1 "GND" H 8000 5780 30  0001 C CNN
+	1    8000 5850
+	1    0    0    -1  
+$EndComp
+$Comp
+L R R24
+U 1 1 4F61E4F1
+P 8100 5500
+F 0 "R24" V 8180 5500 50  0000 C CNN
+F 1 "100k" V 8100 5500 50  0000 C CNN
+	1    8100 5500
+	1    0    0    -1  
+$EndComp
+$Comp
+L R R23
+U 1 1 4F61E4E6
+P 7900 5500
+F 0 "R23" V 7980 5500 50  0000 C CNN
+F 1 "100k" V 7900 5500 50  0000 C CNN
+	1    7900 5500
+	1    0    0    -1  
+$EndComp
+$Comp
+L C C6
+U 1 1 4F61DFF3
+P 3650 1150
+F 0 "C6" V 3700 1250 50  0000 L CNN
+F 1 "100nF" V 3550 1300 50  0000 L CNN
+	1    3650 1150
+	0    1    1    0   
+$EndComp
+$Comp
+L C C4
+U 1 1 4F61DFE9
+P 2750 1400
+F 0 "C4" V 2800 1500 50  0000 L CNN
+F 1 "100nF" V 2650 1550 50  0000 L CNN
+	1    2750 1400
+	0    1    1    0   
+$EndComp
 NoConn ~ 950  5700
 NoConn ~ 1750 5700
 NoConn ~ 1750 5400
@@ -844,8 +1033,8 @@ F 1 "RNG" V 6350 1000 50  0000 C CNN
 	1    6300 1000
 	0    -1   -1   0   
 $EndComp
-NoConn ~ 6500 3500
-NoConn ~ 6500 3400
+NoConn ~ 6600 3500
+NoConn ~ 6600 3400
 $Comp
 L R R22
 U 1 1 4D3F19C4
@@ -885,19 +1074,19 @@ $EndComp
 $Comp
 L C C5
 U 1 1 4D3F1433
-P 2750 1400
-F 0 "C5" V 2800 1500 50  0000 L CNN
-F 1 "100nF" V 2650 1550 50  0000 L CNN
-	1    2750 1400
+P 2750 1150
+F 0 "C5" V 2800 1250 50  0000 L CNN
+F 1 "100nF" V 2650 1300 50  0000 L CNN
+	1    2750 1150
 	0    1    1    0   
 $EndComp
 $Comp
 L +5V #PWR07
 U 1 1 4D39D7B1
-P 5500 4550
-F 0 "#PWR07" H 5500 4640 20  0001 C CNN
-F 1 "+5V" H 5500 4640 30  0000 C CNN
-	1    5500 4550
+P 5500 4650
+F 0 "#PWR07" H 5500 4740 20  0001 C CNN
+F 1 "+5V" H 5500 4740 30  0000 C CNN
+	1    5500 4650
 	1    0    0    -1  
 $EndComp
 $Comp
@@ -1004,7 +1193,7 @@ L BC307 Q3
 U 1 1 4D36290D
 P 1950 5000
 F 0 "Q3" V 2150 4900 50  0000 C CNN
-F 1 "PNP BC557" V 1800 5050 50  0000 C CNN
+F 1 "PNP BC328" V 1800 5050 50  0000 C CNN
 	1    1950 5000
 	-1   0    0    1   
 $EndComp
@@ -1101,46 +1290,46 @@ $EndComp
 $Comp
 L GND #PWR020
 U 1 1 4D094C1F
-P 6700 4450
-F 0 "#PWR020" H 6700 4450 30  0001 C CNN
-F 1 "GND" H 6700 4380 30  0001 C CNN
-	1    6700 4450
+P 6800 4450
+F 0 "#PWR020" H 6800 4450 30  0001 C CNN
+F 1 "GND" H 6800 4380 30  0001 C CNN
+	1    6800 4450
 	1    0    0    -1  
 $EndComp
 $Comp
 L C C16
 U 1 1 4D094AF6
-P 6000 4350
-F 0 "C16" H 5850 4400 50  0000 L CNN
-F 1 "100nF" H 5850 4250 50  0000 L CNN
-	1    6000 4350
+P 6100 4350
+F 0 "C16" H 5950 4400 50  0000 L CNN
+F 1 "100nF" H 5950 4250 50  0000 L CNN
+	1    6100 4350
 	1    0    0    -1  
 $EndComp
 $Comp
 L C C19
 U 1 1 4D094AC9
-P 6300 4350
-F 0 "C19" H 6450 4400 50  0000 L CNN
-F 1 "100nF" H 6400 4600 50  0000 L CNN
-	1    6300 4350
+P 6400 4350
+F 0 "C19" H 6550 4400 50  0000 L CNN
+F 1 "100nF" H 6500 4600 50  0000 L CNN
+	1    6400 4350
 	1    0    0    -1  
 $EndComp
 $Comp
 L C C17
 U 1 1 4D094AC6
-P 6150 4200
-F 0 "C17" H 6050 4100 50  0000 L CNN
-F 1 "100nF" H 6200 3950 50  0000 L CNN
-	1    6150 4200
+P 6250 4200
+F 0 "C17" H 6150 4100 50  0000 L CNN
+F 1 "100nF" H 6300 3950 50  0000 L CNN
+	1    6250 4200
 	1    0    0    -1  
 $EndComp
 $Comp
 L C C18
 U 1 1 4D094A8E
-P 6450 4200
-F 0 "C18" H 6500 4300 50  0000 L CNN
-F 1 "100nF" H 6600 4100 50  0000 L CNN
-	1    6450 4200
+P 6550 4200
+F 0 "C18" H 6600 4300 50  0000 L CNN
+F 1 "100nF" H 6700 4100 50  0000 L CNN
+	1    6550 4200
 	-1   0    0    1   
 $EndComp
 $Comp
@@ -1218,10 +1407,10 @@ $EndComp
 $Comp
 L CONN_7 JP7
 U 1 1 4D093CC0
-P 6850 3600
-F 0 "JP7" H 6850 3900 60  0000 C CNN
-F 1 "Sensors" V 6920 3600 60  0000 C CNN
-	1    6850 3600
+P 6950 3600
+F 0 "JP7" H 6950 3900 60  0000 C CNN
+F 1 "Sensors" V 7020 3600 60  0000 C CNN
+	1    6950 3600
 	1    0    0    -1  
 $EndComp
 $Comp
@@ -1482,7 +1671,7 @@ F 1 "MAX232" H 3200 5900 70  0000 C CNN
 	-1   0    0    1   
 $EndComp
 $Comp
-L ATMEGA32-P IC1
+L ATMEGA32-A IC1
 U 1 1 4D091A70
 P 3200 3500
 F 0 "IC1" H 2400 5330 50  0000 L BNN
diff --git a/common/serial.h b/common/serial.h
index 8efe63d7db68e5a4524a775be13440d4fc6ff749..16b1dace20a4ae0f0c0db7235f7b9a2f3f09d614 100644
--- a/common/serial.h
+++ b/common/serial.h
@@ -154,18 +154,15 @@ void FUNC_switch_baudrate(uint32_t baud){
 }
 
 void FUNC_switch_mode_receive(void){
-	//Wait for empty transmit buffer
-	while ( !( UCSRA & (1 << UDRE)) ) {;}
+	while ( !( UCSRA & (1 << TXC)) ) {;} //Wait for transmit complete
 	UCSRB &= ~(1<<TXEN);
-	_delay_ms(1);
 	UCSRB |=  (1<<RXEN);
-	while (UCSRA & (1 << RXC)){ //trash receive buffer
-		UDR;
-	}
+	while (UCSRA & (1 << RXC)){UDR;} //trash receive buffer
 }
 void FUNC_switch_mode_transmit(void){
 	UCSRB |=  (1<<TXEN);
 	UCSRB &= ~(1<<RXEN);
+	while (UCSRA & (1 << RXC)){UDR;} //trash receive buffer
 }