 uint8_t GetNextServoAngle()
 {
     // 0 - 80 / 80 - 100 / 100 - 180
-    if(servoPos <= 0)
+    if(servoPos <= 10)
     {
         servoMovement = right;
-        servoPos = 0;
+        servoPos = 10;
     }
 
-    if(servoPos >= 180)
+    if(servoPos >= 170)
     {
         servoMovement = left;
-        servoPos = 180;
+        servoPos = 170;
     }
 
 

{
    long range = sonar.ping_cm();

    long *rangeData_Current;

    // get current meassurement and add to array


    // Get the next Angle and Move the Servo
    ServoMotor.write(GetNextServoAngle());
    delay(20);

    // if the new servo location is different from the one before calculate an anverage
    if(servoLocation != servoLocationLast || servoMovement != servoMovementLast)
