color.cpp

#include <Arduino.h>
#include "color.h"
#include "Adafruit_TCS34725.h"

// Initiate color sensor instances
static Adafruit_TCS34725 color_sensor_left = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_24MS, TCS34725_GAIN_60X);
static Adafruit_TCS34725 color_sensor_right = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_24MS, TCS34725_GAIN_60X);

static TwoWire Wire2 = TwoWire(PC9, PA8); // I2C channel 3 for right sensor

// Change this to CALIBRATE_READY if no calibration is required
static CalibrationState calibration_state = CALIBRATE_BLACK;

static ColorSensorCal color_cal_left;
static ColorSensorCal color_cal_right;

void printRGB(const Rgb &value)
{
    Serial.print("R:");
    Serial.print(int(value.r));
    Serial.print(" G:");
    Serial.print(int(value.g));
    Serial.print(" B:");
    Serial.print(int(value.b));
}

void readColorValues(Rgb &left_value, Rgb &right_value)
{
    // Turn on LEDs
    color_sensor_left.setInterrupt(false);
    color_sensor_right.setInterrupt(false);
    // Delay 24ms to read
    delay(24);
    color_sensor_left.getRGB(&left_value.r, &left_value.g, &left_value.b);
    color_sensor_right.getRGB(&right_value.r, &right_value.g, &right_value.b);
    // Turn off LEDs
    color_sensor_left.setInterrupt(true);
    color_sensor_right.setInterrupt(true);

    if (calibration_state == CALIBRATE_READY)
    {
        // Calibrate left color value
        left_value.r = constrain(map(left_value.r, color_cal_left.min.r, color_cal_left.max.r, 0, 255), 0, 255);
        left_value.g = constrain(map(left_value.g, color_cal_left.min.g, color_cal_left.max.g, 0, 255), 0, 255);
        left_value.b = constrain(map(left_value.b, color_cal_left.min.b, color_cal_left.max.b, 0, 255), 0, 255);

        // Calibrate right color value
        right_value.r = constrain(map(right_value.r, color_cal_right.min.r, color_cal_right.max.r, 0, 255), 0, 255);
        right_value.g = constrain(map(right_value.g, color_cal_right.min.g, color_cal_right.max.g, 0, 255), 0, 255);
        right_value.b = constrain(map(right_value.b, color_cal_right.min.b, color_cal_right.max.b, 0, 255), 0, 255);
    }
}

float computeHue(const Rgb &rgb)
{
    float red = rgb.r / 255;
    float green = rgb.g / 255;
    float blue = rgb.b / 255;

    float max_component = max(max(red, green), blue);
    float min_component = min(min(red, green), blue);

    float hue;

    float diff = max_component - min_component;

    if (max_component == red)
    {
        hue = (green - blue) / (diff) + (green < blue ? 6.0 : 0);
    }
    else if (max_component == green)
    {
        hue = (blue - red) / (diff) + 2.0;
    }
    else
    {
        hue = (red - green) / (diff) + 4.0;
    }

    return hue * 60;
}

void setupColorSensor()
{
    // Color sensor setup
    // Left sensor on pins SDA=D14 SCK=D15 (default I2C channel 1)
    if (color_sensor_left.begin())
    {
        Serial.println("Found left color sensor");
    }
    else
    {
        Serial.println("Left color sensor not found");
        while (1)
            ;
    }

    // Right sensor on pins SDA=PC9 SCK=PA8
    if (color_sensor_right.begin(TCS34725_ADDRESS, &Wire2))
    {
        Serial.println("Found right color sensor");
    }
    else
    {
        Serial.println("Right color sensor not found");
        while (1)
            ;
    }
};

CalibrationState getCalibrationState() {
    return calibration_state;
}

void calibrateBlack()
{
    // Calibrate black
    Serial.print("Calibrating black");
    readColorValues(color_cal_left.min, color_cal_right.max);
    // Transition to calibrate white
    calibration_state = CALIBRATE_WHITE;
}

void calibrateWhite()
{
    // Calibrate white
    Serial.print("Calibrating white");
    readColorValues(color_cal_left.max, color_cal_right.max);
    // Transition to ready state
    calibration_state = CALIBRATE_READY;
}