         return;
     }
 
-    uint8_t motorSpeed = 1 * 255;
-    uint32_t color = LEDStrip.Color(0, 255, 0);
-    if(range < rangeLimit_Warning1) { color = LEDStrip.Color(127, 255, 0);}
-    if(range < rangeLimit_Warning2) { color = LEDStrip.Color(255, 255, 0); }
-    if(range < rangeLimit_Warning3) { motorSpeed = 0.8 * 255; color = LEDStrip.Color(255, 127, 0); }
-    if(range < rangeLimit_Stop) { color = LEDStrip.Color(255, 0, 0); }
-
-    ColorSet(color);
+    SetLightByRange(range);
+
 
     // obstacle is further away than X cm
     if(wheelSpinCompleted && range > rangeLimit_Stop)
     {
         // drive
-        SetMotor(motorPin1_1, motorPin1_2, motorSpeed);
-        SetMotor(motorPin2_1, motorPin2_2, motorSpeed);
+        SetMotor(motorPin1_1, motorPin1_2, 255);
+        SetMotor(motorPin2_1, motorPin2_2, 255);
 
         rangeLimit_Timestamp = millis();
     }

    return servoPos;
}


void loop()
{

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;
