 int skittleCount = 0;
 // Time when the last skittle was being sorted
 unsigned long lastSkittleTime = 0;
-// The speed in which the top servo moves
-int topServoSpeed = 0;
-// Is the color sensor measuring the Skittle right now
-boolean isColorBeingMeasured = false;
 
 void setup() {
   // Set up the serial for debugging purposes

  static boolean reverseDirection = false;
  // Set the speed of the top servo
  // with 0 being full-speed in one direction, 180 being full speed in the other, and a value near 90 being no movement.
  servoTop.write(reverseDirection ? 180 : topServoSpeed);
  // servoTop.write(180);

  // If color is being measured right now, skip the stuck detection
  if (isColorBeingMeasured)
    return;

  // If the top servo gets stuck
  if (topServoTimeoutEnable && millis() - lastSkittleTime > topServoTimeout) {

    }

  /*
    Serial.print("R: "); Serial.print(r, DEC); Serial.print(" ");
    Serial.print("G: "); Serial.print(g, DEC); Serial.print(" ");
    Serial.print("B: "); Serial.print(b, DEC); Serial.print(" ");
