Diff: detection.cpp

Revision:

0:8a18ceffce1b

Child:

1:a6c13143b151

diff -r 000000000000 -r 8a18ceffce1b detection.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/detection.cpp	Thu Jul 03 08:07:46 2014 +0000
@@ -0,0 +1,118 @@
+#include <mbed.h>
+#include "detection.h"
+
+#define COUNT(x) (sizeof(x)/sizeof(x[0]))
+#define RGB_VALUES (15)
+
+extern RGB_Sensor rgb;
+extern Serial console;
+
+static uint32_t g_buffer_pos;
+static TRGB g_buffer[RGB_VALUES];
+    
+struct Capsule {
+    const char* name;
+    double r, g, b;
+} g_capsules[] = {
+    { "no_capsule", 0, 0, 0 },
+    { "decaffeinato_intenso", -380.97474747474746, -606.4181818181818, -414.5565656565657 },
+    { "arpeggio", -84.13939393939394, -391.35656565656564, -405.6222222222222 }, 
+    { "dulsao", -422.4707070707071, -726.2787878787879, -541.3545454545455 },
+    { "volluto", -480.3393939393939, -768.0040404040404, -530.9494949494949 },
+    { "fortissio", -5.533333333333333, -521.9070707070707, -514.1626262626263 },
+    { "vivalto", -152.07474747474748, -608.4919191919192, -574.739393939394 }
+};
+
+static bool rgb_callback(const TRGB &color)
+{
+    if(g_buffer_pos>=RGB_VALUES)
+        return false;
+
+    g_buffer[g_buffer_pos++] = color;
+    return true;
+}
+
+int match_sample_to_capsule(TRGB& rgb_sample)
+{
+    const int magic_threshold = 10000;
+    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;    
+}
+
+Capsule* read_capsule() 
+{
+    Capsule *capsule = NULL;
+    bool done = false;
+
+    while (!done) 
+    {
+        int histogram[COUNT(g_capsules)] = {0};
+        int first_significant_sample = RGB_VALUES / 3;
+    
+        g_buffer_pos = 0;
+        rgb.capture(rgb_callback);
+        rgb.wait();
+    
+        for (int i = first_significant_sample; i < RGB_VALUES; i++)
+        {        
+            int index = match_sample_to_capsule(g_buffer[i]);
+            histogram[index]++;        
+        }
+    
+        for (int i = 0; i < COUNT(histogram); i++)
+        {
+            if (histogram[i] > 9/*RGB_VALUES / 2*/)
+            {
+                capsule = &g_capsules[i];
+                done = true;
+                break;                
+            }    
+        } 
+        
+        //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");
+            
+    }
+    
+    return capsule;
+}
+
+static Capsule *last_capsule = NULL;
+
+const char* do_detection()
+{
+    Capsule* this_capsule = read_capsule();
+    if (last_capsule)
+    {
+        while (this_capsule == last_capsule)
+            this_capsule = read_capsule();
+            
+        last_capsule = this_capsule;
+    } 
+    else
+    {
+        last_capsule = this_capsule;
+    }
+
+    return this_capsule->name;
+}