diff --git a/schaltungen/displayboard_servo/software/src/main.c b/schaltungen/displayboard_servo/software/src/main.c
index 051907a..18eb5e8 100644
--- a/schaltungen/displayboard_servo/software/src/main.c
+++ b/schaltungen/displayboard_servo/software/src/main.c
@@ -14,6 +14,8 @@
 
 #define BUFSIZE		10
 
+#define DEBUG2
+
 volatile uint16_t syscounter = 0;
 volatile uint16_t power;
 uint8_t data_count = 0;
@@ -92,6 +94,7 @@ static void work_uart() {
  * 	\param	display 	The power value from 0 to 400 (including bounds)
  */
 void set_servo(uint16_t display) {
+	int diff;
 
 	if( display > POWER_MAX ) display = POWER_MAX;
 
@@ -121,7 +124,18 @@ void set_servo(uint16_t display) {
 
 	//cli();								// read and write atomic
 	TIMSK &= ~(_BV(OCIE1A));
-    OCR1A = 2500-display;
+	diff = OCR1A - (2500-display);
+    if(diff <=20 && diff >= -20) {
+    	OCR1A = 2500-display;
+	}
+	else {
+		if(diff <=20) {
+			OCR1A = OCR1A+5;
+		}
+		else if ( diff >= -20 ) {
+			OCR1A = OCR1A-5;
+		}
+	}	
     //sei();
     TIMSK |= _BV(OCIE1A);
 }