edge_color.cpp

00001 #include "mbed.h"
00002 #include "edge_sensor.h"
00003 #include "VEML6040.h"
00004 #include "edge_color.h"
00005 
00006 /* VEML6040 config bits */
00007 /* sensor config loser 4bit */
00008 /* trigger mode etc. */
00009 #define SD_BIT   0x01
00010 #define AF_BIT   0x02
00011 #define TRIG_BIT 0x04
00012 
00013 /* sensor config upper 4bit */
00014 /* integration time */
00015  int sensor_delay[] = { 
00016     40,
00017     80,
00018     160,
00019     320,
00020     640,
00021     1280, 
00022     1280, /* place holder */ 
00023     1280  /* place holder */
00024 } ;
00025 uint16_t        color0_pwm[3] ;
00026 uint16_t        color1_pwm[3] ;
00027 uint16_t        color0_target[3] = { 3000, 3000, 3000 } ;
00028 uint16_t        color1_target[3] = { 3000, 3000, 3000 } ;
00029 
00030 edge_color::edge_color(VEML6040 *sensor, PwmOut *led[], uint16_t *pwm) : edge_sensor() 
00031 {
00032     uint16_t dummy[3] ;
00033     _sensor = sensor ;
00034     _sensor_config = AF_BIT | TRIG_BIT ;
00035     _interval = 30 ;
00036 
00037     _pwm_period = 2000 ; /* 2ms */
00038     _probe = 0xFA00 ; /* to avoid satulation at 255, using 250 */
00039 //    _probe = 0xFF00 ; 
00040     for (int i = 0 ; i < 3 ; i++ ) {
00041         _led[i] = led[i] ;
00042         _led[i]->write(1.0) ; /* turn LED off */
00043         _value[i] = 0 ;
00044         _pwm[i] = pwm[i] ;
00045         _led[i]->period_us(_pwm_period) ;
00046     }
00047     getRGB(dummy) ; // dummy read, the first data is usually garbage
00048 }
00049 
00050 edge_color::~edge_color(void) 
00051 {
00052     delete _sensor ;
00053     delete [] _led ;
00054 }
00055 
00056 void edge_color::setLEDs(uint16_t led_value[]) 
00057 {
00058     for (int i = 0 ; i < 3 ; i++ ) {
00059         _led[i]->write((float)(65535 - led_value[i])/65535.0) ;
00060     }
00061 }
00062 
00063 void edge_color::setLEDs(uint16_t r, uint16_t g, uint16_t b) 
00064 {
00065     _led[0]->write((float)(65535 - r)/65535.0) ;
00066     _led[1]->write((float)(65535 - g)/65535.0) ;
00067     _led[2]->write((float)(65535 - b)/65535.0) ;
00068 }
00069 
00070 void edge_color::reset(void) 
00071 {
00072     for (int i = 0 ; i < 3 ; i++ ) {
00073         _value[i] = 0 ;
00074     }
00075 }
00076 
00077 void edge_color::prepare(void) 
00078 {
00079 //    setLEDs(_pwm) ; // <- the other color sensor turns off (;_;)
00080 }
00081 
00082 void edge_color::sample(void) 
00083 {
00084     setLEDs(_pwm) ;
00085     getRGB(_value) ;
00086 #if 0
00087     _sensor->setCOLORConf( _sensor_config ) ;
00088     wait_ms(sensor_delay[(_sensor_config >> 4)&0x07]*1.25) ;
00089     _sensor->getRData(&_value[0]) ;
00090     _sensor->getGData(&_value[1]) ;
00091     _sensor->getBData(&_value[2]) ;
00092 #endif
00093     _sampled_time = edge_time ;
00094     setLEDs(0, 0, 0) ; /* turn LEDs off */
00095 }
00096 
00097 int edge_color::deliver(void) 
00098 {
00099     int result ;
00100     char timestr[16] ;
00101     print_time(_sampled_time) ;
00102     time2seq(_sampled_time, timestr) ;
00103     printf(" color%d : R = %4d, G = %4d, B = %4d\n",
00104         _id, _value[0], _value[1], _value[2]) ;
00105     if (_id == 1) { /* color1 */
00106     sprintf(_str_buf,
00107           "{\"DEVICE\":\"COLOR\",\"PN\":\"VEML6040\",\"VAL_R\":\"%d\",\"VAL_G\":\"%d\",\"VAL_B\":\"%d\",\"UNIT\":\"mW/cm2\",\"S\":\"%s\",\"E\":\"%d\"}",
00108           _value[0], _value[1], _value[2], timestr, _error_count) ; 
00109     } else { /* color2 */
00110         sprintf(_str_buf,
00111           "{\"DEVICE\":\"COLOR02\",\"PN\":\"VEML6040\",\"VAL_R\":\"%d\",\"VAL_G\":\"%d\",\"VAL_B\":\"%d\",\"UNIT\":\"mW/cm2\",\"S\":\"%s\",\"E\":\"%d\"}",
00112           _value[0], _value[1], _value[2], timestr, _error_count) ; 
00113     }
00114     result = afero->setAttribute(1, _str_buf) ;
00115     return( result == afSUCCESS ) ;
00116 }
00117 
00118 void edge_color::getRGB(uint16_t v[])
00119 {
00120     _sensor->setCOLORConf(_sensor_config) ;
00121 
00122     wait_ms(sensor_delay[(_sensor_config >> 4)&0x07] * 1.25) ;
00123     
00124     _sensor->getRData(&v[0]) ;
00125     _sensor->getGData(&v[1]) ;
00126     _sensor->getBData(&v[2]) ;
00127 }
00128 
00129 /**
00130  * Measure num_ave + 2 times
00131  * and throw away min and max
00132  * before calculating average
00133  */ 
00134 void edge_color::getAveColor(uint16_t v[], int num_ave)
00135 {
00136     int i, c ;
00137     long l[3] = {0, 0, 0} ;
00138     uint16_t min[3] = { 0, 0, 0 } ;
00139     uint16_t max[3] = { 0, 0, 0 } ;
00140     uint16_t *tmp ;
00141     
00142     tmp = new uint16_t[(num_ave+2)*3] ;
00143     
00144     getRGB(&tmp[0]) ; // dummy read
00145     wait(0.1) ;
00146     for (i = 0 ; i < num_ave+2 ; i++ ) {
00147         getRGB(&tmp[i*3]) ;
00148 // printf("%04x %04x %04x\n", tmp[i].r, tmp[i].g, tmp[i].b) ;
00149         for (c = 0 ; c < 3 ; c++ ) { /* c = r, g, b */
00150             if (tmp[i*3+c] < tmp[min[c]]) {
00151                 min[c] = i ;
00152             }
00153             if (tmp[i*3+c] > tmp[max[c]]) {
00154                 max[c] = i ;
00155             }
00156         }
00157     }
00158     for (i = 0 ; i < num_ave+2 ; i++ ) {
00159         for (c = 0 ; c < 3 ; c++ ) {
00160             if ((min[c] != i)&&(max[c] != i)) {
00161                 l[c] += tmp[i*3+c] ;
00162             }
00163         }
00164     }
00165 
00166     delete [] tmp ;
00167     for (c = 0 ; c < 3 ; c++ ) {
00168         v[c] = (uint16_t)(l[c] / num_ave) ;
00169     }
00170 // printf("=== average ===\n") ;
00171 // printf("%04x %04x %04x\n", v[0], v[1], v[2]) ;
00172 }
00173 
00174 void edge_color::calibrate(uint16_t target[], uint16_t result[], int num_ave) 
00175 {
00176    const uint16_t led_interval = 10 ; /* wait 10ms for LED */
00177     double      denominator ;
00178     double      numerator[3] ;
00179     double      a,b,c,d,e,f,g,h,i ;
00180     uint16_t    v[3], tmp[3] ;
00181 //    uint16_t    L[3][3] ;
00182     double    L[3][3] ;
00183     double      ftarget[3] ;
00184     int         idx ;
00185     
00186     ftarget[0] = target[0] ;
00187     ftarget[1] = target[1] ;
00188     ftarget[2] = target[2] ;
00189     printf("=== Calibrating Color Sensor %d ===\n", _id) ;
00190     for (idx = 0 ; idx < 3 ; idx++ ) {
00191         tmp[0] = tmp[1] = tmp[2] = 0 ;
00192         tmp[idx] = _probe ;
00193 
00194         setLEDs(tmp) ;
00195         wait_ms(led_interval) ;
00196         getAveColor(v, num_ave) ;
00197         
00198         printf("R:%5d, G:%5d, B:%5d\n", v[0], v[1], v[2]) ;
00199         L[idx][0] = v[0] ;
00200         L[idx][1] = v[1] ;
00201         L[idx][2] = v[2] ;
00202         setLEDs(0, 0, 0) ; /* clear LEDs */
00203     }
00204     
00205     printf("=== Initial Equation ===\n") ;
00206     for (idx = 0 ; idx < 3 ; idx++) {
00207         printf("%5d * R / %d + %5d * G / %d + %5d * B / %d = %d,\n",
00208         (int)L[0][idx], _probe, (int)L[1][idx], _probe,  (int)L[2][idx], _probe, target[idx]) ;
00209     }
00210 
00211      a = L[0][0] ; b = L[1][0] ; c = L[2][0] ;
00212      d = L[0][1] ; e = L[1][1] ; f = L[2][1] ;
00213      g = L[0][2] ; h = L[1][2] ; i = L[2][2] ;
00214      
00215     denominator = a * (f * h - e * i) + b * (d * i - f * g) + c * (e * g - d * h) ;
00216 
00217     if (denominator != 0) {
00218         numerator[0] = (f * h - e * i) * ftarget[0] 
00219             + b * (i * ftarget[1] - f * ftarget[2]) 
00220             + c * (e * ftarget[2] - h * ftarget[1]) ;
00221 
00222         numerator[1] = -((f * g - d * i) * ftarget[0]
00223             + a * (i * ftarget[1] - f * ftarget[2]) 
00224             + c * (d * ftarget[2] - g * ftarget[1])) ;
00225 
00226         numerator[2] = (e * g - d * h) * ftarget[0]
00227             + a * (h * ftarget[1] - e * ftarget[2]) 
00228             + b * (d * ftarget[2] - g * ftarget[1]) ;
00229 
00230         for (idx = 0 ; idx < 3 ; idx++ ) {
00231             _pwm[idx] = (uint16_t) (0.5 + ((double)_probe * numerator[idx]) / denominator) ;
00232             result[idx] = _pwm[idx] ;
00233         }
00234 
00235         printf("PWM R = %d [0x%04x] ", result[0], result[0]) ; 
00236         printf("G = %d [0x%04x] ", result[1], result[1]) ;
00237         printf("B = %d [0x%04x] ", result[2], result[2]) ;
00238         printf("\n") ;
00239         printf("=== test ===\n") ;
00240         setLEDs(_pwm[0], _pwm[1], _pwm[2]) ;
00241         wait_ms(led_interval) ;
00242         getAveColor(v, num_ave) ;
00243         printf("R:%d, G:%d, B:%d\n", v[0], v[1], v[2]) ;
00244         printf("============\n") ;
00245     } else {
00246         printf("calibration failed, pwm values were not updated\n") ;
00247     }
00248 }