 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(millis() < initialDelay)
+    if(uptime < initialDelay)
     {
         SetLightErrorState();
         return;
     }
 
-    long range = rangeCenter;
-
-    SetLightByRange(range);
+    SetLightByRange(rangeCenter);
 
     // obstacle is further away than X cm
-    if(wheelSpinCompleted && range > rangeLimit_Stop)
+    if(wheelSpinCompleted && rangeCenter > rangeLimit_Stop)
     {
         // drive
         SetMotor(motorPin1_1, motorPin1_2, 255);
         SetMotor(motorPin2_1, motorPin2_2, 255);
 
-        rangeLimit_Timestamp = millis();
+        rangeLimit_Timestamp = uptime;
     }
 
     // TODO - slightly turn if side better than center


        wheelSpinCompleted = false;

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

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

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

