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
- Committer:
- Rhyme
- Date:
- 2018-04-13
- Revision:
- 0:846e2321c637
File content as of revision 0:846e2321c637:
#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 */