Orefatoi / Mbed 2 deprecated afero_poc15_171201

POC1.5 prototype 2 x color sensor 2 x LM75B 3 x AnalogIn 1 x accel

Dependencies:   mbed vt100

edge_sensor/edge_color.cpp@8:5590f55bdf41, 2017-12-07 (annotated)

Committer:
Rhyme
Date:
Thu Dec 07 10:13:13 2017 +0000
Revision:
8:5590f55bdf41
Parent:
7:aa858d789025
Child:
9:f958fa2cdc74
commit at the end of 7-Dec-2017

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Rhyme 0:f0de320e23ac 1 #include "mbed.h"
Rhyme 0:f0de320e23ac 2 #include "edge_sensor.h"
Rhyme 0:f0de320e23ac 3 #include "VEML6040.h"
Rhyme 0:f0de320e23ac 4 #include "edge_color.h"
Rhyme 0:f0de320e23ac 5
Rhyme 0:f0de320e23ac 6 /* VEML6040 config bits */
Rhyme 0:f0de320e23ac 7 /* sensor config loser 4bit */
Rhyme 0:f0de320e23ac 8 /* trigger mode etc. */
Rhyme 0:f0de320e23ac 9 #define SD_BIT 0x01
Rhyme 0:f0de320e23ac 10 #define AF_BIT 0x02
Rhyme 0:f0de320e23ac 11 #define TRIG_BIT 0x04
Rhyme 0:f0de320e23ac 12
Rhyme 0:f0de320e23ac 13 /* sensor config upper 4bit */
Rhyme 0:f0de320e23ac 14 /* integration time */
Rhyme 0:f0de320e23ac 15 int sensor_delay[] = {
Rhyme 0:f0de320e23ac 16 40,
Rhyme 0:f0de320e23ac 17 80,
Rhyme 0:f0de320e23ac 18 160,
Rhyme 0:f0de320e23ac 19 320,
Rhyme 0:f0de320e23ac 20 640,
Rhyme 0:f0de320e23ac 21 1280,
Rhyme 0:f0de320e23ac 22 1280, /* place holder */
Rhyme 0:f0de320e23ac 23 1280 /* place holder */
Rhyme 0:f0de320e23ac 24 } ;
Rhyme 0:f0de320e23ac 25 uint16_t color0_pwm[3] ;
Rhyme 0:f0de320e23ac 26 uint16_t color1_pwm[3] ;
Rhyme 0:f0de320e23ac 27 uint16_t color0_target[3] = { 3000, 3000, 3000 } ;
Rhyme 0:f0de320e23ac 28 uint16_t color1_target[3] = { 3000, 3000, 3000 } ;
Rhyme 0:f0de320e23ac 29
Rhyme 0:f0de320e23ac 30 edge_color::edge_color(VEML6040 *sensor, PwmOut *led[], uint16_t *pwm) : edge_sensor()
Rhyme 0:f0de320e23ac 31 {
Rhyme 0:f0de320e23ac 32 uint16_t dummy[3] ;
Rhyme 0:f0de320e23ac 33 _sensor = sensor ;
Rhyme 0:f0de320e23ac 34 _sensor_config = AF_BIT | TRIG_BIT ;
Rhyme 0:f0de320e23ac 35 _interval = 30 ;
Rhyme 0:f0de320e23ac 36
Rhyme 0:f0de320e23ac 37 _pwm_period = 2000 ; /* 2ms */
Rhyme 0:f0de320e23ac 38 _probe = 0xFA00 ; /* to avoid satulation at 255, using 250 */
Rhyme 0:f0de320e23ac 39 // _probe = 0xFF00 ;
Rhyme 0:f0de320e23ac 40 for (int i = 0 ; i < 3 ; i++ ) {
Rhyme 0:f0de320e23ac 41 _led[i] = led[i] ;
Rhyme 0:f0de320e23ac 42 _led[i]->write(1.0) ; /* turn LED off */
Rhyme 0:f0de320e23ac 43 _value[i] = 0 ;
Rhyme 0:f0de320e23ac 44 _pwm[i] = pwm[i] ;
Rhyme 0:f0de320e23ac 45 _led[i]->period_us(_pwm_period) ;
Rhyme 0:f0de320e23ac 46 }
Rhyme 0:f0de320e23ac 47 getRGB(dummy) ; // dummy read, the first data is usually garbage
Rhyme 0:f0de320e23ac 48 }
Rhyme 0:f0de320e23ac 49
Rhyme 0:f0de320e23ac 50 edge_color::~edge_color(void)
Rhyme 0:f0de320e23ac 51 {
Rhyme 0:f0de320e23ac 52 delete _sensor ;
Rhyme 0:f0de320e23ac 53 delete [] _led ;
Rhyme 0:f0de320e23ac 54 }
Rhyme 0:f0de320e23ac 55
Rhyme 0:f0de320e23ac 56 void edge_color::setLEDs(uint16_t led_value[])
Rhyme 0:f0de320e23ac 57 {
Rhyme 0:f0de320e23ac 58 for (int i = 0 ; i < 3 ; i++ ) {
Rhyme 0:f0de320e23ac 59 _led[i]->write((float)(65535 - led_value[i])/65535.0) ;
Rhyme 0:f0de320e23ac 60 }
Rhyme 0:f0de320e23ac 61 }
Rhyme 0:f0de320e23ac 62
Rhyme 0:f0de320e23ac 63 void edge_color::setLEDs(uint16_t r, uint16_t g, uint16_t b)
Rhyme 0:f0de320e23ac 64 {
Rhyme 0:f0de320e23ac 65 _led[0]->write((float)(65535 - r)/65535.0) ;
Rhyme 0:f0de320e23ac 66 _led[1]->write((float)(65535 - g)/65535.0) ;
Rhyme 0:f0de320e23ac 67 _led[2]->write((float)(65535 - b)/65535.0) ;
Rhyme 0:f0de320e23ac 68 }
Rhyme 0:f0de320e23ac 69
Rhyme 0:f0de320e23ac 70 void edge_color::reset(void)
Rhyme 0:f0de320e23ac 71 {
Rhyme 0:f0de320e23ac 72 for (int i = 0 ; i < 3 ; i++ ) {
Rhyme 0:f0de320e23ac 73 _value[i] = 0 ;
Rhyme 0:f0de320e23ac 74 }
Rhyme 0:f0de320e23ac 75 }
Rhyme 0:f0de320e23ac 76
Rhyme 0:f0de320e23ac 77 void edge_color::prepare(void)
Rhyme 0:f0de320e23ac 78 {
Rhyme 0:f0de320e23ac 79 // setLEDs(_pwm) ; // <- the other color sensor turns off (;_;)
Rhyme 0:f0de320e23ac 80 }
Rhyme 0:f0de320e23ac 81
Rhyme 0:f0de320e23ac 82 void edge_color::sample(void)
Rhyme 0:f0de320e23ac 83 {
Rhyme 0:f0de320e23ac 84 setLEDs(_pwm) ;
Rhyme 6:44ca704f2bc1 85 getRGB(_value) ;
Rhyme 6:44ca704f2bc1 86 #if 0
Rhyme 0:f0de320e23ac 87 _sensor->setCOLORConf( _sensor_config ) ;
Rhyme 0:f0de320e23ac 88 wait_ms(sensor_delay[(_sensor_config >> 4)&0x07]*1.25) ;
Rhyme 0:f0de320e23ac 89 _sensor->getRData(&_value[0]) ;
Rhyme 0:f0de320e23ac 90 _sensor->getGData(&_value[1]) ;
Rhyme 0:f0de320e23ac 91 _sensor->getBData(&_value[2]) ;
Rhyme 6:44ca704f2bc1 92 #endif
Rhyme 0:f0de320e23ac 93 _sampled_time = edge_time ;
Rhyme 0:f0de320e23ac 94 setLEDs(0, 0, 0) ; /* turn LEDs off */
Rhyme 0:f0de320e23ac 95 }
Rhyme 0:f0de320e23ac 96
Rhyme 0:f0de320e23ac 97 int edge_color::deliver(void)
Rhyme 0:f0de320e23ac 98 {
Rhyme 0:f0de320e23ac 99 int result ;
Rhyme 8:5590f55bdf41 100 char timestr[16] ;
Rhyme 0:f0de320e23ac 101 print_time(_sampled_time) ;
Rhyme 8:5590f55bdf41 102 time2seq(_sampled_time, timestr) ;
Rhyme 0:f0de320e23ac 103 printf(" color %d : R = %4d, G = %4d, B = %4d\n",
Rhyme 0:f0de320e23ac 104 _id, _value[0], _value[1], _value[2]) ;
Rhyme 7:aa858d789025 105 if (_id == 1) { /* color1 */
Rhyme 0:f0de320e23ac 106 sprintf(_str_buf,
Rhyme 8:5590f55bdf41 107 "{\"DEVICE\":\"COLOR\",\"PN\":\"VEML6040\",\"VAL_R\":\"%d\",\"VAL_G\":\"%d\",\"VAL_B\":\"%d\",\"UNIT\":\"mW/cm2\",\"S\":\"%s\",\"E\":\"%d\"}",
Rhyme 8:5590f55bdf41 108 _value[0], _value[1], _value[2], timestr, _error_count) ;
Rhyme 7:aa858d789025 109 } else { /* color2 */
Rhyme 7:aa858d789025 110 sprintf(_str_buf,
Rhyme 8:5590f55bdf41 111 "{\"DEVICE\":\"COLOR02\",\"PN\":\"VEML6040\",\"VAL_R\":\"%d\",\"VAL_G\":\"%d\",\"VAL_B\":\"%d\",\"UNIT\":\"mW/cm2\",\"S\":\"%s\",\"E\":\"%d\"}",
Rhyme 8:5590f55bdf41 112 _value[0], _value[1], _value[2], timestr, _error_count) ;
Rhyme 7:aa858d789025 113 }
Rhyme 0:f0de320e23ac 114 result = afero->setAttribute(1, _str_buf) ;
Rhyme 0:f0de320e23ac 115 return( result == afSUCCESS ) ;
Rhyme 0:f0de320e23ac 116 }
Rhyme 0:f0de320e23ac 117
Rhyme 0:f0de320e23ac 118 void edge_color::getRGB(uint16_t v[])
Rhyme 0:f0de320e23ac 119 {
Rhyme 0:f0de320e23ac 120 _sensor->setCOLORConf(_sensor_config) ;
Rhyme 0:f0de320e23ac 121
Rhyme 0:f0de320e23ac 122 wait_ms(sensor_delay[(_sensor_config >> 4)&0x07] * 1.25) ;
Rhyme 0:f0de320e23ac 123
Rhyme 0:f0de320e23ac 124 _sensor->getRData(&v[0]) ;
Rhyme 0:f0de320e23ac 125 _sensor->getGData(&v[1]) ;
Rhyme 0:f0de320e23ac 126 _sensor->getBData(&v[2]) ;
Rhyme 0:f0de320e23ac 127 }
Rhyme 0:f0de320e23ac 128
Rhyme 0:f0de320e23ac 129 /**
Rhyme 0:f0de320e23ac 130 * Measure num_ave + 2 times
Rhyme 0:f0de320e23ac 131 * and throw away min and max
Rhyme 0:f0de320e23ac 132 * before calculating average
Rhyme 0:f0de320e23ac 133 */
Rhyme 0:f0de320e23ac 134 void edge_color::getAveColor(uint16_t v[], int num_ave)
Rhyme 0:f0de320e23ac 135 {
Rhyme 0:f0de320e23ac 136 int i, c ;
Rhyme 0:f0de320e23ac 137 long l[3] = {0, 0, 0} ;
Rhyme 0:f0de320e23ac 138 uint16_t min[3] = { 0, 0, 0 } ;
Rhyme 0:f0de320e23ac 139 uint16_t max[3] = { 0, 0, 0 } ;
Rhyme 0:f0de320e23ac 140 uint16_t *tmp ;
Rhyme 0:f0de320e23ac 141
Rhyme 0:f0de320e23ac 142 tmp = new uint16_t[(num_ave+2)*3] ;
Rhyme 0:f0de320e23ac 143
Rhyme 0:f0de320e23ac 144 getRGB(&tmp[0]) ; // dummy read
Rhyme 0:f0de320e23ac 145 wait(0.1) ;
Rhyme 0:f0de320e23ac 146 for (i = 0 ; i < num_ave+2 ; i++ ) {
Rhyme 0:f0de320e23ac 147 getRGB(&tmp[i*3]) ;
Rhyme 0:f0de320e23ac 148 // printf("%04x %04x %04x\n", tmp[i].r, tmp[i].g, tmp[i].b) ;
Rhyme 0:f0de320e23ac 149 for (c = 0 ; c < 3 ; c++ ) { /* c = r, g, b */
Rhyme 0:f0de320e23ac 150 if (tmp[i*3+c] < tmp[min[c]]) {
Rhyme 0:f0de320e23ac 151 min[c] = i ;
Rhyme 0:f0de320e23ac 152 }
Rhyme 0:f0de320e23ac 153 if (tmp[i*3+c] > tmp[max[c]]) {
Rhyme 0:f0de320e23ac 154 max[c] = i ;
Rhyme 0:f0de320e23ac 155 }
Rhyme 0:f0de320e23ac 156 }
Rhyme 0:f0de320e23ac 157 }
Rhyme 0:f0de320e23ac 158 for (i = 0 ; i < num_ave+2 ; i++ ) {
Rhyme 0:f0de320e23ac 159 for (c = 0 ; c < 3 ; c++ ) {
Rhyme 0:f0de320e23ac 160 if ((min[c] != i)&&(max[c] != i)) {
Rhyme 0:f0de320e23ac 161 l[c] += tmp[i*3+c] ;
Rhyme 0:f0de320e23ac 162 }
Rhyme 0:f0de320e23ac 163 }
Rhyme 0:f0de320e23ac 164 }
Rhyme 0:f0de320e23ac 165
Rhyme 0:f0de320e23ac 166 delete [] tmp ;
Rhyme 0:f0de320e23ac 167 for (c = 0 ; c < 3 ; c++ ) {
Rhyme 0:f0de320e23ac 168 v[c] = (uint16_t)(l[c] / num_ave) ;
Rhyme 0:f0de320e23ac 169 }
Rhyme 0:f0de320e23ac 170 // printf("=== average ===\n") ;
Rhyme 0:f0de320e23ac 171 // printf("%04x %04x %04x\n", v[0], v[1], v[2]) ;
Rhyme 0:f0de320e23ac 172 }
Rhyme 0:f0de320e23ac 173
Rhyme 0:f0de320e23ac 174 void edge_color::calibrate(uint16_t target[], uint16_t result[], int num_ave)
Rhyme 0:f0de320e23ac 175 {
Rhyme 0:f0de320e23ac 176 const uint16_t led_interval = 10 ; /* wait 10ms for LED */
Rhyme 0:f0de320e23ac 177 double denominator ;
Rhyme 0:f0de320e23ac 178 double numerator[3] ;
Rhyme 0:f0de320e23ac 179 double a,b,c,d,e,f,g,h,i ;
Rhyme 0:f0de320e23ac 180 uint16_t v[3], tmp[3] ;
Rhyme 5:eba500888787 181 // uint16_t L[3][3] ;
Rhyme 5:eba500888787 182 double L[3][3] ;
Rhyme 5:eba500888787 183 double ftarget[3] ;
Rhyme 0:f0de320e23ac 184 int idx ;
Rhyme 0:f0de320e23ac 185
Rhyme 5:eba500888787 186 ftarget[0] = target[0] ;
Rhyme 5:eba500888787 187 ftarget[1] = target[1] ;
Rhyme 5:eba500888787 188 ftarget[2] = target[2] ;
Rhyme 0:f0de320e23ac 189 printf("=== Calibrating Color Sensor %d ===\n", _id) ;
Rhyme 0:f0de320e23ac 190 for (idx = 0 ; idx < 3 ; idx++ ) {
Rhyme 0:f0de320e23ac 191 tmp[0] = tmp[1] = tmp[2] = 0 ;
Rhyme 0:f0de320e23ac 192 tmp[idx] = _probe ;
Rhyme 5:eba500888787 193
Rhyme 0:f0de320e23ac 194 setLEDs(tmp) ;
Rhyme 0:f0de320e23ac 195 wait_ms(led_interval) ;
Rhyme 0:f0de320e23ac 196 getAveColor(v, num_ave) ;
Rhyme 0:f0de320e23ac 197
Rhyme 0:f0de320e23ac 198 printf("R:%5d, G:%5d, B:%5d\n", v[0], v[1], v[2]) ;
Rhyme 0:f0de320e23ac 199 L[idx][0] = v[0] ;
Rhyme 0:f0de320e23ac 200 L[idx][1] = v[1] ;
Rhyme 0:f0de320e23ac 201 L[idx][2] = v[2] ;
Rhyme 0:f0de320e23ac 202 setLEDs(0, 0, 0) ; /* clear LEDs */
Rhyme 0:f0de320e23ac 203 }
Rhyme 0:f0de320e23ac 204
Rhyme 0:f0de320e23ac 205 printf("=== Initial Equation ===\n") ;
Rhyme 0:f0de320e23ac 206 for (idx = 0 ; idx < 3 ; idx++) {
Rhyme 0:f0de320e23ac 207 printf("%5d * R / %d + %5d * G / %d + %5d * B / %d = %d,\n",
Rhyme 5:eba500888787 208 (int)L[0][idx], _probe, (int)L[1][idx], _probe, (int)L[2][idx], _probe, target[idx]) ;
Rhyme 0:f0de320e23ac 209 }
Rhyme 0:f0de320e23ac 210
Rhyme 0:f0de320e23ac 211 a = L[0][0] ; b = L[1][0] ; c = L[2][0] ;
Rhyme 0:f0de320e23ac 212 d = L[0][1] ; e = L[1][1] ; f = L[2][1] ;
Rhyme 0:f0de320e23ac 213 g = L[0][2] ; h = L[1][2] ; i = L[2][2] ;
Rhyme 0:f0de320e23ac 214
Rhyme 0:f0de320e23ac 215 denominator = a * (f * h - e * i) + b * (d * i - f * g) + c * (e * g - d * h) ;
Rhyme 0:f0de320e23ac 216
Rhyme 0:f0de320e23ac 217 if (denominator != 0) {
Rhyme 5:eba500888787 218 numerator[0] = (f * h - e * i) * ftarget[0]
Rhyme 5:eba500888787 219 + b * (i * ftarget[1] - f * ftarget[2])
Rhyme 5:eba500888787 220 + c * (e * ftarget[2] - h * ftarget[1]) ;
Rhyme 0:f0de320e23ac 221
Rhyme 5:eba500888787 222 numerator[1] = -((f * g - d * i) * ftarget[0]
Rhyme 5:eba500888787 223 + a * (i * ftarget[1] - f * ftarget[2])
Rhyme 5:eba500888787 224 + c * (d * ftarget[2] - g * ftarget[1])) ;
Rhyme 0:f0de320e23ac 225
Rhyme 5:eba500888787 226 numerator[2] = (e * g - d * h) * ftarget[0]
Rhyme 5:eba500888787 227 + a * (h * ftarget[1] - e * ftarget[2])
Rhyme 5:eba500888787 228 + b * (d * ftarget[2] - g * ftarget[1]) ;
Rhyme 0:f0de320e23ac 229
Rhyme 0:f0de320e23ac 230 for (idx = 0 ; idx < 3 ; idx++ ) {
Rhyme 5:eba500888787 231 _pwm[idx] = (uint16_t) (0.5 + ((double)_probe * numerator[idx]) / denominator) ;
Rhyme 0:f0de320e23ac 232 result[idx] = _pwm[idx] ;
Rhyme 0:f0de320e23ac 233 }
Rhyme 0:f0de320e23ac 234
Rhyme 5:eba500888787 235 printf("PWM R = %d [0x%04x] ", result[0], result[0]) ;
Rhyme 0:f0de320e23ac 236 printf("G = %d [0x%04x] ", result[1], result[1]) ;
Rhyme 0:f0de320e23ac 237 printf("B = %d [0x%04x] ", result[2], result[2]) ;
Rhyme 0:f0de320e23ac 238 printf("\n") ;
Rhyme 0:f0de320e23ac 239 printf("=== test ===\n") ;
Rhyme 0:f0de320e23ac 240 setLEDs(_pwm[0], _pwm[1], _pwm[2]) ;
Rhyme 0:f0de320e23ac 241 wait_ms(led_interval) ;
Rhyme 0:f0de320e23ac 242 getAveColor(v, num_ave) ;
Rhyme 0:f0de320e23ac 243 printf("R:%d, G:%d, B:%d\n", v[0], v[1], v[2]) ;
Rhyme 5:eba500888787 244 printf("============\n") ;
Rhyme 0:f0de320e23ac 245 } else {
Rhyme 0:f0de320e23ac 246 printf("calibration failed, pwm values were not updated\n") ;
Rhyme 0:f0de320e23ac 247 }
Rhyme 0:f0de320e23ac 248 }