La Suno / Mbed 2 deprecated afero_poc15_180413

Added a GPIO to power on/off for external I2C sensor(s) (with LEDs)

Dependencies:   UniGraphic mbed vt100

18-Jun-2018 外部センサの電源オン・オフ機能は下位互換の為に無効になっていました。 この版で再度有効にしました。

edge_sensor/edge_color.cpp@0:846e2321c637, 2018-04-13 (annotated)

Committer:
Rhyme
Date:
Fri Apr 13 04:19:23 2018 +0000
Revision:
0:846e2321c637
power to color sensor on/off test OK. Currently the function is disabled.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Rhyme 0:846e2321c637 1 #include "mbed.h"
Rhyme 0:846e2321c637 2 #include "edge_sensor.h"
Rhyme 0:846e2321c637 3 #include "VEML6040.h"
Rhyme 0:846e2321c637 4 #include "edge_color.h"
Rhyme 0:846e2321c637 5 #include "edge_reset_mgr.h"
Rhyme 0:846e2321c637 6 #include "edge_chart.h"
Rhyme 0:846e2321c637 7
Rhyme 0:846e2321c637 8 /* VEML6040 config bits */
Rhyme 0:846e2321c637 9 /* sensor config loser 4bit */
Rhyme 0:846e2321c637 10 /* trigger mode etc. */
Rhyme 0:846e2321c637 11 #define SD_BIT 0x01
Rhyme 0:846e2321c637 12 #define AF_BIT 0x02
Rhyme 0:846e2321c637 13 #define TRIG_BIT 0x04
Rhyme 0:846e2321c637 14
Rhyme 0:846e2321c637 15 /* sensor config upper 4bit */
Rhyme 0:846e2321c637 16 /* integration time */
Rhyme 0:846e2321c637 17 int sensor_delay[] = {
Rhyme 0:846e2321c637 18 40,
Rhyme 0:846e2321c637 19 80,
Rhyme 0:846e2321c637 20 160,
Rhyme 0:846e2321c637 21 320,
Rhyme 0:846e2321c637 22 640,
Rhyme 0:846e2321c637 23 1280,
Rhyme 0:846e2321c637 24 1280, /* place holder */
Rhyme 0:846e2321c637 25 1280 /* place holder */
Rhyme 0:846e2321c637 26 } ;
Rhyme 0:846e2321c637 27
Rhyme 0:846e2321c637 28 uint16_t color0_pwm[3] ;
Rhyme 0:846e2321c637 29 uint16_t color1_pwm[3] ;
Rhyme 0:846e2321c637 30 uint16_t color0_target[3] = { 3500, 3500, 3500 } ;
Rhyme 0:846e2321c637 31 uint16_t color1_target[3] = { 3500, 3500, 3500 } ;
Rhyme 0:846e2321c637 32
Rhyme 0:846e2321c637 33
Rhyme 0:846e2321c637 34 edge_color::edge_color(VEML6040 *sensor, PwmOut *led[], uint16_t *pwm) : edge_sensor()
Rhyme 0:846e2321c637 35 {
Rhyme 0:846e2321c637 36 uint16_t dummy[3] ;
Rhyme 0:846e2321c637 37 _sensor = sensor ;
Rhyme 0:846e2321c637 38 _sensor_config = AF_BIT | TRIG_BIT ;
Rhyme 0:846e2321c637 39 _interval = 30 ;
Rhyme 0:846e2321c637 40 _calibration_request = 0 ; /* 1 for testing */
Rhyme 0:846e2321c637 41
Rhyme 0:846e2321c637 42 reset_watch_dog() ;
Rhyme 0:846e2321c637 43 _pwm_period = 2000 ; /* 2ms */
Rhyme 0:846e2321c637 44 _probe = 0xFA00 ; /* to avoid satulation at 255, using 250 */
Rhyme 0:846e2321c637 45 // _probe = 0xFF00 ;
Rhyme 0:846e2321c637 46 for (int i = 0 ; i < 3 ; i++ ) {
Rhyme 0:846e2321c637 47 _led[i] = led[i] ;
Rhyme 0:846e2321c637 48 _led[i]->write(1.0) ; /* turn LED off */
Rhyme 0:846e2321c637 49 _value[i] = 0 ;
Rhyme 0:846e2321c637 50 _pwm[i] = pwm[i] ;
Rhyme 0:846e2321c637 51 _led[i]->period_us(_pwm_period) ;
Rhyme 0:846e2321c637 52 }
Rhyme 0:846e2321c637 53 getRGB(dummy) ; // dummy read, the first data is usually garbage
Rhyme 0:846e2321c637 54 reset_watch_dog() ;
Rhyme 0:846e2321c637 55 }
Rhyme 0:846e2321c637 56
Rhyme 0:846e2321c637 57 edge_color::~edge_color(void)
Rhyme 0:846e2321c637 58 {
Rhyme 0:846e2321c637 59 delete _sensor ;
Rhyme 0:846e2321c637 60 delete [] _led ;
Rhyme 0:846e2321c637 61 }
Rhyme 0:846e2321c637 62
Rhyme 0:846e2321c637 63 void edge_color::setLEDs(uint16_t led_value[])
Rhyme 0:846e2321c637 64 {
Rhyme 0:846e2321c637 65 for (int i = 0 ; i < 3 ; i++ ) {
Rhyme 0:846e2321c637 66 _led[i]->write((float)(65535 - led_value[i])/65535.0) ;
Rhyme 0:846e2321c637 67 }
Rhyme 0:846e2321c637 68 }
Rhyme 0:846e2321c637 69
Rhyme 0:846e2321c637 70 void edge_color::setLEDs(uint16_t r, uint16_t g, uint16_t b)
Rhyme 0:846e2321c637 71 {
Rhyme 0:846e2321c637 72 _led[0]->write((float)(65535 - r)/65535.0) ;
Rhyme 0:846e2321c637 73 _led[1]->write((float)(65535 - g)/65535.0) ;
Rhyme 0:846e2321c637 74 _led[2]->write((float)(65535 - b)/65535.0) ;
Rhyme 0:846e2321c637 75 }
Rhyme 0:846e2321c637 76
Rhyme 0:846e2321c637 77 void edge_color::reset(void)
Rhyme 0:846e2321c637 78 {
Rhyme 0:846e2321c637 79 for (int i = 0 ; i < 3 ; i++ ) {
Rhyme 0:846e2321c637 80 _value[i] = 0 ;
Rhyme 0:846e2321c637 81 }
Rhyme 0:846e2321c637 82 }
Rhyme 0:846e2321c637 83
Rhyme 0:846e2321c637 84 void edge_color::prepare(void)
Rhyme 0:846e2321c637 85 {
Rhyme 0:846e2321c637 86 // setLEDs(_pwm) ; // <- the other color sensor turns off (;_;)
Rhyme 0:846e2321c637 87 }
Rhyme 0:846e2321c637 88
Rhyme 0:846e2321c637 89 int edge_color::sample(void)
Rhyme 0:846e2321c637 90 {
Rhyme 0:846e2321c637 91 int result ;
Rhyme 0:846e2321c637 92 reset_watch_dog() ;
Rhyme 0:846e2321c637 93 setLEDs(_pwm) ;
Rhyme 0:846e2321c637 94 reset_watch_dog() ;
Rhyme 0:846e2321c637 95 result = getRGB(_value) ;
Rhyme 0:846e2321c637 96 _sampled_time = edge_time ;
Rhyme 0:846e2321c637 97 setLEDs(0, 0, 0) ; /* turn LEDs off */
Rhyme 0:846e2321c637 98 reset_watch_dog() ;
Rhyme 0:846e2321c637 99 return( result ) ;
Rhyme 0:846e2321c637 100 }
Rhyme 0:846e2321c637 101
Rhyme 0:846e2321c637 102 int edge_color::deliver(void)
Rhyme 0:846e2321c637 103 {
Rhyme 0:846e2321c637 104 int result ;
Rhyme 0:846e2321c637 105 char timestr[16] ;
Rhyme 0:846e2321c637 106 print_time(_sampled_time) ;
Rhyme 0:846e2321c637 107 time2seq(_sampled_time, timestr) ;
Rhyme 0:846e2321c637 108 printf(" color%d : R = %4d, G = %4d, B = %4d\n",
Rhyme 0:846e2321c637 109 _id, _value[0], _value[1], _value[2]) ;
Rhyme 0:846e2321c637 110 if (_id == 1) { /* color1 */
Rhyme 0:846e2321c637 111 sprintf(_str_buf,
Rhyme 0:846e2321c637 112 "{\"DEVICE\":\"COLOR\",\"PN\":\"VEML6040\",\"VAL_R\":\"%d\",\"VAL_G\":\"%d\",\"VAL_B\":\"%d\",\"UNIT\":\"mW/cm2\",\"T\":\"%s\",\"E\":\"%d\"}",
Rhyme 0:846e2321c637 113 _value[0], _value[1], _value[2], timestr, _error_count) ;
Rhyme 0:846e2321c637 114 } else { /* color2 */
Rhyme 0:846e2321c637 115 sprintf(_str_buf,
Rhyme 0:846e2321c637 116 "{\"DEVICE\":\"COLOR02\",\"PN\":\"VEML6040\",\"VAL_R\":\"%d\",\"VAL_G\":\"%d\",\"VAL_B\":\"%d\",\"UNIT\":\"mW/cm2\",\"T\":\"%s\",\"E\":\"%d\"}",
Rhyme 0:846e2321c637 117 _value[0], _value[1], _value[2], timestr, _error_count) ;
Rhyme 0:846e2321c637 118 }
Rhyme 0:846e2321c637 119 result = afero->setAttribute(1, _str_buf) ;
Rhyme 0:846e2321c637 120
Rhyme 0:846e2321c637 121 return( result == afSUCCESS ) ;
Rhyme 0:846e2321c637 122 }
Rhyme 0:846e2321c637 123
Rhyme 0:846e2321c637 124 int color_v2y(float value, edge_chart_type *p)
Rhyme 0:846e2321c637 125 {
Rhyme 0:846e2321c637 126 int y ;
Rhyme 0:846e2321c637 127 if (value < p->min) {
Rhyme 0:846e2321c637 128 value = p->min ;
Rhyme 0:846e2321c637 129 } else if (value > p->max) {
Rhyme 0:846e2321c637 130 value = p->max ;
Rhyme 0:846e2321c637 131 }
Rhyme 0:846e2321c637 132 y = p->top + p->height - 1
Rhyme 0:846e2321c637 133 - (int)((p->height - 2) * value /(p->max - p->min)) ;
Rhyme 0:846e2321c637 134 return( y ) ;
Rhyme 0:846e2321c637 135 }
Rhyme 0:846e2321c637 136
Rhyme 0:846e2321c637 137 void edge_color::show(void)
Rhyme 0:846e2321c637 138 {
Rhyme 0:846e2321c637 139 int r, g, b ;
Rhyme 0:846e2321c637 140 int x ;
Rhyme 0:846e2321c637 141 edge_chart_type *p = &edge_chart[_id] ;
Rhyme 0:846e2321c637 142 if (display) {
Rhyme 0:846e2321c637 143 switch(display_mode) {
Rhyme 0:846e2321c637 144 case DISPLAY_MODE_SUMMARY:
Rhyme 0:846e2321c637 145 reset_watch_dog() ;
Rhyme 0:846e2321c637 146 display->BusEnable(true) ;
Rhyme 0:846e2321c637 147 display->set_font((unsigned char*) Arial12x12);
Rhyme 0:846e2321c637 148 display->set_font_zoom(2, 2) ;
Rhyme 0:846e2321c637 149 display->foreground(White) ;
Rhyme 0:846e2321c637 150 display->locate(EDGE_SUMMARY_X, EDGE_SUMMARY_TIME_Y) ;
Rhyme 0:846e2321c637 151 displayTime(_sampled_time) ;
Rhyme 0:846e2321c637 152 if (_id == 1) {
Rhyme 0:846e2321c637 153 display->locate(EDGE_SUMMARY_X, EDGE_SUMMARY_COLOR1_Y) ;
Rhyme 0:846e2321c637 154 display->printf("Color :%5d,%5d,%5d",
Rhyme 0:846e2321c637 155 _value[0], _value[1], _value[2]) ;
Rhyme 0:846e2321c637 156 } else {
Rhyme 0:846e2321c637 157 display->locate(EDGE_SUMMARY_X, EDGE_SUMMARY_COLOR2_Y) ;
Rhyme 0:846e2321c637 158 display->printf("Color2:%5d,%5d,%5d",
Rhyme 0:846e2321c637 159 _value[0], _value[1], _value[2]) ;
Rhyme 0:846e2321c637 160 }
Rhyme 0:846e2321c637 161 display->BusEnable(false) ;
Rhyme 0:846e2321c637 162 reset_watch_dog() ;
Rhyme 0:846e2321c637 163 break ;
Rhyme 0:846e2321c637 164 case DISPLAY_MODE_CHART:
Rhyme 0:846e2321c637 165 reset_watch_dog() ;
Rhyme 0:846e2321c637 166 x = p->left + p->index + 1 ;
Rhyme 0:846e2321c637 167 r = color_v2y(_value[0], p) ;
Rhyme 0:846e2321c637 168 g = color_v2y(_value[1], p) ;
Rhyme 0:846e2321c637 169 b = color_v2y(_value[2], p) ;
Rhyme 0:846e2321c637 170 display->BusEnable(true) ;
Rhyme 0:846e2321c637 171 if (p->index == 0) {
Rhyme 0:846e2321c637 172 draw_chart_frame(p) ;
Rhyme 0:846e2321c637 173 }
Rhyme 0:846e2321c637 174 display->pixel(x, r, Red) ;
Rhyme 0:846e2321c637 175 display->pixel(x, g, Green) ;
Rhyme 0:846e2321c637 176 display->pixel(x, b, Blue) ;
Rhyme 0:846e2321c637 177 display->BusEnable(false) ;
Rhyme 0:846e2321c637 178 p->index = (p->index + 1) % (p->width - 2) ;
Rhyme 0:846e2321c637 179 break ;
Rhyme 0:846e2321c637 180 }
Rhyme 0:846e2321c637 181 }
Rhyme 0:846e2321c637 182 reset_watch_dog() ;
Rhyme 0:846e2321c637 183 }
Rhyme 0:846e2321c637 184
Rhyme 0:846e2321c637 185 int edge_color::getRGB(uint16_t v[])
Rhyme 0:846e2321c637 186 {
Rhyme 0:846e2321c637 187 int result ;
Rhyme 0:846e2321c637 188 result = _sensor->setCOLORConf(_sensor_config) ;
Rhyme 0:846e2321c637 189 if (result == 0) {
Rhyme 0:846e2321c637 190 wait_ms(sensor_delay[(_sensor_config >> 4)&0x07] * 1.25) ;
Rhyme 0:846e2321c637 191
Rhyme 0:846e2321c637 192 result = _sensor->getRData(&v[0]) ;
Rhyme 0:846e2321c637 193 if (result == 0) {
Rhyme 0:846e2321c637 194 wait_ms(10) ;
Rhyme 0:846e2321c637 195 result = _sensor->getGData(&v[1]) ;
Rhyme 0:846e2321c637 196 if (result == 0) {
Rhyme 0:846e2321c637 197 wait_ms(10) ;
Rhyme 0:846e2321c637 198 result = _sensor->getBData(&v[2]) ;
Rhyme 0:846e2321c637 199 if (result == 0) {
Rhyme 0:846e2321c637 200 wait_ms(10) ;
Rhyme 0:846e2321c637 201 }
Rhyme 0:846e2321c637 202 }
Rhyme 0:846e2321c637 203 }
Rhyme 0:846e2321c637 204 }
Rhyme 0:846e2321c637 205 return( result ) ;
Rhyme 0:846e2321c637 206 }
Rhyme 0:846e2321c637 207
Rhyme 0:846e2321c637 208 /**
Rhyme 0:846e2321c637 209 * Measure num_ave + 2 times
Rhyme 0:846e2321c637 210 * and throw away min and max
Rhyme 0:846e2321c637 211 * before calculating average
Rhyme 0:846e2321c637 212 */
Rhyme 0:846e2321c637 213 void edge_color::getAveColor(uint16_t led[], uint16_t v[], int num_ave)
Rhyme 0:846e2321c637 214 {
Rhyme 0:846e2321c637 215 int i, c ;
Rhyme 0:846e2321c637 216 uint16_t min[3] = { 0, 0, 0 } ;
Rhyme 0:846e2321c637 217 uint16_t max[3] = { 0, 0, 0 } ;
Rhyme 0:846e2321c637 218 uint16_t tmp[3] ;
Rhyme 0:846e2321c637 219 long sum[3] = { 0, 0, 0 } ;
Rhyme 0:846e2321c637 220
Rhyme 0:846e2321c637 221 reset_watch_dog() ;
Rhyme 0:846e2321c637 222 setLEDs(led) ;
Rhyme 0:846e2321c637 223 getRGB(tmp) ; // dummy read
Rhyme 0:846e2321c637 224 setLEDs(0, 0, 0) ;
Rhyme 0:846e2321c637 225 wait_ms(10) ;
Rhyme 0:846e2321c637 226 for (i = 0 ; i < num_ave+2 ; i++ ) {
Rhyme 0:846e2321c637 227 reset_watch_dog() ;
Rhyme 0:846e2321c637 228 setLEDs(led) ;
Rhyme 0:846e2321c637 229 getRGB(tmp) ;
Rhyme 0:846e2321c637 230 setLEDs(0, 0, 0) ;
Rhyme 0:846e2321c637 231 wait_ms(10) ;
Rhyme 0:846e2321c637 232 for (c = 0 ; c < 3 ; c++ ) {
Rhyme 0:846e2321c637 233 sum[c] += tmp[c] ;
Rhyme 0:846e2321c637 234 if ((i == 0) || (tmp[c] < min[c])) {
Rhyme 0:846e2321c637 235 min[c] = tmp[c] ;
Rhyme 0:846e2321c637 236 }
Rhyme 0:846e2321c637 237 if ((i == 0) || (tmp[c] > max[c])) {
Rhyme 0:846e2321c637 238 max[c] = tmp[c] ;
Rhyme 0:846e2321c637 239 }
Rhyme 0:846e2321c637 240 }
Rhyme 0:846e2321c637 241 }
Rhyme 0:846e2321c637 242 reset_watch_dog() ;
Rhyme 0:846e2321c637 243 for (c = 0 ; c < 3 ; c++ ) {
Rhyme 0:846e2321c637 244 sum[c] = sum[c] - (min[c] + max[c]) ;
Rhyme 0:846e2321c637 245 v[c] = (uint16_t)(sum[c] / num_ave) ;
Rhyme 0:846e2321c637 246 }
Rhyme 0:846e2321c637 247 // delete [] tmp ;
Rhyme 0:846e2321c637 248 // printf("=== average ===\n") ;
Rhyme 0:846e2321c637 249 // printf("%04x %04x %04x\n", v[0], v[1], v[2]) ;
Rhyme 0:846e2321c637 250 }
Rhyme 0:846e2321c637 251
Rhyme 0:846e2321c637 252 #if 1
Rhyme 0:846e2321c637 253 void edge_color::calibrate(uint16_t target[], uint16_t result[], int num_ave)
Rhyme 0:846e2321c637 254 {
Rhyme 0:846e2321c637 255 // const uint16_t led_interval = 10 ; /* wait 10ms for LED */
Rhyme 0:846e2321c637 256 float denominator ;
Rhyme 0:846e2321c637 257 float numerator[3] ;
Rhyme 0:846e2321c637 258 float a,b,c,d,e,f,g,h,i ;
Rhyme 0:846e2321c637 259 uint16_t v[3], tmp[3] ;
Rhyme 0:846e2321c637 260 uint16_t L[3][3] ;
Rhyme 0:846e2321c637 261 int idx ;
Rhyme 0:846e2321c637 262 uint8_t conf ;
Rhyme 0:846e2321c637 263
Rhyme 0:846e2321c637 264 printf("=== Calibrating Color Sensor %d ===\n", _id) ;
Rhyme 0:846e2321c637 265 for (idx = 0 ; idx < 3 ; idx++ ) {
Rhyme 0:846e2321c637 266 reset_watch_dog() ;
Rhyme 0:846e2321c637 267 tmp[0] = tmp[1] = tmp[2] = 0 ;
Rhyme 0:846e2321c637 268 tmp[idx] = _probe ;
Rhyme 0:846e2321c637 269
Rhyme 0:846e2321c637 270 // setLEDs(tmp) ;
Rhyme 0:846e2321c637 271 // wait_ms(led_interval) ;
Rhyme 0:846e2321c637 272 getAveColor(tmp, v, num_ave) ;
Rhyme 0:846e2321c637 273
Rhyme 0:846e2321c637 274 printf("R:%5d, G:%5d, B:%5d\n", v[0], v[1], v[2]) ;
Rhyme 0:846e2321c637 275 L[idx][0] = v[0] ;
Rhyme 0:846e2321c637 276 L[idx][1] = v[1] ;
Rhyme 0:846e2321c637 277 L[idx][2] = v[2] ;
Rhyme 0:846e2321c637 278 // setLEDs(0, 0, 0) ; /* clear LEDs */
Rhyme 0:846e2321c637 279 }
Rhyme 0:846e2321c637 280
Rhyme 0:846e2321c637 281 reset_watch_dog() ;
Rhyme 0:846e2321c637 282 printf("=== Initial Equation ===\n") ;
Rhyme 0:846e2321c637 283 for (idx = 0 ; idx < 3 ; idx++) {
Rhyme 0:846e2321c637 284 printf("%5d * R / %d + %5d * G / %d + %5d * B / %d = %d,\n",
Rhyme 0:846e2321c637 285 L[0][idx], _probe, L[1][idx], _probe, L[2][idx], _probe, target[idx]) ;
Rhyme 0:846e2321c637 286 }
Rhyme 0:846e2321c637 287
Rhyme 0:846e2321c637 288 a = L[0][0] ; b = L[1][0] ; c = L[2][0] ;
Rhyme 0:846e2321c637 289 d = L[0][1] ; e = L[1][1] ; f = L[2][1] ;
Rhyme 0:846e2321c637 290 g = L[0][2] ; h = L[1][2] ; i = L[2][2] ;
Rhyme 0:846e2321c637 291
Rhyme 0:846e2321c637 292 denominator = a * (f * h - e * i) + b * (d * i - f * g) + c * (e * g - d * h) ;
Rhyme 0:846e2321c637 293 // printf("Denominator = %f\n", denominator) ;
Rhyme 0:846e2321c637 294
Rhyme 0:846e2321c637 295 if (denominator != 0) {
Rhyme 0:846e2321c637 296 numerator[0] = (f * h - e * i) * target[0]
Rhyme 0:846e2321c637 297 + b * (i * target[1] - f * target[2])
Rhyme 0:846e2321c637 298 + c * (e * target[2] - h * target[1]) ;
Rhyme 0:846e2321c637 299
Rhyme 0:846e2321c637 300 numerator[1] = -((f * g - d * i) * target[0]
Rhyme 0:846e2321c637 301 + a * (i * target[1] - f * target[2])
Rhyme 0:846e2321c637 302 + c * (d * target[2] - g * target[1])) ;
Rhyme 0:846e2321c637 303
Rhyme 0:846e2321c637 304 numerator[2] = (e * g - d * h) * target[0]
Rhyme 0:846e2321c637 305 + a * (h * target[1] - e * target[2])
Rhyme 0:846e2321c637 306 + b * (d * target[2] - g * target[1]) ;
Rhyme 0:846e2321c637 307
Rhyme 0:846e2321c637 308 for (idx = 0 ; idx < 3 ; idx++ ) {
Rhyme 0:846e2321c637 309 // printf("Numerator[%d] = %f\n", idx, numerator[idx]) ;
Rhyme 0:846e2321c637 310 _pwm[idx] = (uint16_t) (0.5 + (((double)_probe * numerator[idx]) / denominator)) ;
Rhyme 0:846e2321c637 311 result[idx] = _pwm[idx] ;
Rhyme 0:846e2321c637 312 }
Rhyme 0:846e2321c637 313
Rhyme 0:846e2321c637 314 printf("PWM R = %d [0x%04x] ", result[0], result[0]) ;
Rhyme 0:846e2321c637 315 wait_ms(1) ;
Rhyme 0:846e2321c637 316 printf("G = %d [0x%04x] ", result[1], result[1]) ;
Rhyme 0:846e2321c637 317 wait_ms(1) ;
Rhyme 0:846e2321c637 318 printf("B = %d [0x%04x] ", result[2], result[2]) ;
Rhyme 0:846e2321c637 319 wait_ms(1) ;
Rhyme 0:846e2321c637 320 printf("\n") ;
Rhyme 0:846e2321c637 321 wait_ms(1) ;
Rhyme 0:846e2321c637 322 printf("=== test ===\n") ;
Rhyme 0:846e2321c637 323 // setLEDs(_pwm[0], _pwm[1], _pwm[2]) ;
Rhyme 0:846e2321c637 324 // wait_ms(led_interval) ;
Rhyme 0:846e2321c637 325 getAveColor(_pwm, v, num_ave) ;
Rhyme 0:846e2321c637 326 printf("R:%d, G:%d, B:%d\n", v[0], v[1], v[2]) ;
Rhyme 0:846e2321c637 327 printf("============\n") ;
Rhyme 0:846e2321c637 328 wait_ms(1) ;
Rhyme 0:846e2321c637 329 } else {
Rhyme 0:846e2321c637 330 printf("calibration failed, pwm values were not updated\n") ;
Rhyme 0:846e2321c637 331 }
Rhyme 0:846e2321c637 332 printf("Reseting Color Sensor ... ") ;
Rhyme 0:846e2321c637 333 reset_watch_dog() ;
Rhyme 0:846e2321c637 334 _sensor->getCOLORConf(&conf) ;
Rhyme 0:846e2321c637 335 wait_ms(10) ;
Rhyme 0:846e2321c637 336 _sensor->setCOLORConf(conf | 0x01) ; /* shutdown VEML6040 */
Rhyme 0:846e2321c637 337 wait_ms(200) ;
Rhyme 0:846e2321c637 338 reset_watch_dog() ;
Rhyme 0:846e2321c637 339 _sensor->setCOLORConf(conf) ;
Rhyme 0:846e2321c637 340 wait_ms(200) ;
Rhyme 0:846e2321c637 341 printf("Done\n") ;
Rhyme 0:846e2321c637 342 _calibration_request = 0 ;
Rhyme 0:846e2321c637 343 _status = EDGE_SENSOR_INACTIVE ;
Rhyme 0:846e2321c637 344 reset_watch_dog() ;
Rhyme 0:846e2321c637 345 }
Rhyme 0:846e2321c637 346 #endif /* calibration int version */
Rhyme 0:846e2321c637 347
Rhyme 0:846e2321c637 348 #if 0
Rhyme 0:846e2321c637 349 void edge_color::calibrate(uint16_t target[], uint16_t result[], int num_ave)
Rhyme 0:846e2321c637 350 {
Rhyme 0:846e2321c637 351 const uint16_t led_interval = 10 ; /* wait 10ms for LED */
Rhyme 0:846e2321c637 352 double denominator ;
Rhyme 0:846e2321c637 353 double numerator[3] ;
Rhyme 0:846e2321c637 354 double a,b,c,d,e,f,g,h,i ;
Rhyme 0:846e2321c637 355 uint16_t v[3], tmp[3] ;
Rhyme 0:846e2321c637 356 // uint16_t L[3][3] ;
Rhyme 0:846e2321c637 357 double L[3][3] ;
Rhyme 0:846e2321c637 358 double ftarget[3] ;
Rhyme 0:846e2321c637 359 int idx ;
Rhyme 0:846e2321c637 360 uint8_t conf ;
Rhyme 0:846e2321c637 361
Rhyme 0:846e2321c637 362 ftarget[0] = target[0] ;
Rhyme 0:846e2321c637 363 ftarget[1] = target[1] ;
Rhyme 0:846e2321c637 364 ftarget[2] = target[2] ;
Rhyme 0:846e2321c637 365 printf("=== Calibrating Color Sensor %d ===\n", _id) ;
Rhyme 0:846e2321c637 366 for (idx = 0 ; idx < 3 ; idx++ ) {
Rhyme 0:846e2321c637 367 reset_watch_dog() ;
Rhyme 0:846e2321c637 368 tmp[0] = tmp[1] = tmp[2] = 0 ;
Rhyme 0:846e2321c637 369 tmp[idx] = _probe ;
Rhyme 0:846e2321c637 370
Rhyme 0:846e2321c637 371 setLEDs(tmp) ;
Rhyme 0:846e2321c637 372 wait_ms(led_interval) ;
Rhyme 0:846e2321c637 373 getAveColor(v, num_ave) ;
Rhyme 0:846e2321c637 374
Rhyme 0:846e2321c637 375 printf("R:%5d, G:%5d, B:%5d\n", v[0], v[1], v[2]) ;
Rhyme 0:846e2321c637 376 L[idx][0] = v[0] ;
Rhyme 0:846e2321c637 377 L[idx][1] = v[1] ;
Rhyme 0:846e2321c637 378 L[idx][2] = v[2] ;
Rhyme 0:846e2321c637 379 setLEDs(0, 0, 0) ; /* clear LEDs */
Rhyme 0:846e2321c637 380 }
Rhyme 0:846e2321c637 381
Rhyme 0:846e2321c637 382 reset_watch_dog() ;
Rhyme 0:846e2321c637 383 printf("=== Initial Equation ===\n") ;
Rhyme 0:846e2321c637 384 for (idx = 0 ; idx < 3 ; idx++) {
Rhyme 0:846e2321c637 385 printf("%5d * R / %d + %5d * G / %d + %5d * B / %d = %d,\n",
Rhyme 0:846e2321c637 386 (int)L[0][idx], _probe, (int)L[1][idx], _probe, (int)L[2][idx], _probe, target[idx]) ;
Rhyme 0:846e2321c637 387 }
Rhyme 0:846e2321c637 388
Rhyme 0:846e2321c637 389 a = L[0][0] ; b = L[1][0] ; c = L[2][0] ;
Rhyme 0:846e2321c637 390 d = L[0][1] ; e = L[1][1] ; f = L[2][1] ;
Rhyme 0:846e2321c637 391 g = L[0][2] ; h = L[1][2] ; i = L[2][2] ;
Rhyme 0:846e2321c637 392
Rhyme 0:846e2321c637 393 denominator = a * (f * h - e * i) + b * (d * i - f * g) + c * (e * g - d * h) ;
Rhyme 0:846e2321c637 394
Rhyme 0:846e2321c637 395 if (denominator != 0) {
Rhyme 0:846e2321c637 396 numerator[0] = (f * h - e * i) * ftarget[0]
Rhyme 0:846e2321c637 397 + b * (i * ftarget[1] - f * ftarget[2])
Rhyme 0:846e2321c637 398 + c * (e * ftarget[2] - h * ftarget[1]) ;
Rhyme 0:846e2321c637 399
Rhyme 0:846e2321c637 400 numerator[1] = -((f * g - d * i) * ftarget[0]
Rhyme 0:846e2321c637 401 + a * (i * ftarget[1] - f * ftarget[2])
Rhyme 0:846e2321c637 402 + c * (d * ftarget[2] - g * ftarget[1])) ;
Rhyme 0:846e2321c637 403
Rhyme 0:846e2321c637 404 numerator[2] = (e * g - d * h) * ftarget[0]
Rhyme 0:846e2321c637 405 + a * (h * ftarget[1] - e * ftarget[2])
Rhyme 0:846e2321c637 406 + b * (d * ftarget[2] - g * ftarget[1]) ;
Rhyme 0:846e2321c637 407
Rhyme 0:846e2321c637 408 for (idx = 0 ; idx < 3 ; idx++ ) {
Rhyme 0:846e2321c637 409 _pwm[idx] = (uint16_t) (0.5 + ((double)_probe * numerator[idx]) / denominator) ;
Rhyme 0:846e2321c637 410 result[idx] = _pwm[idx] ;
Rhyme 0:846e2321c637 411 }
Rhyme 0:846e2321c637 412
Rhyme 0:846e2321c637 413 printf("PWM R = %d [0x%04x] ", result[0], result[0]) ;
Rhyme 0:846e2321c637 414 wait_ms(1) ;
Rhyme 0:846e2321c637 415 printf("G = %d [0x%04x] ", result[1], result[1]) ;
Rhyme 0:846e2321c637 416 wait_ms(1) ;
Rhyme 0:846e2321c637 417 printf("B = %d [0x%04x] ", result[2], result[2]) ;
Rhyme 0:846e2321c637 418 wait_ms(1) ;
Rhyme 0:846e2321c637 419 printf("\n") ;
Rhyme 0:846e2321c637 420 wait_ms(1) ;
Rhyme 0:846e2321c637 421 printf("=== test ===\n") ;
Rhyme 0:846e2321c637 422 setLEDs(_pwm[0], _pwm[1], _pwm[2]) ;
Rhyme 0:846e2321c637 423 wait_ms(led_interval) ;
Rhyme 0:846e2321c637 424 getAveColor(v, num_ave) ;
Rhyme 0:846e2321c637 425 printf("R:%d, G:%d, B:%d\n", v[0], v[1], v[2]) ;
Rhyme 0:846e2321c637 426 printf("============\n") ;
Rhyme 0:846e2321c637 427 wait_ms(1) ;
Rhyme 0:846e2321c637 428 } else {
Rhyme 0:846e2321c637 429 printf("calibration failed, pwm values were not updated\n") ;
Rhyme 0:846e2321c637 430 }
Rhyme 0:846e2321c637 431 reset_watch_dog() ;
Rhyme 0:846e2321c637 432 _sensor->getCOLORConf(&conf) ;
Rhyme 0:846e2321c637 433 wait_ms(10) ;
Rhyme 0:846e2321c637 434 _sensor->setCOLORConf(conf | 0x01) ; /* shutdown VEML6040 */
Rhyme 0:846e2321c637 435 wait_ms(200) ;
Rhyme 0:846e2321c637 436 reset_watch_dog() ;
Rhyme 0:846e2321c637 437 _sensor->setCOLORConf(conf) ;
Rhyme 0:846e2321c637 438 wait_ms(200) ;
Rhyme 0:846e2321c637 439 _calibration_request = 0 ;
Rhyme 0:846e2321c637 440 _status = EDGE_SENSOR_INACTIVE ;
Rhyme 0:846e2321c637 441 reset_watch_dog() ;
Rhyme 0:846e2321c637 442 }
Rhyme 0:846e2321c637 443 #endif /* calibration double version */