diff --git a/example/IMU/Motion_Correction/motion_correction/motion_correction.ino b/example/IMU/Motion_Correction/motion_correction/motion_correction.ino
index 58416df..95cc821 100644
--- a/example/IMU/Motion_Correction/motion_correction/motion_correction.ino
+++ b/example/IMU/Motion_Correction/motion_correction/motion_correction.ino
@@ -1,9 +1,8 @@
 #include "Dezibot.h"
-#define interval 30
 Dezibot dezibot = Dezibot();
-#define maxDuty 160
-#define minDuty 90
-#define baseDuty 110
+#define maxDuty 6000
+#define minDuty 3500
+#define baseDuty 3900
 void setup() {
   // put your setup code here, to run once:
   dezibot.begin();
@@ -12,14 +11,18 @@ void setup() {
   //analogWrite(11, 2000);
   //analogWrite(12, 2000);
 }
-uint8_t leftDuty= baseDuty;
-uint8_t rightDuty = baseDuty;
+uint16_t leftDuty= baseDuty;
+uint16_t rightDuty = baseDuty;
 unsigned long previous = millis();
 unsigned long current = millis();
 long averageZRotation = 0;
 long averageXAccel = 0;
 int left=0;
 int right=0;
+int difference = 0;
+int changerate = 0;
+int interval = 40;
+int threshold =  150;
 int resultcounter = 0;
 void loop() {
   // put your main code here, to run repeatedly:
@@ -28,76 +31,46 @@ void loop() {
   averageXAccel += dezibot.motionDetection.getAcceleration().x;
   averageZRotation += result.z;
   //Serial.println(result.z);
-  if (result.z > 100){
+  if (result.z > threshold){
     right++;
-  } else if(result.z < -100) {
+  } else if(result.z < -threshold) {
     left++;
   }
   resultcounter++;
   if ((current - previous) > interval){
-    Serial.println("=============================================");
-    previous = current;
-    averageZRotation = averageZRotation / resultcounter;
-    averageXAccel = averageXAccel / resultcounter;
-    Serial.print("Z:");
-    Serial.println(averageZRotation);
+    //averageZRotation = averageZRotation / resultcounter;
+    difference = abs(left-right);
+    if (difference>25){
+      changerate = 100;
+    } else if(difference>20){
+      changerate = 50;
+    } else if(difference >15){
+      changerate = 30;
+    } else if(difference > 10){
+      changerate = 10;
+    } else{
+      changerate = 5; 
+    }
+    //Serial.print("Z:");
+    //Serial.println(averageZRotation);
     dezibot.display.clearDisplay();
     dezibot.display.setCursor(0, 0);
     dezibot.display.setTextColor(WHITE);
     dezibot.display.setTextSize(2); // Draw 2X-scale text
-    dezibot.display.println(averageZRotation);
+     dezibot.display.println(averageZRotation);
+    //dezibot.display.println(resultcounter);
     dezibot.display.print(left);
     dezibot.display.print("   ");
     dezibot.display.println(right);
-    Serial.println("=============================================");
-    /*if(averageZRotation < -20){
-      //turns right
-      if((rightDuty < maxDuty || leftDuty <= minDuty) && rightDuty<255){
-        rightDuty++;
-      } else{
-        leftDuty--;
-      }
-    } else if(averageZRotation > 20){
-      //turns left
-      if(leftDuty < maxDuty || rightDuty <= minDuty && leftDuty<255){
-        leftDuty++;
-      } else {
-        rightDuty--;
-      }
-    }*/
     if(left>right){ //rotates anticlock
-      leftDuty++;
-      rightDuty--;
-      /*if(abs(leftDuty-baseDuty)<abs(rightDuty-baseDuty)){
-        rightDuty--;
-      } else{
-        leftDuty++;
-      }
-        
-      if (leftDuty == maxDuty){
-        rightDuty -= 5;
-        leftDuty -= 1;
-       }else {
-      leftDuty++;
-      }*/
-    } else if((left-right)<0){
-      leftDuty--;
-      rightDuty++;
-      /*if(abs(leftDuty-baseDuty)>abs(rightDuty-baseDuty)){
-        rightDuty++;
-      } else{
-        leftDuty--;
-      }
-      if( leftDuty == minDuty){
-        rightDuty += 5;
-        leftDuty += 1;
-      } else{
-        leftDuty--;
-      }*/
+      leftDuty+=changerate;
+      rightDuty-=changerate;
+    } else if(left<right){
+      leftDuty-=changerate;
+      rightDuty+=changerate;
     } 
-    analogWrite(12,leftDuty);
-    analogWrite(11, rightDuty);
-
+    dezibot.motion.left.setSpeed(leftDuty);
+    dezibot.motion.right.setSpeed(rightDuty);
     dezibot.display.println(leftDuty);
     dezibot.display.println(rightDuty);
     dezibot.display.display();
@@ -108,5 +81,6 @@ void loop() {
     resultcounter = 0;
     left = 0;
     right = 0;
+    previous = current;
   }
 }
diff --git a/src/motionDetection/MotionDetection.cpp b/src/motionDetection/MotionDetection.cpp
index b675e28..e22fe65 100644
--- a/src/motionDetection/MotionDetection.cpp
+++ b/src/motionDetection/MotionDetection.cpp
@@ -15,7 +15,13 @@ void MotionDetection::begin(void){
     handler->transfer(0x1F);
     handler->transfer(0x0F);
     //busy Wait for startup 
-    delayMicroseconds(200);
+    delayMicroseconds(250);
+    //set Gyroconfig
+    handler->transfer(0x20);
+    handler->transfer(0x25);
+    //set Gyro Filter
+    handler->transfer(0x23);
+    handler->transfer(0x37);
     digitalWrite(34,HIGH);
     handler->endTransaction();
 };