 // Create the object for the bottom standard servo
 Servo servoBtm;
 // Create the color sensor object
-Adafruit_TCS34725 tcs = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_50MS, TCS34725_GAIN_1X);
+Adafruit_TCS34725 tcs = Adafruit_TCS34725(C_ATIME, TCS34725_GAIN_1X);
 
 // The number of skittles sorted
 int skittleCount = 0;

  // 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 ++;
      Serial.println(millis() - lastSkittleTime);
      lastSkittleTime = millis();
      Serial.println("Start measuring this Skittle's color.");
      isColorBeingMeasured = true;
      min_clear = 200;
    }

  }
  if(isColorBeingMeasured){
    if(c < min_clear){

  servoBtm.write(btmAngle);
}

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() {
