diff --git a/src/main.cpp b/src/main.cpp
index 45ad182..5a91f83 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -16,7 +16,7 @@ const int PIN_IN4 = 10;
 
 const int SERVO_PIN = 7; 
 
-Ultrasonic ultrasonic(A2,A3);
+Ultrasonic ultrasonic(A2,A3, 80000UL);
 int distance = 21;
 
   
@@ -59,9 +59,9 @@ void enableHigh() {
 }
 
 void setServo(int degrees) {
-  int lenMicroSecondsOfPulse = (float) degrees/ 90.0 * 1000 + 300;
+  int lenMicroSecondsOfPulse = (float) degrees/ 90.0 * 1000 + 550;
   int lenMicroSecondsOfPeriod = 20 * 1000; // 20 milliseconds (ms) 
-  for (int i = 0; i < 200; ++i) {
+  for (int i = 0; i < 300; ++i) {
     digitalWrite(SERVO_PIN, HIGH); 
     // Delay for the length of the pulse
     delayMicroseconds(lenMicroSecondsOfPulse);
@@ -94,6 +94,21 @@ void test(){
   setMotors(255, 255);
   delay(2000);
   stopMotors();
+  setMotors(0, 255);
+  delay(2000);
+  stopMotors();
+  setMotors(255, 0);
+  delay(2000);
+  stopMotors();
+  setMotors(-255, -255);
+  delay(2000);
+  stopMotors();
+  setMotors(0, -255);
+  delay(2000);
+  stopMotors();
+  setMotors(-255, 0);
+  delay(2000);
+  stopMotors();
 }
 
 // put your setup code here, to run once:
@@ -111,7 +126,8 @@ void setup() {
   digitalWrite(PIN_ENB, HIGH);
 
   // Display Initialisieren 
-  test();
+  setServo(90);
+  //test();
  
  
 }
@@ -137,49 +153,29 @@ void loop() {
       setServo(90);
       while(distance<50) {
         distance = ultrasonic.distanceRead();
-       
-       
-       
-       
-       
-       
-       
-       
-       
-       
         Serial.println("Turning left: ");
         Serial.println(distance);
-        setMotors(-120,0);
-        delay(10);
+        setMotors(-140,0);
+        //delay(10);
       }
       delay(500);
     } else {
       setServo(90);
       while(distance<50) {
         distance = ultrasonic.distanceRead();
-       
-       
-       
-       
-       
-       
-       
-       
-       
-       
         Serial.println("Turning right: ");
         Serial.println(distance);
-        setMotors(0,-120);
-        delay(10);
+        setMotors(0,-140);
+        //delay(10);
       }
       delay(500);
     }
     
   } else {
-    if((distance>215)||(distance<=0)){
+    if((distance>155)||(distance<=0)){
       setMotors(255, 255);
     } else {
-      setMotors(distance+70, distance+70);
+      setMotors(distance+100, distance+100);
     }    
   }
   delay(10);