edge_color.cpp

00001 #include "mbed.h"
00002 #include "edge_sensor.h"
00003 #include "VEML6040.h"
00004 #include "edge_color.h"
00005 #include "edge_reset_mgr.h"
00006 #include "edge_chart.h"
00007 
00008 /* VEML6040 config bits */
00009 /* sensor config loser 4bit */
00010 /* trigger mode etc. */
00011 #define SD_BIT   0x01
00012 #define AF_BIT   0x02
00013 #define TRIG_BIT 0x04
00014 
00015 /* sensor config upper 4bit */
00016 /* integration time */
00017  int sensor_delay[] = { 
00018     40,
00019     80,
00020     160,
00021     320,
00022     640,
00023     1280, 
00024     1280, /* place holder */ 
00025     1280  /* place holder */
00026 } ;
00027 
00028 uint16_t        color0_pwm[3] ;
00029 uint16_t        color1_pwm[3] ;
00030 uint16_t        color0_target[3] = { 3500, 3500, 3500 } ;
00031 uint16_t        color1_target[3] = { 3500, 3500, 3500 } ;
00032 
00033 
00034 edge_color::edge_color(VEML6040 *sensor, PwmOut *led[], uint16_t *pwm) : edge_sensor() 
00035 {
00036     uint16_t dummy[3] ;
00037     _sensor = sensor ;
00038     _sensor_config = AF_BIT | TRIG_BIT ;
00039     _interval = 30 ;
00040     _calibration_request = 0 ; /* 1 for testing */ 
00041 
00042 reset_watch_dog() ;
00043     _pwm_period = 2000 ; /* 2ms */
00044     _probe = 0xFA00 ; /* to avoid satulation at 255, using 250 */
00045 //    _probe = 0xFF00 ; 
00046     for (int i = 0 ; i < 3 ; i++ ) {
00047         _led[i] = led[i] ;
00048         _led[i]->write(1.0) ; /* turn LED off */
00049         _value[i] = 0 ;
00050         _pwm[i] = pwm[i] ;
00051         _led[i]->period_us(_pwm_period) ;
00052     }
00053     getRGB(dummy) ; // dummy read, the first data is usually garbage
00054 reset_watch_dog() ;
00055 }
00056 
00057 edge_color::~edge_color(void) 
00058 {
00059     delete _sensor ;
00060     delete [] _led ;
00061 }
00062 
00063 void edge_color::setLEDs(uint16_t led_value[]) 
00064 {
00065     for (int i = 0 ; i < 3 ; i++ ) {
00066         _led[i]->write((float)(65535 - led_value[i])/65535.0) ;
00067     }
00068 }
00069 
00070 void edge_color::setLEDs(uint16_t r, uint16_t g, uint16_t b) 
00071 {
00072     _led[0]->write((float)(65535 - r)/65535.0) ;
00073     _led[1]->write((float)(65535 - g)/65535.0) ;
00074     _led[2]->write((float)(65535 - b)/65535.0) ;
00075 }
00076 
00077 void edge_color::reset(void) 
00078 {
00079     for (int i = 0 ; i < 3 ; i++ ) {
00080         _value[i] = 0 ;
00081     }
00082 }
00083 
00084 void edge_color::prepare(void) 
00085 {
00086 //    setLEDs(_pwm) ; // <- the other color sensor turns off (;_;)
00087 }
00088 
00089 int edge_color::sample(void) 
00090 {
00091     int result ;
00092 reset_watch_dog() ;
00093     setLEDs(_pwm) ;
00094 reset_watch_dog() ;
00095     result = getRGB(_value) ;
00096     _sampled_time = edge_time ;
00097     setLEDs(0, 0, 0) ; /* turn LEDs off */
00098 reset_watch_dog() ;
00099     return( result ) ;
00100 }
00101 
00102 int edge_color::deliver(void) 
00103 {
00104     int result ;
00105     char timestr[16] ;
00106     print_time(_sampled_time) ;
00107     time2seq(_sampled_time, timestr) ;
00108     printf(" color%d : R = %4d, G = %4d, B = %4d\n",
00109         _id, _value[0], _value[1], _value[2]) ;
00110     if (_id == 1) { /* color1 */
00111     sprintf(_str_buf,
00112           "{\"DEVICE\":\"COLOR\",\"PN\":\"VEML6040\",\"VAL_R\":\"%d\",\"VAL_G\":\"%d\",\"VAL_B\":\"%d\",\"UNIT\":\"mW/cm2\",\"T\":\"%s\",\"E\":\"%d\"}",
00113           _value[0], _value[1], _value[2], timestr, _error_count) ; 
00114     } else { /* color2 */
00115         sprintf(_str_buf,
00116           "{\"DEVICE\":\"COLOR02\",\"PN\":\"VEML6040\",\"VAL_R\":\"%d\",\"VAL_G\":\"%d\",\"VAL_B\":\"%d\",\"UNIT\":\"mW/cm2\",\"T\":\"%s\",\"E\":\"%d\"}",
00117           _value[0], _value[1], _value[2], timestr, _error_count) ; 
00118     }
00119     result = afero->setAttribute(1, _str_buf) ;
00120 
00121     return( result == afSUCCESS ) ;
00122 }
00123 
00124 int color_v2y(float value, edge_chart_type *p)
00125 {
00126     int y ;
00127     if (value < p->min) {
00128         value = p->min ;
00129     } else if (value > p->max) {
00130         value = p->max ;
00131     }
00132     y = p->top + p->height - 1
00133         - (int)((p->height - 2) * value /(p->max - p->min)) ;
00134     return( y ) ;
00135 }
00136 
00137 void edge_color::show(void)
00138 {
00139     int r, g, b ;
00140     int x ;
00141     edge_chart_type *p = &edge_chart[_id] ;
00142     if (display) {
00143         switch(display_mode) {
00144         case DISPLAY_MODE_SUMMARY:
00145             reset_watch_dog() ;
00146             display->BusEnable(true) ;
00147             display->set_font((unsigned char*) Arial12x12);
00148             display->set_font_zoom(2, 2) ;
00149             display->foreground(White) ;
00150             display->locate(EDGE_SUMMARY_X, EDGE_SUMMARY_TIME_Y) ;
00151             displayTime(_sampled_time) ;
00152             if (_id == 1) {
00153                 display->locate(EDGE_SUMMARY_X, EDGE_SUMMARY_COLOR1_Y) ;
00154                 display->printf("Color :%5d,%5d,%5d",
00155                     _value[0], _value[1], _value[2]) ;
00156             } else {
00157                 display->locate(EDGE_SUMMARY_X, EDGE_SUMMARY_COLOR2_Y) ;
00158                 display->printf("Color2:%5d,%5d,%5d",
00159                     _value[0], _value[1], _value[2]) ;
00160             }
00161             display->BusEnable(false) ;
00162             reset_watch_dog() ;
00163             break ;
00164         case DISPLAY_MODE_CHART:
00165             reset_watch_dog() ;
00166             x = p->left + p->index + 1 ;
00167             r = color_v2y(_value[0], p) ;
00168             g = color_v2y(_value[1], p) ;
00169             b = color_v2y(_value[2], p) ;
00170             display->BusEnable(true) ;
00171             if (p->index == 0) {
00172                 draw_chart_frame(p) ;
00173             }
00174             display->pixel(x, r, Red) ;
00175             display->pixel(x, g, Green) ;
00176             display->pixel(x, b, Blue) ;
00177             display->BusEnable(false) ;
00178             p->index = (p->index + 1) % (p->width - 2) ;
00179             break ;       
00180         }
00181     }   
00182     reset_watch_dog() ;
00183 }
00184 
00185 int edge_color::getRGB(uint16_t v[])
00186 {
00187     int result ;
00188     result = _sensor->setCOLORConf(_sensor_config) ;
00189     if (result == 0) {
00190         wait_ms(sensor_delay[(_sensor_config >> 4)&0x07] * 1.25) ;
00191      
00192         result = _sensor->getRData(&v[0]) ;
00193         if (result == 0) {
00194             wait_ms(10) ;
00195             result = _sensor->getGData(&v[1]) ;
00196             if (result == 0) {
00197                 wait_ms(10) ;
00198                 result = _sensor->getBData(&v[2]) ;
00199                 if (result == 0) {
00200                     wait_ms(10) ;
00201                 }
00202             }
00203         }
00204     }
00205     return( result ) ;
00206 }
00207 
00208 /**
00209  * Measure num_ave + 2 times
00210  * and throw away min and max
00211  * before calculating average
00212  */ 
00213 void edge_color::getAveColor(uint16_t led[], uint16_t v[], int num_ave)
00214 {
00215     int i, c ;
00216     uint16_t min[3] = { 0, 0, 0 } ;
00217     uint16_t max[3] = { 0, 0, 0 } ;
00218     uint16_t tmp[3] ;
00219     long sum[3] = { 0, 0, 0 } ;
00220         
00221 reset_watch_dog() ;
00222     setLEDs(led) ;
00223     getRGB(tmp) ; // dummy read
00224     setLEDs(0, 0, 0) ;
00225     wait_ms(10) ;
00226     for (i = 0 ; i < num_ave+2 ; i++ ) {
00227 reset_watch_dog() ;
00228         setLEDs(led) ;
00229         getRGB(tmp) ;
00230         setLEDs(0, 0, 0) ;
00231         wait_ms(10) ;
00232         for (c = 0 ; c < 3 ; c++ ) {
00233             sum[c] += tmp[c] ;
00234             if ((i == 0) || (tmp[c] < min[c])) {
00235                 min[c] = tmp[c] ;
00236             }
00237             if ((i == 0) || (tmp[c] > max[c])) {
00238                 max[c] = tmp[c] ;
00239             }
00240         }
00241     }    
00242 reset_watch_dog() ;
00243     for (c = 0 ; c < 3 ; c++ ) {
00244         sum[c] = sum[c] - (min[c] + max[c]) ;
00245         v[c] = (uint16_t)(sum[c] / num_ave) ;
00246     }
00247 //    delete [] tmp ;
00248 // printf("=== average ===\n") ;
00249 // printf("%04x %04x %04x\n", v[0], v[1], v[2]) ;
00250 }
00251 
00252 #if 1
00253 void edge_color::calibrate(uint16_t target[], uint16_t result[], int num_ave) 
00254 {
00255 //   const uint16_t led_interval = 10 ; /* wait 10ms for LED */
00256     float      denominator ;
00257     float      numerator[3] ;
00258     float      a,b,c,d,e,f,g,h,i ;
00259     uint16_t  v[3], tmp[3] ;
00260     uint16_t  L[3][3] ;
00261     int       idx ;
00262     uint8_t   conf ;
00263 
00264     printf("=== Calibrating Color Sensor %d ===\n", _id) ;
00265     for (idx = 0 ; idx < 3 ; idx++ ) {
00266 reset_watch_dog() ;
00267         tmp[0] = tmp[1] = tmp[2] = 0 ;
00268         tmp[idx] = _probe ;
00269 
00270 //        setLEDs(tmp) ;
00271 //        wait_ms(led_interval) ;
00272         getAveColor(tmp, v, num_ave) ;
00273         
00274         printf("R:%5d, G:%5d, B:%5d\n", v[0], v[1], v[2]) ;
00275         L[idx][0] = v[0] ;
00276         L[idx][1] = v[1] ;
00277         L[idx][2] = v[2] ;
00278 //        setLEDs(0, 0, 0) ; /* clear LEDs */
00279     }
00280     
00281 reset_watch_dog() ;
00282     printf("=== Initial Equation ===\n") ;
00283     for (idx = 0 ; idx < 3 ; idx++) {
00284         printf("%5d * R / %d + %5d * G / %d + %5d * B / %d = %d,\n",
00285         L[0][idx], _probe, L[1][idx], _probe,  L[2][idx], _probe, target[idx]) ;
00286     }
00287 
00288      a = L[0][0] ; b = L[1][0] ; c = L[2][0] ;
00289      d = L[0][1] ; e = L[1][1] ; f = L[2][1] ;
00290      g = L[0][2] ; h = L[1][2] ; i = L[2][2] ;
00291      
00292     denominator = a * (f * h - e * i) + b * (d * i - f * g) + c * (e * g - d * h) ;
00293 //    printf("Denominator = %f\n", denominator) ;
00294 
00295     if (denominator != 0) {
00296         numerator[0] = (f * h - e * i) * target[0] 
00297             + b * (i * target[1] - f * target[2]) 
00298             + c * (e * target[2] - h * target[1]) ;
00299 
00300         numerator[1] = -((f * g - d * i) * target[0]
00301             + a * (i * target[1] - f * target[2]) 
00302             + c * (d * target[2] - g * target[1])) ;
00303 
00304         numerator[2] = (e * g - d * h) * target[0]
00305             + a * (h * target[1] - e * target[2]) 
00306             + b * (d * target[2] - g * target[1]) ;
00307 
00308         for (idx = 0 ; idx < 3 ; idx++ ) {
00309 // printf("Numerator[%d] = %f\n", idx, numerator[idx]) ;
00310             _pwm[idx] = (uint16_t) (0.5 + (((double)_probe * numerator[idx]) / denominator)) ;
00311             result[idx] = _pwm[idx] ;
00312         }
00313 
00314         printf("PWM R = %d [0x%04x] ", result[0], result[0]) ; 
00315         wait_ms(1) ;
00316         printf("G = %d [0x%04x] ", result[1], result[1]) ;
00317         wait_ms(1) ;
00318         printf("B = %d [0x%04x] ", result[2], result[2]) ;
00319         wait_ms(1) ;
00320         printf("\n") ;
00321         wait_ms(1) ;
00322         printf("=== test ===\n") ;
00323 //        setLEDs(_pwm[0], _pwm[1], _pwm[2]) ;
00324 //        wait_ms(led_interval) ;
00325         getAveColor(_pwm, v, num_ave) ;
00326         printf("R:%d, G:%d, B:%d\n", v[0], v[1], v[2]) ;
00327         printf("============\n") ;
00328         wait_ms(1) ;
00329     } else {
00330         printf("calibration failed, pwm values were not updated\n") ;
00331     }
00332     printf("Reseting Color Sensor ... ") ;
00333 reset_watch_dog() ;
00334     _sensor->getCOLORConf(&conf) ;
00335     wait_ms(10) ;
00336     _sensor->setCOLORConf(conf | 0x01) ; /* shutdown VEML6040 */
00337     wait_ms(200) ;
00338 reset_watch_dog() ;
00339     _sensor->setCOLORConf(conf) ;
00340     wait_ms(200) ;
00341     printf("Done\n") ;
00342     _calibration_request = 0 ;
00343     _status = EDGE_SENSOR_INACTIVE ;
00344 reset_watch_dog() ;
00345 }
00346 #endif /* calibration int version */
00347 
00348 #if 0
00349 void edge_color::calibrate(uint16_t target[], uint16_t result[], int num_ave) 
00350 {
00351    const uint16_t led_interval = 10 ; /* wait 10ms for LED */
00352     double      denominator ;
00353     double      numerator[3] ;
00354     double      a,b,c,d,e,f,g,h,i ;
00355     uint16_t    v[3], tmp[3] ;
00356 //    uint16_t    L[3][3] ;
00357     double    L[3][3] ;
00358     double      ftarget[3] ;
00359     int         idx ;
00360     uint8_t     conf ;
00361     
00362     ftarget[0] = target[0] ;
00363     ftarget[1] = target[1] ;
00364     ftarget[2] = target[2] ;
00365     printf("=== Calibrating Color Sensor %d ===\n", _id) ;
00366     for (idx = 0 ; idx < 3 ; idx++ ) {
00367 reset_watch_dog() ;
00368         tmp[0] = tmp[1] = tmp[2] = 0 ;
00369         tmp[idx] = _probe ;
00370 
00371         setLEDs(tmp) ;
00372         wait_ms(led_interval) ;
00373         getAveColor(v, num_ave) ;
00374         
00375         printf("R:%5d, G:%5d, B:%5d\n", v[0], v[1], v[2]) ;
00376         L[idx][0] = v[0] ;
00377         L[idx][1] = v[1] ;
00378         L[idx][2] = v[2] ;
00379         setLEDs(0, 0, 0) ; /* clear LEDs */
00380     }
00381     
00382 reset_watch_dog() ;
00383     printf("=== Initial Equation ===\n") ;
00384     for (idx = 0 ; idx < 3 ; idx++) {
00385         printf("%5d * R / %d + %5d * G / %d + %5d * B / %d = %d,\n",
00386         (int)L[0][idx], _probe, (int)L[1][idx], _probe,  (int)L[2][idx], _probe, target[idx]) ;
00387     }
00388 
00389      a = L[0][0] ; b = L[1][0] ; c = L[2][0] ;
00390      d = L[0][1] ; e = L[1][1] ; f = L[2][1] ;
00391      g = L[0][2] ; h = L[1][2] ; i = L[2][2] ;
00392      
00393     denominator = a * (f * h - e * i) + b * (d * i - f * g) + c * (e * g - d * h) ;
00394 
00395     if (denominator != 0) {
00396         numerator[0] = (f * h - e * i) * ftarget[0] 
00397             + b * (i * ftarget[1] - f * ftarget[2]) 
00398             + c * (e * ftarget[2] - h * ftarget[1]) ;
00399 
00400         numerator[1] = -((f * g - d * i) * ftarget[0]
00401             + a * (i * ftarget[1] - f * ftarget[2]) 
00402             + c * (d * ftarget[2] - g * ftarget[1])) ;
00403 
00404         numerator[2] = (e * g - d * h) * ftarget[0]
00405             + a * (h * ftarget[1] - e * ftarget[2]) 
00406             + b * (d * ftarget[2] - g * ftarget[1]) ;
00407 
00408         for (idx = 0 ; idx < 3 ; idx++ ) {
00409             _pwm[idx] = (uint16_t) (0.5 + ((double)_probe * numerator[idx]) / denominator) ;
00410             result[idx] = _pwm[idx] ;
00411         }
00412 
00413         printf("PWM R = %d [0x%04x] ", result[0], result[0]) ; 
00414         wait_ms(1) ;
00415         printf("G = %d [0x%04x] ", result[1], result[1]) ;
00416         wait_ms(1) ;
00417         printf("B = %d [0x%04x] ", result[2], result[2]) ;
00418         wait_ms(1) ;
00419         printf("\n") ;
00420         wait_ms(1) ;
00421         printf("=== test ===\n") ;
00422         setLEDs(_pwm[0], _pwm[1], _pwm[2]) ;
00423         wait_ms(led_interval) ;
00424         getAveColor(v, num_ave) ;
00425         printf("R:%d, G:%d, B:%d\n", v[0], v[1], v[2]) ;
00426         printf("============\n") ;
00427         wait_ms(1) ;
00428     } else {
00429         printf("calibration failed, pwm values were not updated\n") ;
00430     }
00431 reset_watch_dog() ;
00432     _sensor->getCOLORConf(&conf) ;
00433     wait_ms(10) ;
00434     _sensor->setCOLORConf(conf | 0x01) ; /* shutdown VEML6040 */
00435     wait_ms(200) ;
00436 reset_watch_dog() ;
00437     _sensor->setCOLORConf(conf) ;
00438     wait_ms(200) ;
00439     _calibration_request = 0 ;
00440     _status = EDGE_SENSOR_INACTIVE ;
00441 reset_watch_dog() ;
00442 }
00443 #endif /* calibration double version */