     }
 
     // obstacle is further away than X cm
-    if(wheelSpinCompleted && rangeCenter > rangeLimit_Stop)
-    {
-        // drive
+    // and no obstacle in front of the IR sensor
+    if(!obstacleFound || wheelSpinCompleted && rangeCenter > rangeLimit_Stop)
+    {
+        rangeLimit_Timestamp = uptime;
+
+        // slightly turn if side better than center
+        if(rangeCenter < rangeLimit_Warning1 && rangeRight > 0 && rangeLeft > 0)
+        {
+            if(rangeRight > rangeLeft && rangeRight > rangeCenter )
+            {
+                // drive straight
+                SetMotor(motorPin1_1, motorPin1_2, 255);
+                SetMotor(motorPin2_1, motorPin2_2, 128);
+            }
+
+            if(rangeLeft > rangeRight && rangeLeft > rangeCenter )
+            {
+                // drive straight
+                SetMotor(motorPin1_1, motorPin1_2, 128);
+                SetMotor(motorPin2_1, motorPin2_2, 255);
+            }
+        }
+
+
+        // drive straight
         SetMotor(motorPin1_1, motorPin1_2, 255);
         SetMotor(motorPin2_1, motorPin2_2, 255);
 
-        rangeLimit_Timestamp = uptime;
-    }
-
-    // TODO - slightly turn if side better than center
-
-    else
-    {
-        if(rangeLeft > rangeRight)
-        {
-            SetMotor(motorPin1_1, motorPin1_2, 255);
-            SetMotor(motorPin2_1, motorPin2_2, -255);
-        }
-        else
+        return;
+    }
+
+
+    else
+    {
+        if(servoRangeDirection == NONE)
+        {
+            SetServoRangeDirection();
+        }
+
+        if(servoRangeDirection == LEFT)
         {
             SetMotor(motorPin1_1, motorPin1_2, -255);
             SetMotor(motorPin2_1, motorPin2_2, 255);
         }
+        else
+        {
+            SetMotor(motorPin1_1, motorPin1_2, 255);
+            SetMotor(motorPin2_1, motorPin2_2, +255);
+        }
 
         wheelSpinCompleted = false;
 
         if(rangeCenter > rangeLimit_Warning3 && (rangeLimit_Timestamp + rangeLimit_RotateFor) <= uptime)
         {
             wheelSpinCompleted = true;
-        }
-    }
-}
-
+            servoRangeDirection = NONE;
+        }
+    }
+}
+


    // attach servo pin and set to initial direction
    ServoMotor.attach(servoPin);
    ServoMotor.write(servoInitPos);
    ServoMotor.detach();
}


    long range = sonar.ping_cm();

    // fallback if return value is out of range
    if(range == 0 && rotating) { range = maxDistance; }
    if(range == 0) { range = lastRange; }
    if(range > 0  && !rotating) { lastRange = range;}

    if(servoPos == servoCenter) { rangeCenter = range; }
    if(servoPos < servoCenter) { rangeLeft = range; }
