diff --git a/schaltungen/displayboard_servo/software/src/main.c b/schaltungen/displayboard_servo/software/src/main.c
index 9e02ce0..e01b768 100644
--- a/schaltungen/displayboard_servo/software/src/main.c
+++ b/schaltungen/displayboard_servo/software/src/main.c
@@ -10,6 +10,7 @@
 
 #define POWER_MIN	0
 #define POWER_MAX	400
+#define SERVO_STEPS	125
 
 #define BUFSIZE		32
 
@@ -36,13 +37,19 @@ void reset_input_buffer(void) {
 	data_count = 0;
 	memset(data_in, 0, BUFSIZE);
 }
+
+static void update_servo_position() {
+	uint32_t power;
+	power = ( (pd.voltage_gen / 100) * (pd.current_gen / 100) ) / 100;      
+	set_servo(power & 0xffff); 
+}
    
 static void timer_init(void) {
-	// clock is 8MHz
-	TCCR1B |= _BV(WGM12) | _BV(CS11) | _BV(CS10);       // CTC Mode for Timer 1 (16Bit) with prescale of 64
-    OCR1A = 2250;                           // Neutralposition ((2500-2312)*0.008ms)=1,5ms)
-	TIMSK = _BV(OCIE1A);
-	TCCR1A = (1<<COM1A0);                   // Togglen bei Compare Match
+	// CTC Mode for Timer 1 (16Bit) with prescale of 64
+	TCCR1B |= _BV(WGM12) | _BV(CS11) | _BV(CS10);       
+	OCR1A = 2250;           // set Servo to max. position
+	TIMSK |= _BV(OCIE1A);	// enable timer interrupt
+	TCCR1A |= (1<<COM1A0);   // toggle OCR1A on overflow
 }
 
 static void ports_init(void) {
@@ -50,86 +57,69 @@ static void ports_init(void) {
 }
 
 static void process_command(char *command) {
-	
-    if(command[0] == 'A') {
+	if(command[0] == 'A') {
 		// command must be in the following format:
-        // A$voltage_gen,$voltage_reg,$current_gen,loadsw,batsw,gensw\n
-        // examples: 
-        //	A24500,13400,12000,1,1,1B
-        //	A19900,11000,15000,1,1,1B
-        //	A20000,12300,10000,1,1,1B
+		// A$voltage_gen,$voltage_reg,$current_gen,loadsw,batsw,gensw\n
+		// examples: 
+		//	A24500,13400,12000,1,1,1B
+		//	A19900,11000,15000,1,1,1B
+		//	A20000,12300,10000,1,1,1B
 
-        char *token;
-        uint8_t tokencounter = 0;
+		char *token;
+		uint8_t tokencounter = 0;
 
-        token = strtok(command, ","); 
-        
-        while( token ) {
-            switch(tokencounter) {
-                case 0:
-                    pd.voltage_gen = atoi(++token);	// skip the A in front of the number
-                    break;
-                case 1:
-                    pd.voltage_reg = atoi(token);
-                    break;
-                case 2:
-                    pd.current_gen = atoi(token);
-                    break;
-                case 3:
-                    if(atoi(token) == 1) pd.loadsw = 1;
-                    else pd.loadsw = 0;
-                    break;
-                case 4:
-                    if(atoi(token) == 1) pd.batsw = 1;
-                    else pd.batsw = 0;
-                    break;
-                case 5:
-                    if(atoi(token) == 1) pd.gensw = 1;
-                    else pd.gensw = 0;
-                    break;
-            }
+		token = strtok(command, ","); 
 
-            tokencounter++;
-            token = strtok(NULL, ",");
-        }
-    }
+		while( token ) {
+			switch(tokencounter) {
+				case 0:
+					// skip the A in front of the number
+					pd.voltage_gen = atoi(++token);
+					break;
+				case 1:
+					pd.voltage_reg = atoi(token);
+					break;
+				case 2:
+					pd.current_gen = atoi(token);
+					break;
+				case 3:
+					if(atoi(token) == 1) pd.loadsw = 1;
+					else pd.loadsw = 0;
+					break;
+				case 4:
+					if(atoi(token) == 1) pd.batsw = 1;
+					else pd.batsw = 0;
+					break;
+				case 5:
+					if(atoi(token) == 1) pd.gensw = 1;
+					else pd.gensw = 0;
+					break;
+			}
+			
+			tokencounter++;
+			token = strtok(NULL, ",");
+		}
+
+		update_servo_position();        
+	}
 }
 
 static void work_uart() {
+	uint16_t c = uart_getc();
 
-    uint16_t c = uart_getc();
-
-    if ( !(c & UART_NO_DATA) ) {
-		
+	if ( !(c & UART_NO_DATA) ) {
 		data_in[data_count] = (c & 0xff);
-		
-		/*
-		uart_print_uint16(data_count);
-		uart_puts_P(" char: ");
-		uart_putc(c >> 8);
-		uart_putc(c & 0xff);
-		uart_puts_P(";\r\n");
-		*/
-		/*
-		uart_puts_P("data: ");
-		
-		for(uint8_t i = 0; i < BUFSIZE; i++) {
-			uart_putc(data_in[i]);
-		}
-		uart_puts_P("\r\n");
-		*/
 
-        if (data_in[data_count] == 'B') {
-            process_command(data_in);
-            
-            reset_input_buffer();
-        } 
-        
-        data_count++;
-		if(data_count >= BUFSIZE) {
+		if (data_in[data_count] == 'B') {
+			process_command(data_in);    
 			reset_input_buffer();
-		}   
-    }
+		} 
+
+		data_count++;
+		if(data_count >= BUFSIZE) {	// buffer overflow
+			reset_input_buffer();
+		}
+	}
 }
 
 /**
@@ -144,15 +134,22 @@ void set_servo(uint16_t display) {
 
 	display = POWER_MAX-display;	  // invert the value / servo
 
-    display = display * 10;   // shift, since we have to divide by 3,2 (32)
-    display = display / 32;   // instead of dividing by 3,2
-    display = display + 125;
+	display = display * 10;				// *10 otherwise we need float
+	display = display / ( (POWER_MAX*10) / SERVO_STEPS );
+	display = display + SERVO_STEPS;
 
-	cli();		// read and write atomic
-	if( display < 125 ) display = 125;
-	if( display > 250 ) display = 250;
-    OCR1A = 2500-display;
-    sei();    
+	uart_puts_P("display = ");
+	uart_print_uint16(display);
+	uart_puts_P("\r\n");
+
+	//cli();								// read and write atomic
+	TIMSK &= ~_BV(OCIE1A);
+
+	if( display < 125 ) display = 125;	// just make sure, the timer
+	if( display > 250 ) display = 250;  // is never out ouf 20ms grid
+	OCR1A = 2500-display;
+
+	TIMSK |= _BV(OCIE1A);
 }
 
 
@@ -174,25 +171,20 @@ int main(void) {
 	sei();
 	ports_init();
 	timer_init();
-    uart_init(UART_BAUD_SELECT(38400,F_CPU));
-    reset_input_buffer();
+	uart_init(UART_BAUD_SELECT(38400,F_CPU));
+	reset_input_buffer();
 	demo_display();
 	
-	uint32_t power;
-
+	
 	while(1) {
-	    work_uart();
+		work_uart();
 
-        if(syscounter >= 100) {	// about every two seconds
-			memset(data_in, 0, BUFSIZE);
-			data_count = 0;
-            uart_putc('a'); 	// send a to receive values from master box
-			       
-			power = ( (pd.voltage_gen / 100) * (pd.current_gen / 100) ) / 100;      
-            set_servo(power & 0xffff);
-            
-            syscounter = 0;
-        }
+		if((syscounter % 25) == 0) {	// about every 500ms
+			reset_input_buffer();
+			uart_putc('a'); 	// send a to receive values from master box
+		}
+		
+		syscounter %= 1000;
 	}
 	
 	return(0);
@@ -204,9 +196,9 @@ int main(void) {
  */
 ISR(TIMER1_COMPA_vect) {
 	syscounter++;
-    OCR1A = 2500-OCR1A;         // Das Servosignal wird aus der Differenz von
-                                // Periodenlänge (2500*0,008ms=20ms) und letztem
-                                // Vergleichswert (OCR1A) gebildet	
+	OCR1A = 2500-OCR1A;	// Das Servosignal wird aus der Differenz von
+						// Periodenlänge (2500*0,008ms=20ms) und letztem
+						// Vergleichswert (OCR1A) gebildet	
 }