Nespresso RGB Sensor / ColorDetector

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   GroveColourSensor

Fork of ColorDetector by Brian Daniels

ColorDetector.cpp@7:14ea7e5381ea, 2015-06-30 (annotated)

Committer:
bridadan
Date:
Tue Jun 30 02:37:15 2015 +0000
Revision:
7:14ea7e5381ea
Parent:
5:710031221b4d
Child:
9:487acc0ebf8f
disabled interrupts around I2C Read block

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bridadan 0:b6814eefafc8 1 #include "ColorDetector.h"
bridadan 5:710031221b4d 2 #include "math.h"
bridadan 0:b6814eefafc8 3
bridadan 3:c05a96428ce8 4
bridadan 5:710031221b4d 5 ColorDetector::ColorDetector(GroveColourSensor *sensor, uint16_t threshold, uint16_t varianceThreshold, uint16_t diffThreshold, uint16_t samplesPerReading)
bridadan 5:710031221b4d 6 :sensor(sensor), threshold(threshold), varianceThreshold(varianceThreshold), diffThreshold(diffThreshold), changeDetected(false), curBuffPointer(0),
bridadan 0:b6814eefafc8 7 samplesPerReading(samplesPerReading) {
bridadan 0:b6814eefafc8 8 // Empty
bridadan 0:b6814eefafc8 9 }
bridadan 0:b6814eefafc8 10
bridadan 0:b6814eefafc8 11
bridadan 3:c05a96428ce8 12
bridadan 0:b6814eefafc8 13 RGBC ColorDetector::takeSamples(uint16_t numSamples) {
bridadan 0:b6814eefafc8 14 uint32_t tot_r = 0;
bridadan 0:b6814eefafc8 15 uint32_t tot_g = 0;
bridadan 0:b6814eefafc8 16 uint32_t tot_b = 0;
bridadan 0:b6814eefafc8 17 uint32_t tot_c = 0;
bridadan 0:b6814eefafc8 18
bridadan 7:14ea7e5381ea 19 __disable_irq();
bridadan 0:b6814eefafc8 20 for (uint32_t i = curBuffPointer; (i < curBuffPointer + numSamples) && (i < CD_BUFF_LEN); i++) {
bridadan 0:b6814eefafc8 21 sensor->readBlock(sample_buf + i);
bridadan 0:b6814eefafc8 22 tot_r += sample_buf[i].ch.red;
bridadan 0:b6814eefafc8 23 tot_g += sample_buf[i].ch.green;
bridadan 0:b6814eefafc8 24 tot_b += sample_buf[i].ch.blue;
bridadan 0:b6814eefafc8 25 tot_c += sample_buf[i].ch.clear;
bridadan 0:b6814eefafc8 26 }
bridadan 7:14ea7e5381ea 27 __enable_irq();
bridadan 0:b6814eefafc8 28 curBuffPointer += numSamples;
bridadan 0:b6814eefafc8 29
bridadan 0:b6814eefafc8 30 RGBC ret;
bridadan 0:b6814eefafc8 31 ret.ch.red = tot_r / numSamples;
bridadan 0:b6814eefafc8 32 ret.ch.green = tot_g / numSamples;
bridadan 0:b6814eefafc8 33 ret.ch.blue = tot_b / numSamples;
bridadan 0:b6814eefafc8 34 ret.ch.clear = tot_c / numSamples;
bridadan 0:b6814eefafc8 35
bridadan 0:b6814eefafc8 36 return ret;
bridadan 0:b6814eefafc8 37 }
bridadan 0:b6814eefafc8 38
bridadan 5:710031221b4d 39 void ColorDetector::calculateVariance(RGBC *variance, RGBC *buffer, int numSamples, RGBC mean) {
bridadan 5:710031221b4d 40 double diff_r = 0;
bridadan 5:710031221b4d 41 double diff_g = 0;
bridadan 5:710031221b4d 42 double diff_b = 0;
bridadan 5:710031221b4d 43 double diff_c = 0;
bridadan 5:710031221b4d 44 for (int i = 0; i < numSamples; i++) {
bridadan 5:710031221b4d 45 double tmp = (double)sample_buf[i].ch.red - (double)mean.ch.red;
bridadan 5:710031221b4d 46 diff_r += tmp * tmp;
bridadan 5:710031221b4d 47
bridadan 5:710031221b4d 48 tmp = (double)sample_buf[i].ch.green - (double)mean.ch.green;
bridadan 5:710031221b4d 49 diff_g += tmp * tmp;
bridadan 5:710031221b4d 50
bridadan 5:710031221b4d 51 tmp = (double)sample_buf[i].ch.blue - (double)mean.ch.blue;
bridadan 5:710031221b4d 52 diff_b += tmp * tmp;
bridadan 5:710031221b4d 53
bridadan 5:710031221b4d 54 tmp = (double)sample_buf[i].ch.clear - (double)mean.ch.clear;
bridadan 5:710031221b4d 55 diff_c += tmp * tmp;
bridadan 5:710031221b4d 56 }
bridadan 5:710031221b4d 57
bridadan 5:710031221b4d 58
bridadan 5:710031221b4d 59 diff_r = sqrt(diff_r/numSamples);
bridadan 5:710031221b4d 60 diff_g = sqrt(diff_g/numSamples);
bridadan 5:710031221b4d 61 diff_b = sqrt(diff_b/numSamples);
bridadan 5:710031221b4d 62 diff_c = sqrt(diff_c/numSamples);
bridadan 5:710031221b4d 63
bridadan 5:710031221b4d 64
bridadan 5:710031221b4d 65 variance->ch.red = (uint16_t)diff_r;
bridadan 5:710031221b4d 66 variance->ch.green = (uint16_t)diff_g;
bridadan 5:710031221b4d 67 variance->ch.blue = (uint16_t)diff_b;
bridadan 5:710031221b4d 68 variance->ch.clear = (uint16_t)diff_c;
bridadan 0:b6814eefafc8 69 }
bridadan 0:b6814eefafc8 70
bridadan 3:c05a96428ce8 71
bridadan 5:710031221b4d 72 void ColorDetector::setBaseline() {
bridadan 5:710031221b4d 73 RGBC variance;
bridadan 5:710031221b4d 74 bool cont = true;
bridadan 0:b6814eefafc8 75
bridadan 5:710031221b4d 76 while(cont) {
bridadan 5:710031221b4d 77 baselineAvg = takeSamples(samplesPerReading);
bridadan 5:710031221b4d 78 calculateVariance(&variance, sample_buf, curBuffPointer, baselineAvg);
bridadan 5:710031221b4d 79 cont = !isSteady(&variance);
bridadan 5:710031221b4d 80 }
bridadan 5:710031221b4d 81 }
bridadan 5:710031221b4d 82
bridadan 5:710031221b4d 83 bool ColorDetector::isSteady(RGBC *variance) {
bridadan 5:710031221b4d 84 bool steady = true;
bridadan 0:b6814eefafc8 85 for (int i = 0; i < 4; i++) {
bridadan 5:710031221b4d 86 if (variance->data[i] > varianceThreshold) {
bridadan 5:710031221b4d 87 steady = false;
bridadan 5:710031221b4d 88 }
bridadan 5:710031221b4d 89 }
bridadan 5:710031221b4d 90 return steady;
bridadan 5:710031221b4d 91 }
bridadan 5:710031221b4d 92
bridadan 5:710031221b4d 93 int ColorDetector::sample() {
bridadan 5:710031221b4d 94 RGBC variance;
bridadan 5:710031221b4d 95 int retVal = 0;
bridadan 5:710031221b4d 96
bridadan 5:710031221b4d 97 lastAvg = takeSamples(samplesPerReading);
bridadan 5:710031221b4d 98 calculateVariance(&variance, sample_buf, curBuffPointer, lastAvg);
bridadan 5:710031221b4d 99
bridadan 5:710031221b4d 100 if (isSteady(&variance)) {
bridadan 5:710031221b4d 101 // Calculate diff
bridadan 5:710031221b4d 102 int32_t totalDiff = 0;
bridadan 4:a6d6f8f41266 103
bridadan 5:710031221b4d 104 for (int i = 0; i < 4; i++) {
bridadan 5:710031221b4d 105 int diff = abs(((int)lastAvg.data[i]) - ((int)baselineAvg.data[i]));
bridadan 5:710031221b4d 106
bridadan 5:710031221b4d 107 if (diff > 128) {
bridadan 5:710031221b4d 108 diff = 255 - diff;
bridadan 5:710031221b4d 109 }
bridadan 5:710031221b4d 110
bridadan 5:710031221b4d 111 totalDiff += diff;
bridadan 4:a6d6f8f41266 112 }
bridadan 4:a6d6f8f41266 113
bridadan 5:710031221b4d 114 for (int i = 0; i < DIFF_BUFF_LEN - 1; i++) {
bridadan 5:710031221b4d 115 diffBuff[i] = diffBuff[i + 1];
bridadan 0:b6814eefafc8 116 }
bridadan 0:b6814eefafc8 117
bridadan 5:710031221b4d 118 diffBuff[DIFF_BUFF_LEN - 1] = totalDiff;
bridadan 5:710031221b4d 119
bridadan 5:710031221b4d 120 printf("totalDiff: %d\r\n", totalDiff);
bridadan 5:710031221b4d 121 if (totalDiff > threshold) {
bridadan 5:710031221b4d 122 if (getTotalPastDiff() < diffThreshold && !changeDetected) {
bridadan 5:710031221b4d 123 changeDetected = true;
bridadan 5:710031221b4d 124 retVal = curBuffPointer;
bridadan 5:710031221b4d 125 }
bridadan 5:710031221b4d 126 } else {
bridadan 5:710031221b4d 127 if (changeDetected) {
bridadan 5:710031221b4d 128 changeDetected = false;
bridadan 5:710031221b4d 129 }
bridadan 5:710031221b4d 130 }
bridadan 0:b6814eefafc8 131 }
bridadan 0:b6814eefafc8 132
bridadan 5:710031221b4d 133 curBuffPointer = 0;
bridadan 0:b6814eefafc8 134 return retVal;
bridadan 0:b6814eefafc8 135 }
bridadan 0:b6814eefafc8 136
bridadan 5:710031221b4d 137 int32_t ColorDetector::getTotalPastDiff() {
bridadan 5:710031221b4d 138 int32_t totalDiff = 0;
bridadan 5:710031221b4d 139 for (int i = 0; i < DIFF_BUFF_LEN - 1; i++) {
bridadan 5:710031221b4d 140 totalDiff += abs(diffBuff[DIFF_BUFF_LEN - 1] - diffBuff[i]);
bridadan 5:710031221b4d 141 }
bridadan 5:710031221b4d 142 return totalDiff;
bridadan 5:710031221b4d 143 }
bridadan 0:b6814eefafc8 144 RGBC* ColorDetector::getBuffer() {
bridadan 0:b6814eefafc8 145 return sample_buf;
bridadan 0:b6814eefafc8 146 }