 
   lcd.print("Color Sorter");
 
+  // Set every element in colorResults to RESULT_NO_DATA_YET
+  for (int i = 0; i < sizeof(colorResults); i++) {
+    colorResults[i] = RESULT_UNKNOWN;
+  }
+
 }
 
 void updateTopServo() {
   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 : 0);
+  // Check if the color is being re-measured
+  if (isRemeasuring) {
+    boolean drct = remeasuringReverseDirection;
+    if (reverseDirection) drct = !drct;
+    servoTop.write(drct ? 180 : 0);
+  } else {
+    // 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 : 0);
+  }
 
   // If the top servo gets stuck
   if (F_TOP_SERVO_TIMEOUT_ENABLED && millis() - lastSkittleTime > C_TOP_SERVO_TIMEOUT) {


#define C_HOLE_CLEAR 300 // The clear value used to determine the hole's arrival
#define C_IDEAL_CLEAR 96 // The ideal clear value for color detection
#define C_ALLOWED_COLOR_VARIANCE 64 // Allowed color variance for color detection

#define F_TOP_SERVO_TIMEOUT_ENABLED true // Enable the timeout for the top servo
#define C_TOP_SERVO_TIMEOUT 6000 // Timeout to determine if the top servo is stuck

#define C_COLOR_SIGNAL_TIME 1000 // Each Skittle gets 1s color view signal

// Include libraries
#include <LiquidCrystal.h>      // LCD display

}

void updateColorView() {
  static int index = 0;
  unsigned long lastColorSignalTime = 0;
  if (HAS_RESULT(colorResults[index])) {
    lastColorSignalTime = millis();
    switch (colorResults[index]) {
      case 0:
        Serial.println("RED");
        analogWrite(PIN_COLOR_RED, 255);
