Diff: edge_sensor/edge_color.cpp

Revision:

0:0b6732b53bf4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/edge_sensor/edge_color.cpp	Tue Apr 24 08:58:33 2018 +0000
@@ -0,0 +1,443 @@
+#include "mbed.h"
+#include "edge_sensor.h"
+#include "VEML6040.h"
+#include "edge_color.h"
+#include "edge_reset_mgr.h"
+#include "edge_chart.h"
+
+/* VEML6040 config bits */
+/* sensor config loser 4bit */
+/* trigger mode etc. */
+#define SD_BIT   0x01
+#define AF_BIT   0x02
+#define TRIG_BIT 0x04
+
+/* sensor config upper 4bit */
+/* integration time */
+ int sensor_delay[] = { 
+    40,
+    80,
+    160,
+    320,
+    640,
+    1280, 
+    1280, /* place holder */ 
+    1280  /* place holder */
+} ;
+
+uint16_t        color0_pwm[3] ;
+uint16_t        color1_pwm[3] ;
+uint16_t        color0_target[3] = { 3500, 3500, 3500 } ;
+uint16_t        color1_target[3] = { 3500, 3500, 3500 } ;
+
+
+edge_color::edge_color(VEML6040 *sensor, PwmOut *led[], uint16_t *pwm) : edge_sensor() 
+{
+    uint16_t dummy[3] ;
+    _sensor = sensor ;
+    _sensor_config = AF_BIT | TRIG_BIT ;
+    _interval = 30 ;
+    _calibration_request = 0 ; /* 1 for testing */ 
+
+reset_watch_dog() ;
+    _pwm_period = 2000 ; /* 2ms */
+    _probe = 0xFA00 ; /* to avoid satulation at 255, using 250 */
+//    _probe = 0xFF00 ; 
+    for (int i = 0 ; i < 3 ; i++ ) {
+        _led[i] = led[i] ;
+        _led[i]->write(1.0) ; /* turn LED off */
+        _value[i] = 0 ;
+        _pwm[i] = pwm[i] ;
+        _led[i]->period_us(_pwm_period) ;
+    }
+    getRGB(dummy) ; // dummy read, the first data is usually garbage
+reset_watch_dog() ;
+}
+
+edge_color::~edge_color(void) 
+{
+    delete _sensor ;
+    delete [] _led ;
+}
+
+void edge_color::setLEDs(uint16_t led_value[]) 
+{
+    for (int i = 0 ; i < 3 ; i++ ) {
+        _led[i]->write((float)(65535 - led_value[i])/65535.0) ;
+    }
+}
+
+void edge_color::setLEDs(uint16_t r, uint16_t g, uint16_t b) 
+{
+    _led[0]->write((float)(65535 - r)/65535.0) ;
+    _led[1]->write((float)(65535 - g)/65535.0) ;
+    _led[2]->write((float)(65535 - b)/65535.0) ;
+}
+
+void edge_color::reset(void) 
+{
+    for (int i = 0 ; i < 3 ; i++ ) {
+        _value[i] = 0 ;
+    }
+}
+
+void edge_color::prepare(void) 
+{
+//    setLEDs(_pwm) ; // <- the other color sensor turns off (;_;)
+}
+
+int edge_color::sample(void) 
+{
+    int result ;
+reset_watch_dog() ;
+    setLEDs(_pwm) ;
+reset_watch_dog() ;
+    result = getRGB(_value) ;
+    _sampled_time = edge_time ;
+    setLEDs(0, 0, 0) ; /* turn LEDs off */
+reset_watch_dog() ;
+    return( result ) ;
+}
+
+int edge_color::deliver(void) 
+{
+    int result ;
+    char timestr[16] ;
+    print_time(_sampled_time) ;
+    time2seq(_sampled_time, timestr) ;
+    printf(" color%d : R = %4d, G = %4d, B = %4d\n",
+        _id, _value[0], _value[1], _value[2]) ;
+    if (_id == 1) { /* color1 */
+    sprintf(_str_buf,
+          "{\"DEVICE\":\"COLOR\",\"PN\":\"VEML6040\",\"VAL_R\":\"%d\",\"VAL_G\":\"%d\",\"VAL_B\":\"%d\",\"UNIT\":\"mW/cm2\",\"T\":\"%s\",\"E\":\"%d\"}",
+          _value[0], _value[1], _value[2], timestr, _error_count) ; 
+    } else { /* color2 */
+        sprintf(_str_buf,
+          "{\"DEVICE\":\"COLOR02\",\"PN\":\"VEML6040\",\"VAL_R\":\"%d\",\"VAL_G\":\"%d\",\"VAL_B\":\"%d\",\"UNIT\":\"mW/cm2\",\"T\":\"%s\",\"E\":\"%d\"}",
+          _value[0], _value[1], _value[2], timestr, _error_count) ; 
+    }
+    result = afero->setAttribute(1, _str_buf) ;
+
+    return( result == afSUCCESS ) ;
+}
+
+int color_v2y(float value, edge_chart_type *p)
+{
+    int y ;
+    if (value < p->min) {
+        value = p->min ;
+    } else if (value > p->max) {
+        value = p->max ;
+    }
+    y = p->top + p->height - 1
+        - (int)((p->height - 2) * value /(p->max - p->min)) ;
+    return( y ) ;
+}
+
+void edge_color::show(void)
+{
+    int r, g, b ;
+    int x ;
+    edge_chart_type *p = &edge_chart[_id] ;
+    if (display) {
+        switch(display_mode) {
+        case DISPLAY_MODE_SUMMARY:
+            reset_watch_dog() ;
+            display->BusEnable(true) ;
+            display->set_font((unsigned char*) Arial12x12);
+            display->set_font_zoom(2, 2) ;
+            display->foreground(White) ;
+            display->locate(EDGE_SUMMARY_X, EDGE_SUMMARY_TIME_Y) ;
+            displayTime(_sampled_time) ;
+            if (_id == 1) {
+                display->locate(EDGE_SUMMARY_X, EDGE_SUMMARY_COLOR1_Y) ;
+                display->printf("Color :%5d,%5d,%5d",
+                    _value[0], _value[1], _value[2]) ;
+            } else {
+                display->locate(EDGE_SUMMARY_X, EDGE_SUMMARY_COLOR2_Y) ;
+                display->printf("Color2:%5d,%5d,%5d",
+                    _value[0], _value[1], _value[2]) ;
+            }
+            display->BusEnable(false) ;
+            reset_watch_dog() ;
+            break ;
+        case DISPLAY_MODE_CHART:
+            reset_watch_dog() ;
+            x = p->left + p->index + 1 ;
+            r = color_v2y(_value[0], p) ;
+            g = color_v2y(_value[1], p) ;
+            b = color_v2y(_value[2], p) ;
+            display->BusEnable(true) ;
+            if (p->index == 0) {
+                draw_chart_frame(p) ;
+            }
+            display->pixel(x, r, Red) ;
+            display->pixel(x, g, Green) ;
+            display->pixel(x, b, Blue) ;
+            display->BusEnable(false) ;
+            p->index = (p->index + 1) % (p->width - 2) ;
+            break ;       
+        }
+    }   
+    reset_watch_dog() ;
+}
+
+int edge_color::getRGB(uint16_t v[])
+{
+    int result ;
+    result = _sensor->setCOLORConf(_sensor_config) ;
+    if (result == 0) {
+        wait_ms(sensor_delay[(_sensor_config >> 4)&0x07] * 1.25) ;
+     
+        result = _sensor->getRData(&v[0]) ;
+        if (result == 0) {
+            wait_ms(10) ;
+            result = _sensor->getGData(&v[1]) ;
+            if (result == 0) {
+                wait_ms(10) ;
+                result = _sensor->getBData(&v[2]) ;
+                if (result == 0) {
+                    wait_ms(10) ;
+                }
+            }
+        }
+    }
+    return( result ) ;
+}
+
+/**
+ * Measure num_ave + 2 times
+ * and throw away min and max
+ * before calculating average
+ */ 
+void edge_color::getAveColor(uint16_t led[], uint16_t v[], int num_ave)
+{
+    int i, c ;
+    uint16_t min[3] = { 0, 0, 0 } ;
+    uint16_t max[3] = { 0, 0, 0 } ;
+    uint16_t tmp[3] ;
+    long sum[3] = { 0, 0, 0 } ;
+        
+reset_watch_dog() ;
+    setLEDs(led) ;
+    getRGB(tmp) ; // dummy read
+    setLEDs(0, 0, 0) ;
+    wait_ms(10) ;
+    for (i = 0 ; i < num_ave+2 ; i++ ) {
+reset_watch_dog() ;
+        setLEDs(led) ;
+        getRGB(tmp) ;
+        setLEDs(0, 0, 0) ;
+        wait_ms(10) ;
+        for (c = 0 ; c < 3 ; c++ ) {
+            sum[c] += tmp[c] ;
+            if ((i == 0) || (tmp[c] < min[c])) {
+                min[c] = tmp[c] ;
+            }
+            if ((i == 0) || (tmp[c] > max[c])) {
+                max[c] = tmp[c] ;
+            }
+        }
+    }    
+reset_watch_dog() ;
+    for (c = 0 ; c < 3 ; c++ ) {
+        sum[c] = sum[c] - (min[c] + max[c]) ;
+        v[c] = (uint16_t)(sum[c] / num_ave) ;
+    }
+//    delete [] tmp ;
+// printf("=== average ===\n") ;
+// printf("%04x %04x %04x\n", v[0], v[1], v[2]) ;
+}
+
+#if 1
+void edge_color::calibrate(uint16_t target[], uint16_t result[], int num_ave) 
+{
+//   const uint16_t led_interval = 10 ; /* wait 10ms for LED */
+    float      denominator ;
+    float      numerator[3] ;
+    float      a,b,c,d,e,f,g,h,i ;
+    uint16_t  v[3], tmp[3] ;
+    uint16_t  L[3][3] ;
+    int       idx ;
+    uint8_t   conf ;
+
+    printf("=== Calibrating Color Sensor %d ===\n", _id) ;
+    for (idx = 0 ; idx < 3 ; idx++ ) {
+reset_watch_dog() ;
+        tmp[0] = tmp[1] = tmp[2] = 0 ;
+        tmp[idx] = _probe ;
+
+//        setLEDs(tmp) ;
+//        wait_ms(led_interval) ;
+        getAveColor(tmp, v, num_ave) ;
+        
+        printf("R:%5d, G:%5d, B:%5d\n", v[0], v[1], v[2]) ;
+        L[idx][0] = v[0] ;
+        L[idx][1] = v[1] ;
+        L[idx][2] = v[2] ;
+//        setLEDs(0, 0, 0) ; /* clear LEDs */
+    }
+    
+reset_watch_dog() ;
+    printf("=== Initial Equation ===\n") ;
+    for (idx = 0 ; idx < 3 ; idx++) {
+        printf("%5d * R / %d + %5d * G / %d + %5d * B / %d = %d,\n",
+        L[0][idx], _probe, L[1][idx], _probe,  L[2][idx], _probe, target[idx]) ;
+    }
+
+     a = L[0][0] ; b = L[1][0] ; c = L[2][0] ;
+     d = L[0][1] ; e = L[1][1] ; f = L[2][1] ;
+     g = L[0][2] ; h = L[1][2] ; i = L[2][2] ;
+     
+    denominator = a * (f * h - e * i) + b * (d * i - f * g) + c * (e * g - d * h) ;
+//    printf("Denominator = %f\n", denominator) ;
+
+    if (denominator != 0) {
+        numerator[0] = (f * h - e * i) * target[0] 
+            + b * (i * target[1] - f * target[2]) 
+            + c * (e * target[2] - h * target[1]) ;
+
+        numerator[1] = -((f * g - d * i) * target[0]
+            + a * (i * target[1] - f * target[2]) 
+            + c * (d * target[2] - g * target[1])) ;
+
+        numerator[2] = (e * g - d * h) * target[0]
+            + a * (h * target[1] - e * target[2]) 
+            + b * (d * target[2] - g * target[1]) ;
+
+        for (idx = 0 ; idx < 3 ; idx++ ) {
+// printf("Numerator[%d] = %f\n", idx, numerator[idx]) ;
+            _pwm[idx] = (uint16_t) (0.5 + (((double)_probe * numerator[idx]) / denominator)) ;
+            result[idx] = _pwm[idx] ;
+        }
+
+        printf("PWM R = %d [0x%04x] ", result[0], result[0]) ; 
+        wait_ms(1) ;
+        printf("G = %d [0x%04x] ", result[1], result[1]) ;
+        wait_ms(1) ;
+        printf("B = %d [0x%04x] ", result[2], result[2]) ;
+        wait_ms(1) ;
+        printf("\n") ;
+        wait_ms(1) ;
+        printf("=== test ===\n") ;
+//        setLEDs(_pwm[0], _pwm[1], _pwm[2]) ;
+//        wait_ms(led_interval) ;
+        getAveColor(_pwm, v, num_ave) ;
+        printf("R:%d, G:%d, B:%d\n", v[0], v[1], v[2]) ;
+        printf("============\n") ;
+        wait_ms(1) ;
+    } else {
+        printf("calibration failed, pwm values were not updated\n") ;
+    }
+    printf("Reseting Color Sensor ... ") ;
+reset_watch_dog() ;
+    _sensor->getCOLORConf(&conf) ;
+    wait_ms(10) ;
+    _sensor->setCOLORConf(conf | 0x01) ; /* shutdown VEML6040 */
+    wait_ms(200) ;
+reset_watch_dog() ;
+    _sensor->setCOLORConf(conf) ;
+    wait_ms(200) ;
+    printf("Done\n") ;
+    _calibration_request = 0 ;
+    _status = EDGE_SENSOR_INACTIVE ;
+reset_watch_dog() ;
+}
+#endif /* calibration int version */
+
+#if 0
+void edge_color::calibrate(uint16_t target[], uint16_t result[], int num_ave) 
+{
+   const uint16_t led_interval = 10 ; /* wait 10ms for LED */
+    double      denominator ;
+    double      numerator[3] ;
+    double      a,b,c,d,e,f,g,h,i ;
+    uint16_t    v[3], tmp[3] ;
+//    uint16_t    L[3][3] ;
+    double    L[3][3] ;
+    double      ftarget[3] ;
+    int         idx ;
+    uint8_t     conf ;
+    
+    ftarget[0] = target[0] ;
+    ftarget[1] = target[1] ;
+    ftarget[2] = target[2] ;
+    printf("=== Calibrating Color Sensor %d ===\n", _id) ;
+    for (idx = 0 ; idx < 3 ; idx++ ) {
+reset_watch_dog() ;
+        tmp[0] = tmp[1] = tmp[2] = 0 ;
+        tmp[idx] = _probe ;
+
+        setLEDs(tmp) ;
+        wait_ms(led_interval) ;
+        getAveColor(v, num_ave) ;
+        
+        printf("R:%5d, G:%5d, B:%5d\n", v[0], v[1], v[2]) ;
+        L[idx][0] = v[0] ;
+        L[idx][1] = v[1] ;
+        L[idx][2] = v[2] ;
+        setLEDs(0, 0, 0) ; /* clear LEDs */
+    }
+    
+reset_watch_dog() ;
+    printf("=== Initial Equation ===\n") ;
+    for (idx = 0 ; idx < 3 ; idx++) {
+        printf("%5d * R / %d + %5d * G / %d + %5d * B / %d = %d,\n",
+        (int)L[0][idx], _probe, (int)L[1][idx], _probe,  (int)L[2][idx], _probe, target[idx]) ;
+    }
+
+     a = L[0][0] ; b = L[1][0] ; c = L[2][0] ;
+     d = L[0][1] ; e = L[1][1] ; f = L[2][1] ;
+     g = L[0][2] ; h = L[1][2] ; i = L[2][2] ;
+     
+    denominator = a * (f * h - e * i) + b * (d * i - f * g) + c * (e * g - d * h) ;
+
+    if (denominator != 0) {
+        numerator[0] = (f * h - e * i) * ftarget[0] 
+            + b * (i * ftarget[1] - f * ftarget[2]) 
+            + c * (e * ftarget[2] - h * ftarget[1]) ;
+
+        numerator[1] = -((f * g - d * i) * ftarget[0]
+            + a * (i * ftarget[1] - f * ftarget[2]) 
+            + c * (d * ftarget[2] - g * ftarget[1])) ;
+
+        numerator[2] = (e * g - d * h) * ftarget[0]
+            + a * (h * ftarget[1] - e * ftarget[2]) 
+            + b * (d * ftarget[2] - g * ftarget[1]) ;
+
+        for (idx = 0 ; idx < 3 ; idx++ ) {
+            _pwm[idx] = (uint16_t) (0.5 + ((double)_probe * numerator[idx]) / denominator) ;
+            result[idx] = _pwm[idx] ;
+        }
+
+        printf("PWM R = %d [0x%04x] ", result[0], result[0]) ; 
+        wait_ms(1) ;
+        printf("G = %d [0x%04x] ", result[1], result[1]) ;
+        wait_ms(1) ;
+        printf("B = %d [0x%04x] ", result[2], result[2]) ;
+        wait_ms(1) ;
+        printf("\n") ;
+        wait_ms(1) ;
+        printf("=== test ===\n") ;
+        setLEDs(_pwm[0], _pwm[1], _pwm[2]) ;
+        wait_ms(led_interval) ;
+        getAveColor(v, num_ave) ;
+        printf("R:%d, G:%d, B:%d\n", v[0], v[1], v[2]) ;
+        printf("============\n") ;
+        wait_ms(1) ;
+    } else {
+        printf("calibration failed, pwm values were not updated\n") ;
+    }
+reset_watch_dog() ;
+    _sensor->getCOLORConf(&conf) ;
+    wait_ms(10) ;
+    _sensor->setCOLORConf(conf | 0x01) ; /* shutdown VEML6040 */
+    wait_ms(200) ;
+reset_watch_dog() ;
+    _sensor->setCOLORConf(conf) ;
+    wait_ms(200) ;
+    _calibration_request = 0 ;
+    _status = EDGE_SENSOR_INACTIVE ;
+reset_watch_dog() ;
+}
+#endif /* calibration double version */