   }
   delay(setupWaitTime); 
   //set threshold for green-blue high
-  th_Val[6].r_t = ((float) r * thErrOFF); 
-  th_Val[6].g_t = ((float) g * thErrON);
-  th_Val[6].b_t = ((float) b * thErrON);
+     r_h = g_h = b_h = 0;
+  while(true)
+  {
+    tcs.getRawData(&r, &g, &b, &c); //get env info
+    if(b > b_h)
+    {
+      b_h = b;
+      if(r_h < r)
+      {
+        r_h = r;
+      }
+    }
+    else if(g > g_h)
+    {
+      g_h = g;
+      if(r_h < r)
+      {
+        r_h = r;
+      }
+    }
+    else
+    {
+     th_Val[6].r_t = ((float) r_h * thErrOFF); 
+     th_Val[6].g_t = ((float) g_h * thErrON);
+     th_Val[6].b_t = ((float) b_h * thErrON);
+      break;
+    }
+  }
   while((g > th_Val[0].g_t) | (b > th_Val[0].b_t)) //wait until green and blue is LOW
   {
     tcs.getRawData(&r, &g, &b, &c); 

  }
  delay(setupWaitTime); 
  //set threshold for red-green high
  th_Val[4].r_t = ((float) r * thErrON); 
  th_Val[4].g_t = ((float) g * thErrON);
  th_Val[4].b_t = ((float) b * thErrOFF);
  while((r > th_Val[0].r_t) | (g > th_Val[0].g_t)) //wait until red and green is LOW
  {
    tcs.getRawData(&r, &g, &b, &c); 

  }
  delay(setupWaitTime); 
  //set threshold for red-green-blue high
     r_h = g_h = b_h = 0;
  while(true)
  {
    tcs.getRawData(&r, &g, &b, &c); //get env info
    if(r > r_h)
    {
      r_h = r;
    }
    else if(g > g_h)
    {
      g_h = g;
    }
    else if (b_h < b)
      {
        b_h = b;
      }
    else
    {
       th_Val[7].r_t = ((float) r_h * thErrON); 
       th_Val[7].g_t = ((float) g_h * thErrON);
        th_Val[7].b_t = ((float) b_h * thErrON);
      break;
    }
  }

  while((g > th_Val[0].g_t) | (b > th_Val[0].b_t) | (r > th_Val[0].r_t)) //wait until red, green and blue is HIGH
  {
    tcs.getRawData(&r, &g, &b, &c); 
