Added a GPIO to power on/off for external I2C sensor(s) (with LEDs)
Dependencies: UniGraphic mbed vt100
18-Jun-2018 外部センサの電源オン・オフ機能は下位互換の為に無効になっていました。 この版で再度有効にしました。
Diff: edge_sensor/edge_color.cpp
- Revision:
- 0:846e2321c637
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/edge_sensor/edge_color.cpp Fri Apr 13 04:19:23 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 */