   // Get color measurement values
   uint16_t r, g, b, c;
   tcs.getRawData(&r, &g, &b, &c);
-  calibrateColors(&r, &g, &b, &c);
-  // If clear value is less than 300
-  if (c < 300 && !isColorBeingMeasured) {
-    // This is when the color sensor sees into the hole
-
-    if (millis() - lastSkittleTime > 1000) {
-      skittleCount ++;
+  // Convert colors to the 8-bits color standard values
+  standardizeColors(&r, &g, &b, &c);
+  
+  // Check if the hole arrives at the color sensor
+  if (c < C_HOLE_CLEAR && !isColorBeingMeasured) {
       Serial.println(millis() - lastSkittleTime);
       lastSkittleTime = millis();
-      Serial.println("Start measuring this Skittle's color.");
+      // Serial.println("Start measuring this Skittle's color.");
       isColorBeingMeasured = true;
-      min_clear = 200;
-    }
-
+      min_clear = C_HOLE_CLEAR;// C_SKITTLE_CLEAR;
   }
   if(isColorBeingMeasured){
     if(c < min_clear){

      best_b = b;
    }

     if (c > 300) {

          Serial.println("Finish measuring.");
          isColorBeingMeasured = false;
          lastSkittleTime = millis();
          Serial.print("R: "); Serial.print(best_r, DEC); Serial.print(" ");
          Serial.print("G: "); Serial.print(best_g, DEC); Serial.print(" ");
          Serial.print("B: "); Serial.print(best_b, DEC); Serial.print(" ");
          Serial.print("C: "); Serial.print(min_clear, DEC); Serial.print(" ");
          Serial.println(" ");
    
   
    }
  }

 

    if (c < 300) {
      
    if (millis() - lastSkittleTime > 1000) 
      lastSkittleTime = millis();
    
    }

  /*
    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(" ");
    Serial.print("C: "); Serial.print(c, DEC); Serial.print(" ");
    Serial.println(" ");
  
*/


}



// Constants
#define C_ATIME TCS34725_INTEGRATIONTIME_101MS //  Color sensor integration time 50ms
#define C_CYCLES (256-C_ATIME) // Color sensor cycles according to the integration time

#define C_HOLE_CLEAR 256 // The clear value used to determine the hole's arrival
#define C_SKITTLE_CLEAR 160 // The clear value used to determine the Skittle's arrival

#define F_CALIBRATING false // Is calibration in progress

const boolean topServoTimeoutEnable = true; // Enable the timeout for the top servo
const int topServoTimeout = 5000; // Timeout to determine if the top servo is stuck

// Colors
typedef struct{
  uint16_t r;
  uint16_t g;
  uint16_t b;
} c_color;

c_color rgb(uint16_t r, uint16_t g, uint16_t b){
  c_color c;
  c.r = r; c.g = g; c.b = b;
  return c;
}
// Calibrated Skittle's Colors
// These are the skittle's colors 
#define C_COLOR_GREEN rgb(64, 109, 51) 

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