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

const uint8_t rangeLimit_Warning3 = 25;
const uint8_t rangeLimit_Stop = 15;

const uint16_t rangeLimit_RotateFor = 500;
unsigned long rangeLimit_Timestamp = 0;
bool wheelSpinCompleted = true;


void loop()
{

    uint32_t uptime = millis();
    GetRange()

    if(uptime % 5000 == 0)
    {
        SetServoAngle(servoTiltRight)
    }

    if(uptime % 5500 == 0)
    {
        SetServoAngle(servoCenter)
    }


    if(uptime % 9000 == 0)
    {
        SetServoAngle(servoTiltLeft)
    }

    if(uptime % 9500 == 0)
    {
        SetServoAngle(servoCenter)
    }

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

    SetLightByRange(rangeCenter);

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

        rangeLimit_Timestamp = uptime;
    }

    // TODO - slightly turn if side better than center
