 {
     ServoMotor.write(angle);
     delay(20);
+    servoPos = angle;
 }
 
 

void loop()
{

    GetRange()

    // Wait for initial scans before performing any actions
    if(millis() < initialDelay)
    {
        SetLightErrorState();
        return;
    }

    long range = rangeCenter;

    SetLightByRange(range);

    // obstacle is further away than X cm
    if(wheelSpinCompleted && range > rangeLimit_Stop)
    {
        // drive
        SetMotor(motorPin1_1, motorPin1_2, 255);
        SetMotor(motorPin2_1, motorPin2_2, 255);

        rangeLimit_Timestamp = millis();
    }

    // TODO - slightly turn if side better than center


        wheelSpinCompleted = false;

        if(rangeCenter > rangeLimit_Warning3 && (rangeLimit_Timestamp + rangeLimit_RotateFor) <= uptime)
        {
            wheelSpinCompleted = true;
        }
