La Suno / Mbed 2 deprecated afero_poc15_180403R

Temporary Connector Reversed Version

Dependencies:   UniGraphic mbed vt100

afero_poc15_180403R , J1 のピン配置を反転させたヴァージョンです。

Color2系を使用するためには以下のピンをジャンパで接続してください。
J1-D7 <-> J1-D0
J1-D6 <-> J1-D1

(調査中) また、こちらでテストした範囲では、
FRDM-KL25Z の V3.3 を、Modulo2 の VCC_3V3 ピンに接続してやる必要がありました。

尚、J1-D1, D0 を使用するために UART を無効にしているため
ログは表示されません。

TFTモジュールについて　
aitendoのTFTモジュールはデフォルトでは8bit bus モードになっています。
/media/uploads/Rhyme/img_2364.jpg

半田のジャンパを変えて、SPIの設定にしてください。
/media/uploads/Rhyme/img_2363.jpg

サーミスタについて
POC1.5 では サーミスタは 25℃の時に抵抗値が 50.0kΩになる502AT-11 が
4.95kΩのプルアップ(実際は10kΩx2の並列)で使用されていました。

今回の試作では抵抗値が 10.0kΩの 103AT-11 が
5.1kΩのプルアップで使用されていますので、係数を合わせるために
SMTC502AT-11 のコンストラクタを　
R0 = 10.0
R1 = 5.1
B = 3435
T0 = 298.15
で呼ぶように変更しました。

edge_sensor/edge_color.cpp@1:6c54dc8acf96, 2018-04-24 (annotated)

Committer:
Rhyme
Date:
Tue Apr 24 12:18:10 2018 +0000
Revision:
1:6c54dc8acf96
Parent:
0:0b6732b53bf4 
to adjust with 103AT-11 with 5.1k pull-up, the constructor of 502AT-11 is called with R0=10.0, R1=5.1, B=3435, T0=298.15

Who changed what in which revision?

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