Needs more diagnosis when new color sensor arrives…will be much simpler with the specific color sensor ”.h” file

The output is only in serial now, but the “rejection” component(do something to gumball if wrong color)will be implemented soon.

#include <Wire.h>

#define I2C_ADDRESS 0x74       // 7bit

#define REG_CAP_RED       0x06
#define REG_CAP_GREEN     0x07
#define REG_CAP_BLUE      0x08
#define REG_CAP_CLEAR     0x09

#define REG_INT_RED_LO    0x0A
#define REG_INT_RED_HI    0x0B
#define REG_INT_GREEN_LO  0x0C
#define REG_INT_GREEN_HI  0x0D
#define REG_INT_BLUE_LO   0x0E
#define REG_INT_BLUE_HI   0x0F
#define REG_INT_CLEAR_LO  0x10
#define REG_INT_CLEAR_HI  0x11

#define REG_DATA_RED_LO   0x40
#define REG_DATA_RED_HI   0x41
#define REG_DATA_GREEN_LO 0x42
#define REG_DATA_GREEN_HI 0x43
#define REG_DATA_BLUE_LO  0x44
#define REG_DATA_BLUE_HI  0x45
#define REG_DATA_CLEAR_LO 0x46
#define REG_DATA_CLEAR_HI 0x47

void set_register(unsigned char r, unsigned char v)
{
  Wire.beginTransmission(I2C_ADDRESS);
  Wire.send(r);
  Wire.send(v);
  Wire.endTransmission();
}

unsigned char read_register(unsigned char r)
{
  unsigned char v;
  Wire.beginTransmission(I2C_ADDRESS);
  Wire.send(r);  // register to read
  Wire.endTransmission();

  Wire.requestFrom(I2C_ADDRESS, 1); // read a byte
  while(!Wire.available()) {
    // waiting
  }
  v = Wire.receive();
  return v;
}

void setup(void)
{
  Wire.begin();
  Serial.begin(9600);

  // sensor gain setting 
  // CAPs are 4bit (higher value will result in lower output)
  set_register(REG_CAP_RED, 0x03);
  set_register(REG_CAP_GREEN, 0x03);
  set_register(REG_CAP_BLUE, 0x03);
  set_register(REG_CAP_CLEAR, 0x08);
  // INTs are 12bit (higher value will result in higher sensor value)
  set_register(REG_INT_RED_LO, 0xC4);
  set_register(REG_INT_RED_HI, 0x09);
  set_register(REG_INT_GREEN_LO, 0xC4);
  set_register(REG_INT_GREEN_HI, 0x09);
  set_register(REG_INT_BLUE_LO, 0xC4);
  set_register(REG_INT_BLUE_HI, 0x09);
  set_register(REG_INT_CLEAR_LO, 0xC4);
  set_register(REG_INT_CLEAR_HI, 0x09);
}

void loop()
{
  int r, g, b, c;

  set_register(0x00,0x01); // start sensing

  while(read_register(0x00) != 0) {
    // waiting for a result
  }
  // 10bit results
  c = read_register(REG_DATA_CLEAR_LO) + (read_register(REG_DATA_CLEAR_HI)<<8);
  r = read_register(REG_DATA_RED_LO) + (read_register(REG_DATA_RED_HI)<<8);
  g = read_register(REG_DATA_GREEN_LO) + (read_register(REG_DATA_GREEN_HI)<<8);
  b = read_register(REG_DATA_BLUE_LO) + (read_register(REG_DATA_BLUE_HI)<<8);

  Serial.print("crgb=");
  Serial.print(c ,DEC);
  Serial.print(",");
  Serial.print(r ,DEC);
  Serial.print(",");
  Serial.print(g ,DEC);
  Serial.print(",");
  Serial.println(b ,DEC);

  delay(999);
}