 // Ultrasonic Servo
 Servo ServoMotor;
 const uint8_t servoPin = 10;
-uint8_t servoInitPos = 90;
-
-uint8_t servoLimitLeft = 10;
-uint8_t servoTiltLeft = 50;
-uint8_t servoLimitRight = 170;
-uint8_t servoTiltRight = 130;
-uint8_t servoCenter = 90;
+
+const uint8_t servoLimitLeft = 10;
+const uint8_t servoTiltLeft = 50;
+const uint8_t servoLimitRight = 170;
+const uint8_t servoTiltRight = 130;
+const uint8_t servoCenter = 90;
+
+enum direction {
+  LEFT,
+  NONE,
+  RIGHT
+};
+direction servoRangeDirection = NONE;
 
 // IR obstacle detection
 int obstaclePin = A1;
-
 
 
 void setup()

    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; }

    }

    // obstacle is further away than X cm
    // 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);

        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;
        }
    }
}

