 
     // attach servo pin and set to initial direction
     ServoMotor.attach(servoPin);
-    ServoMotor.write(servoPos);
-}
-
-void GetRange()
-{
-    long range = sonar.ping_cm();;
-
-    if(servoPos == servoCenter)
-    {
-        rangeCenter = range;
-    }
-
-    if(servoPos < servoCenter)
-    {
-        rangeLeft = range;
-    }
-
-    if(servoPos > servoCenter)
-    {
-        rangeRight = range;
-    }
+    ServoMotor.write(servoInitPos);
+    ServoMotor.detach();
+}
+
+void GetRange(int servoPos, bool rotating)
+{
+    long range = sonar.ping_cm();
+
+    // fallback if return value is out of range
+    if(range == 0) { range = lastRange; }
+    else { lastRange = range;}
+
+    if(range == 0 && rotating) { range = maxDistance; }
+
+    if(servoPos == servoCenter) { rangeCenter = range; }
+    if(servoPos < servoCenter) { rangeLeft = range; }
+    if(servoPos > servoCenter) { rangeRight = range; }
+}
+
+bool GetObstacle()
+{
+    if(analogRead(obstaclePin) < 500)
+    {
+        return true;
+    }
+    return false;
 }
 
 void SetServoAngle(uint8_t angle)
 {
     ServoMotor.write(angle);
     delay(20);
-    servoPos = angle;
 }
 
 

    pinMode(motorPin2_1, OUTPUT);
    pinMode(motorPin2_2, OUTPUT);

    Serial.begin(9600); // Starts the serial communication

    LEDStrip.begin();


// Ultrasonic Servo
Servo ServoMotor;
const uint8_t servoPin = 10;
uint8_t servoInitPos = 90;

uint8_t servoLimitLeft = 10;
uint8_t servoTiltLeft = 50;
