 
 // Ultrasonic Servo
 Servo ServoMotor;
-const uint8_t servoPin = 6;
-uint8_t servoPos = 90;
+const uint8_t servoPin = 10;
+uint8_t servoInitPos = 90;
 
 uint8_t servoLimitLeft = 10;
 uint8_t servoTiltLeft = 50;

    ColorSet(color);
}

void SetLightErrorState()
{
    ColorSet(LEDStrip.Color(0, 0, 255));

    SetMotor(motorPin1_1, motorPin1_2, 0);
    SetMotor(motorPin2_1, motorPin2_2, 0);
}

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)

uint8_t servoTiltRight = 130;
uint8_t servoCenter = 90;

// IR obstacle detection
int obstaclePin = A1;



void setup()
