 {
     long range = sonar.ping_cm();
 
+    Serial.print(servoPos);
+    Serial.print(" - ");
+    Serial.println(range);
+
+    if(range == 0)
+    {
+        //delay(5000);
+    }
+
     long *rangeData_Current;
 
     // get current meassurement and add to array

uint8_t GetNextServoAngle()
{
    // 0 - 80 / 80 - 100 / 100 - 180
    if(servoPos <= 0)
    {
        servoMovement = right;
        servoPos = 0;
    }

    if(servoPos >= 180)
    {
        servoMovement = left;
        servoPos = 180;
    }



    }

    long range = average_Center;
    //Serial.println(average_Center);
}


