Adafruit TCS34725: red, green, blue and clear counts

// Color sorter: a TCS34725 looks at the candy in front of it and a servo
// swings the chute to that color's bin, as in the candy and bead sorters
// people build.
//
//   TCS34725   SDA -> A4   SCL -> A5   VIN -> 5 V   GND -> GND   (LED left high: lit)
//   servo      signal -> 9   red -> 5 V   brown -> GND
//
// The sensor gives four counts: red, green, blue and clear. Their shares of
// the total say what color it is whatever the brightness, so the sketch
// compares them with a table of the six colors it knows, measured once with
// the LED on, and picks the nearest. A low clear count is an empty chute.
//
// In the simulator the sorter's feed disc brings the candies one at a time:
// watch the chute move, and the candy land in its bin.

#include <Wire.h>
#include <Servo.h>
#include "Adafruit_TCS34725.h"

Adafruit_TCS34725 tcs = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_50MS, TCS34725_GAIN_4X);
Servo chute;

const int SERVO_PIN = 9;
const uint16_t EMPTY_BELOW = 1500;  // clear count of the empty chute

struct Reference {
  const char *name;
  uint16_t red, green, blue;  // shares of the total, in thousandths
  uint8_t angle;              // where the chute points for this bin
};

const Reference COLORS[] = {
  {"red", 647, 203, 148, 15},
  {"orange", 566, 342, 90, 45},
  {"yellow", 475, 431, 92, 75},
  {"green", 192, 524, 283, 105},
  {"blue", 137, 313, 549, 135},
  {"purple", 324, 243, 431, 165},
};
const uint8_t COUNT = sizeof(COLORS) / sizeof(COLORS[0]);

int8_t last = -2;

void setup() {
  Serial.begin(9600);
  chute.attach(SERVO_PIN);
  chute.write(90);
  if (!tcs.begin()) {
    Serial.println("No TCS34725 found ... check your connections");
    while (1) delay(10);
  }
  Serial.println("Color sorter ready");
}

// The nearest color in the table, or -1 for an empty chute.
int8_t classify(uint16_t r, uint16_t g, uint16_t b, uint16_t c) {
  if (c < EMPTY_BELOW) return -1;
  uint32_t total = (uint32_t)r + g + b;
  int32_t rs = (uint32_t)r * 1000 / total;
  int32_t gs = (uint32_t)g * 1000 / total;
  int32_t bs = (uint32_t)b * 1000 / total;
  int8_t best = 0;
  int32_t bestDistance = 0x7fffffff;
  for (uint8_t i = 0; i < COUNT; i++) {
    int32_t dr = rs - COLORS[i].red;
    int32_t dg = gs - COLORS[i].green;
    int32_t db = bs - COLORS[i].blue;
    int32_t d = dr * dr + dg * dg + db * db;
    if (d < bestDistance) {
      bestDistance = d;
      best = i;
    }
  }
  return best;
}

void loop() {
  uint16_t r, g, b, c;
  tcs.getRawData(&r, &g, &b, &c);
  int8_t color = classify(r, g, b, c);
  if (color == last) return;
  last = color;
  if (color < 0) {
    Serial.println("chute empty");
    return;
  }
  Serial.print(COLORS[color].name);
  Serial.print(" -> bin ");
  Serial.print(color + 1);
  Serial.print("  (R ");
  Serial.print(r);
  Serial.print(" G ");
  Serial.print(g);
  Serial.print(" B ");
  Serial.print(b);
  Serial.print(" C ");
  Serial.print(c);
  Serial.println(")");
  chute.write(COLORS[color].angle);
  delay(300);
}