   servoBtm.write(btmAngle);
 }
 
-void calibrateColors(uint16_t *r, uint16_t *g, uint16_t *b, uint16_t *c){
-  /* Calibrated colors against the unit 8-bits color standard where the maximum
-   *  color value is 256. 
+void standardizeColors(uint16_t *r, uint16_t *g, uint16_t *b, uint16_t *c){
+  /* Standardize colors to the unit 8-bits color standard where the 
+   *  maximum color value is 256. 
   */
   *r /= C_CYCLES; *g /= C_CYCLES; *b /= C_CYCLES; *c /= C_CYCLES;
+}
+
+void maximizeColors(uint16_t *r, uint16_t *g, uint16_t *b, uint16_t *c){
+  /* Maximize colors based on the clear value.
+  */
+  float scale = 256.0 / *c;
+  Serial.print("scale");Serial.print(scale);
+  *r *= scale; *g *= scale; *b *= scale;
+}
+
+void __calibrating(uint16_t r, uint16_t g, uint16_t b){
+  /* This function is used to test and get the average color 
+   *  of a certain Skittle
+  */
+  static uint16_t a_r = r, a_g = g, a_b = b, count = 0;
+  //if(abs(r - a_r) > 64 || abs(g - a_g) > 64 || abs(b - a_b) > 64 ){
+  //  Serial.println("Difference too big. This data is discarded.");
+  //}else{
+    a_r = (a_r * count + r) / (count + 1);
+    a_g = (a_g * count + g) / (count + 1);
+    a_b = (a_b * count + b) / (count + 1);
+    count++;
+    Serial.println("Average color:");
+    printColors(a_r, a_g, a_b, 0);
+  //}
+  
+  
+}
+
+int checkColor(c_color c){
+  c_color c2 = C_COLOR_GREEN;
+  uint16_t color_diff_r = abs(int(c.r) - int(c2.r));
+  uint16_t color_diff_g = abs(int(c.g) - int(c2.g));
+  uint16_t color_diff_b = abs(int(c.b) - int(c2.b));
+  Serial.println("Color diff:");
+  printColors(color_diff_r, color_diff_g, color_diff_b, 0);
+  return 0;
+}
+
+void printColors(uint16_t r, uint16_t g, uint16_t b, uint16_t c){
+  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(" ");
 }
 
 void updateColorSensor() {

      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(" ");
  
*/


}

  // Get color measurement values
  uint16_t r, g, b, c;
  tcs.getRawData(&r, &g, &b, &c);
  // 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.");
      isColorBeingMeasured = true;
      min_clear = C_HOLE_CLEAR;// C_SKITTLE_CLEAR;
  }
  if(isColorBeingMeasured){
    if(c < min_clear){
