File content as of revision 3:4eefc032a319:

#include <mbed.h>
#include "detection.h"

#define RGB_TRESHOLD 0

#define COUNT(x) (sizeof(x)/sizeof(x[0]))
//#define DETECTION_DEBUG

#ifdef DETECTION_DEBUG
extern Serial console;
#endif
    
static const Capsule g_capsules[] = {
    { "no_capsule", 0, 0, 0 },
    { "arpeggio", 182.091796875, 390.759765625, 557.3984375 },
    { "decaffeinato_intenso", 265.15234375, 383.55859375, 464.64453125 },
    { "livanto", 417.59765625, 551.00390625, 522.685546875 },
    { "roma", 159.708984375, 407.90234375, 476.068359375 },
    { "rosabaya", 707.041015625, 1193.072265625, 1382.546875 },
    { "volluto", 736.615234375, 1041.916015625, 849.5234375 }
};

RGB_Detection::RGB_Detection(PinName red, PinName green, PinName blue, PinName adc) :
    m_rgb(red, green, blue, adc), 
    m_last_capsule(NULL)
{
}

RGB_Detection::~RGB_Detection()
{
}

int RGB_Detection::match_sample_to_capsule(TRGB& rgb_sample)
{
    const int magic_threshold = 7000;
    double min_so_far = 1e20;
    int best_index = 0;

    for (int j = 1; j < COUNT(g_capsules); j++)
    {
        double delta_r = g_capsules[j].r - (rgb_sample.ch.red / RGB_OVERSAMPLING);
        double delta_g = g_capsules[j].g - (rgb_sample.ch.green / RGB_OVERSAMPLING);
        double delta_b = g_capsules[j].b - (rgb_sample.ch.blue / RGB_OVERSAMPLING);
        double d = (delta_r * delta_r + delta_g * delta_g + delta_b * delta_b);
                    
        if (d < min_so_far && d < magic_threshold) 
        {
            min_so_far = d;
            best_index = j;
        }
    }
    
    return best_index;    
}

const Capsule* RGB_Detection::read_capsule(void) 
{
    Capsule const *capsule = NULL;
    bool done = false;

    while (!done) 
    {
        int histogram[COUNT(g_capsules)] = {0};
        int samples_in_buffer = m_rgb.trigger(m_buffer, COUNT(m_buffer), RGB_TRESHOLD);
        
        for (int i = 0; i < samples_in_buffer; i++)
        {        
            // console.printf("buffer[%d] = {%i, %i, %i}\r\n", i, m_buffer[i].data[0] / RGB_OVERSAMPLING, m_buffer[i].data[1] / RGB_OVERSAMPLING, m_buffer[i].data[2] / RGB_OVERSAMPLING);
            int index = match_sample_to_capsule(m_buffer[i]);
            histogram[index]++;        
        }
    
        for (int i = 0; i < COUNT(histogram); i++)
        {
            if (histogram[i] > 45/*RGB_VALUES / 2*/)
            {
                capsule = &g_capsules[i];
                done = true;
                break;                
            }    
        }
        
#ifdef DETECTION_DEBUG
        console.printf("------------------------\r\n");
        for (int i = 0; i < COUNT(histogram); i++)
        {
            console.printf("%s: %d\r\n", g_capsules[i].name, histogram[i]);
        }
        console.printf("------------------------\r\n\r\n");            
#endif
    }
    
    return capsule;
}

const char* RGB_Detection::run(void)
{
    Capsule const *this_capsule = read_capsule();

    if (m_last_capsule)
    {
        while (this_capsule == m_last_capsule)
            this_capsule = read_capsule();
    }        
        
    m_last_capsule = this_capsule;

    return this_capsule->name;
}