From 6c6433404a517eb1c6fc49af33651138bd882c3b Mon Sep 17 00:00:00 2001 From: Paul Date: Mon, 17 Mar 2014 17:25:14 +0100 Subject: [PATCH] Improved calibration stability Decreased possibility that second (red) calibration disk is not detected; adjusted calibration code to more closely resemble code within App --- Tonino.ino | 161 ++++++++++++++++++-------------------- Tonino/tonino_tcs3200.cpp | 36 ++++++--- Tonino/tonino_tcs3200.h | 8 +- 3 files changed, 105 insertions(+), 100 deletions(-) diff --git a/Tonino.ino b/Tonino.ino index b901ad4..2be0e56 100644 --- a/Tonino.ino +++ b/Tonino.ino @@ -1,5 +1,3 @@ -// Tonino.ino -//---------------- // Arduino code for Tonino (my-tonino.com) using // - DFRduino Nano // - Adafruit 0.56" 7-segment LCD backpack display @@ -46,7 +44,7 @@ // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. // ------------------------------------------------------------------------------------------ -#define VERSION "1 0 7" +#define VERSION "1 0 8" #include #include @@ -147,7 +145,6 @@ void setup() { if (tConfig.getCheckCalInit() && colorSense.isCalibrating() == LOW_PLATE) { display.clear(); if (calibrate()) { - delay(200); display.done(); } } else { @@ -206,6 +203,7 @@ void loop() { delay(750); display.circle(1, 500); display.clear(); + wdt_reset(); delay(500); scanAndDisplay(); @@ -272,42 +270,43 @@ boolean calibrate() { display.calibration1(); WRITEDEBUG("Calibrating ... "); - delay(1000); + delay(1500); + + // scan 1 + colorSense.scan(NULL, false, &sd_1); + if (tSerial.checkCommands()) return false; + display.calibration1(); + delay(750); + if (tSerial.checkCommands()) return false; + + // dark scan to remove potential external light + sensorData sd_d; + colorSense.scan(NULL, false, &sd_d, false); + if (tSerial.checkCommands()) return false; + display.calibration1(); + delay(750); + if (tSerial.checkCommands()) return false; + + // scan 2 + sensorData sd_2; + colorSense.scan(NULL, false, &sd_2); + display.calibration1(); + delay(200); + + float redavg = (sd_1.value[RED_IDX] + sd_2.value[RED_IDX]) / 2.0 - sd_d.value[RED_IDX]; + float blueavg = (sd_1.value[BLUE_IDX] + sd_2.value[BLUE_IDX]) / 2.0 - sd_d.value[BLUE_IDX]; - // scan for calibration plate 1 - int32_t tval1 = colorSense.scan(NULL, false, &sd_1); - - if ((abs(sd_1.value[RED_IDX]-LOW_RED) < RED_RANGE_LOW) && - (abs(sd_1.value[BLUE_IDX]-LOW_BLUE) < BLUE_RANGE_LOW)) { - // really found calibration plate 1 - do a second scan for averaging and display OK - sensorData sd_2; - display.calibration1(); - - // dark scan to remove potential external light - if (tSerial.checkCommands()) return false; - delay(750); - if (tSerial.checkCommands()) return false; - sensorData dsd_2; - colorSense.scan(NULL, false, &dsd_2, false); - delay(750); - if (tSerial.checkCommands()) return false; - - // make a second scan and calc an average - int32_t tval2 = colorSense.scan(NULL, false, &sd_2); - - float rb_low = (sd_1.value[RED_IDX] + sd_2.value[RED_IDX] - 2*dsd_2.value[RED_IDX]) / - (float)(sd_1.value[BLUE_IDX] + sd_2.value[BLUE_IDX] - 2*dsd_2.value[BLUE_IDX]); - - WRITEDEBUG((sd_1.value[RED_IDX] + sd_2.value[RED_IDX] - 2*dsd_2.value[RED_IDX]) / 2); + if ((abs(redavg - LOW_RED) < RED_RANGE_LOW) && (abs(blueavg - LOW_BLUE) < BLUE_RANGE_LOW)) { + + float rb_low = redavg / blueavg; + + WRITEDEBUG(redavg); WRITEDEBUG("/"); - WRITEDEBUG((sd_1.value[BLUE_IDX] + sd_2.value[BLUE_IDX] - 2*dsd_2.value[BLUE_IDX]) / 2); + WRITEDEBUG(blueavg); WRITEDEBUG("="); WRITEDEBUGF(rb_low, 5); WRITEDEBUG("; "); - // DON'T write measured average T-value to display as it is confusing for the user - // displayNum((int32_t)((tval1 + tval2) / 2.0 + 0.5)); - // wait for other plate while (true) { display.calibration2(); @@ -319,70 +318,62 @@ boolean calibrate() { } display.up(); while (colorSense.isLight()) { - delay(250); + delay(500); if (tSerial.checkCommands()) return false; } display.calibration2(); - delay(500); + delay(250); - // scan for calibration plate 2 - // this is done with a quick (only white) measurement - if (colorSense.isCalibrating() == HIGH_PLATE) { - delay(200); - // make a thorough scan - tval1 = colorSense.scan(NULL, false, &sd_1); + // scan 1 for calibration plate 2 + colorSense.scan(NULL, false, &sd_1); + if (tSerial.checkCommands()) return false; + display.calibration2(); + delay(750); + if (tSerial.checkCommands()) return false; + + // dark scan to remove potential external light + colorSense.scan(NULL, false, &sd_d, false); + if (tSerial.checkCommands()) return false; + display.calibration2(); + delay(750); + if (tSerial.checkCommands()) return false; - if ((abs(sd_1.value[RED_IDX]-HIGH_RED) < RED_RANGE_HIGH) && - (abs(sd_1.value[BLUE_IDX]-HIGH_BLUE) < BLUE_RANGE_HIGH)) { - // really found calibration plate 2 - calc and save calibration + // scan 2 + colorSense.scan(NULL, false, &sd_2); + display.calibration2(); + + redavg = (sd_1.value[RED_IDX] + sd_2.value[RED_IDX]) / 2.0 - sd_d.value[RED_IDX]; + blueavg = (sd_1.value[BLUE_IDX] + sd_2.value[BLUE_IDX]) / 2.0 - sd_d.value[BLUE_IDX]; - display.calibration2(); - if (tSerial.checkCommands()) return false; - - // dark scan to remove potential external light - delay(750); - colorSense.scan(NULL, false, &dsd_2, false); - if (tSerial.checkCommands()) return false; - delay(750); - if (tSerial.checkCommands()) return false; + float rb_high = redavg / blueavg; + + float cal[2]; + cal[0] = (HIGH_TARGET - LOW_TARGET) / (rb_high - rb_low); + cal[1] = LOW_TARGET - cal[0]*rb_low; + + WRITEDEBUG((sd_1.value[RED_IDX] + sd_2.value[RED_IDX] - 2*sd_d.value[RED_IDX]) / 2); + WRITEDEBUG("/"); + WRITEDEBUG((sd_1.value[BLUE_IDX] + sd_2.value[BLUE_IDX] - 2*sd_d.value[BLUE_IDX]) / 2); + WRITEDEBUG("="); + WRITEDEBUGF(rb_high, 5); - // make a second scan and calc an average - int32_t tval2 = colorSense.scan(NULL, false, &sd_2); - display.calibration2(); - float rb_high = (sd_1.value[RED_IDX] + sd_2.value[RED_IDX] - 2*dsd_2.value[RED_IDX]) / - (float)(sd_1.value[BLUE_IDX] + sd_2.value[BLUE_IDX] - 2*dsd_2.value[BLUE_IDX]); - - float cal[2]; - cal[0] = (HIGH_TARGET - LOW_TARGET) / (rb_high - rb_low); - cal[1] = LOW_TARGET - cal[0]*rb_low; - - WRITEDEBUG((sd_1.value[RED_IDX] + sd_2.value[RED_IDX] - 2*dsd_2.value[RED_IDX]) / 2); - WRITEDEBUG("/"); - WRITEDEBUG((sd_1.value[BLUE_IDX] + sd_2.value[BLUE_IDX] - 2*dsd_2.value[BLUE_IDX]) / 2); - WRITEDEBUG("="); - WRITEDEBUGF(rb_high, 5); - - WRITEDEBUG(" => "); - WRITEDEBUGF(cal[0], 5); - WRITEDEBUG(", "); - WRITEDEBUGLNF(cal[1], 5); - - // DON'T write measured average T-value to display as it is confusing for the user - // displayNum((int32_t)((tval1 + tval2) / 2.0 + 0.5)); - if (tSerial.checkCommands()) return false; - - tConfig.setCalibration(cal); - delay(2500); - return true; - } - } - delay(500); + WRITEDEBUG(" => "); + WRITEDEBUGF(cal[0], 5); + WRITEDEBUG(", "); + WRITEDEBUGLNF(cal[1], 5); + + if (tSerial.checkCommands()) return false; + + tConfig.setCalibration(cal); + delay(200); + return true; } } else { // could not detect first plate even though quick check thought so - error and continue display.error(); delay(3000); + return false; } } diff --git a/Tonino/tonino_tcs3200.cpp b/Tonino/tonino_tcs3200.cpp index 8bbc0dd..9bb0783 100644 --- a/Tonino/tonino_tcs3200.cpp +++ b/Tonino/tonino_tcs3200.cpp @@ -180,7 +180,7 @@ int32_t TCS3200::scan(float *raw, bool displayAnim, sensorData *outersd, boolean digitalWrite(_POWER, HIGH); delay(50); - WRITEDEBUG(" ext light: "); + WRITEDEBUGLN("ext light:"); uint32_t ds; if (_colorMode & COLOR_WHITE) { setFilter(WHITE_IDX); // white sensor @@ -236,7 +236,7 @@ void TCS3200::sensorOff() { } // returns 1 if the first, darker, 2 if the second, lighter calibration plate is detected -// 0 otherwise; a quick sampling with LEDs in conducted +// 0 otherwise; a quick sampling with LEDs switched on is conducted uint8_t TCS3200::isCalibrating() { uint8_t samplingBackup = _readDiv; _readDiv = QUICK_SAMPLING; @@ -247,6 +247,8 @@ uint8_t TCS3200::isCalibrating() { digitalWrite(_POWER, HIGH); digitalWrite(_LED, HIGH); + delay(50); + setFilter(RED_IDX); // red sensor int32_t wval = readSingle(); setFilter(BLUE_IDX); // blue sensor @@ -258,10 +260,10 @@ uint8_t TCS3200::isCalibrating() { uint8_t cal = 0; if ((abs(wval-LOW_RED) < RED_RANGE_LOW) && (abs(bval-LOW_BLUE) < BLUE_RANGE_LOW)) { // found first calibration plate - cal = 1; + cal = LOW_PLATE; } else if ((abs(wval-HIGH_RED) < RED_RANGE_HIGH) && (abs(bval-HIGH_BLUE) < BLUE_RANGE_HIGH)) { // found second calibration plate - cal = 2; + cal = HIGH_PLATE; } // else found no calibration plate @@ -313,15 +315,27 @@ uint32_t TCS3200::readSingle(void) { // set the sensor color filter void TCS3200::setFilter(uint8_t f) { - WRITEDEBUG("setFilter "); + //WRITEDEBUG("setFilter "); switch (f) { - case RED_IDX: WRITEDEBUGLN("R"); digitalWrite(_S2, LOW); digitalWrite(_S3, LOW); break; - case GREEN_IDX: WRITEDEBUGLN("G"); digitalWrite(_S2, HIGH); digitalWrite(_S3, HIGH); break; - case BLUE_IDX: WRITEDEBUGLN("B"); digitalWrite(_S2, LOW); digitalWrite(_S3, HIGH); break; - case WHITE_IDX: WRITEDEBUGLN("X"); digitalWrite(_S2, HIGH); digitalWrite(_S3, LOW); break; + case RED_IDX: /*WRITEDEBUGLN("R");*/ + digitalWrite(_S2, LOW); + digitalWrite(_S3, LOW); + break; + case GREEN_IDX: /*WRITEDEBUGLN("G");*/ + digitalWrite(_S2, HIGH); + digitalWrite(_S3, HIGH); + break; + case BLUE_IDX: /*WRITEDEBUGLN("B");*/ + digitalWrite(_S2, LOW); + digitalWrite(_S3, HIGH); + break; + case WHITE_IDX: /*WRITEDEBUGLN("X");*/ + digitalWrite(_S2, HIGH); + digitalWrite(_S3, LOW); + break; default: - WRITEDEBUG("ERROR: unknown filter: "); - WRITEDEBUGLN(f); + WRITEDEBUG("ERROR: unknown filter: "); + WRITEDEBUGLN(f); } } diff --git a/Tonino/tonino_tcs3200.h b/Tonino/tonino_tcs3200.h index 263f59a..99b18b9 100644 --- a/Tonino/tonino_tcs3200.h +++ b/Tonino/tonino_tcs3200.h @@ -87,10 +87,10 @@ #define LOW_BLUE 1470 // brown disk blue reading #define HIGH_RED 11300 // red disk white reading #define HIGH_BLUE 3050 // red disk blue reading -#define RED_RANGE_LOW 1200 -#define RED_RANGE_HIGH 4200 -#define BLUE_RANGE_LOW 750 -#define BLUE_RANGE_HIGH 1450 +#define RED_RANGE_LOW 1300 +#define RED_RANGE_HIGH 4300 +#define BLUE_RANGE_LOW 800 +#define BLUE_RANGE_HIGH 1550 #define LOW_TARGET 1.5 // brown disk target r/b value #define HIGH_TARGET 3.7 // red disk target r/b value