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@0:f0de320e23ac, 2017-12-01 (annotated)

Committer:
Rhyme
Date:
Fri Dec 01 06:16:31 2017 +0000
Revision:
0:f0de320e23ac
Child:
5:eba500888787
OLED display for GAS pressure started working

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 0:f0de320e23ac 85 _sensor->setCOLORConf( _sensor_config ) ;
Rhyme 0:f0de320e23ac 86 wait_ms(sensor_delay[(_sensor_config >> 4)&0x07]*1.25) ;
Rhyme 0:f0de320e23ac 87 _sensor->getRData(&_value[0]) ;
Rhyme 0:f0de320e23ac 88 _sensor->getGData(&_value[1]) ;
Rhyme 0:f0de320e23ac 89 _sensor->getBData(&_value[2]) ;
Rhyme 0:f0de320e23ac 90 _sampled_time = edge_time ;
Rhyme 0:f0de320e23ac 91 setLEDs(0, 0, 0) ; /* turn LEDs off */
Rhyme 0:f0de320e23ac 92 }
Rhyme 0:f0de320e23ac 93
Rhyme 0:f0de320e23ac 94 int edge_color::deliver(void)
Rhyme 0:f0de320e23ac 95 {
Rhyme 0:f0de320e23ac 96 int result ;
Rhyme 0:f0de320e23ac 97 print_time(_sampled_time) ;
Rhyme 0:f0de320e23ac 98 printf(" color %d : R = %4d, G = %4d, B = %4d\n",
Rhyme 0:f0de320e23ac 99 _id, _value[0], _value[1], _value[2]) ;
Rhyme 0:f0de320e23ac 100 sprintf(_str_buf,
Rhyme 0:f0de320e23ac 101 "{\"DEVICE\":\"COLOR%d\",\"PN\":\"VEML6040\",\"VAL_R\":\"%d\",\"VAL_G\":\"%d\",\"VAL_B\":\"%d\",\"UNIT\":\"mW/cm2\",\"S\":\"%06d\",\"E\":\"%d\"}",
Rhyme 0:f0de320e23ac 102 _id, _value[0], _value[1], _value[2],time2seq(_sampled_time), _error_count) ;
Rhyme 0:f0de320e23ac 103 result = afero->setAttribute(1, _str_buf) ;
Rhyme 0:f0de320e23ac 104 return( result == afSUCCESS ) ;
Rhyme 0:f0de320e23ac 105 }
Rhyme 0:f0de320e23ac 106
Rhyme 0:f0de320e23ac 107 void edge_color::getRGB(uint16_t v[])
Rhyme 0:f0de320e23ac 108 {
Rhyme 0:f0de320e23ac 109 _sensor->setCOLORConf(_sensor_config) ;
Rhyme 0:f0de320e23ac 110
Rhyme 0:f0de320e23ac 111 wait_ms(sensor_delay[(_sensor_config >> 4)&0x07] * 1.25) ;
Rhyme 0:f0de320e23ac 112
Rhyme 0:f0de320e23ac 113 _sensor->getRData(&v[0]) ;
Rhyme 0:f0de320e23ac 114 _sensor->getGData(&v[1]) ;
Rhyme 0:f0de320e23ac 115 _sensor->getBData(&v[2]) ;
Rhyme 0:f0de320e23ac 116 }
Rhyme 0:f0de320e23ac 117
Rhyme 0:f0de320e23ac 118 /**
Rhyme 0:f0de320e23ac 119 * Measure num_ave + 2 times
Rhyme 0:f0de320e23ac 120 * and throw away min and max
Rhyme 0:f0de320e23ac 121 * before calculating average
Rhyme 0:f0de320e23ac 122 */
Rhyme 0:f0de320e23ac 123 void edge_color::getAveColor(uint16_t v[], int num_ave)
Rhyme 0:f0de320e23ac 124 {
Rhyme 0:f0de320e23ac 125 int i, c ;
Rhyme 0:f0de320e23ac 126 long l[3] = {0, 0, 0} ;
Rhyme 0:f0de320e23ac 127 uint16_t min[3] = { 0, 0, 0 } ;
Rhyme 0:f0de320e23ac 128 uint16_t max[3] = { 0, 0, 0 } ;
Rhyme 0:f0de320e23ac 129 uint16_t *tmp ;
Rhyme 0:f0de320e23ac 130
Rhyme 0:f0de320e23ac 131 tmp = new uint16_t[(num_ave+2)*3] ;
Rhyme 0:f0de320e23ac 132
Rhyme 0:f0de320e23ac 133 getRGB(&tmp[0]) ; // dummy read
Rhyme 0:f0de320e23ac 134 wait(0.1) ;
Rhyme 0:f0de320e23ac 135 for (i = 0 ; i < num_ave+2 ; i++ ) {
Rhyme 0:f0de320e23ac 136 getRGB(&tmp[i*3]) ;
Rhyme 0:f0de320e23ac 137 // printf("%04x %04x %04x\n", tmp[i].r, tmp[i].g, tmp[i].b) ;
Rhyme 0:f0de320e23ac 138 for (c = 0 ; c < 3 ; c++ ) { /* c = r, g, b */
Rhyme 0:f0de320e23ac 139 if (tmp[i*3+c] < tmp[min[c]]) {
Rhyme 0:f0de320e23ac 140 min[c] = i ;
Rhyme 0:f0de320e23ac 141 }
Rhyme 0:f0de320e23ac 142 if (tmp[i*3+c] > tmp[max[c]]) {
Rhyme 0:f0de320e23ac 143 max[c] = i ;
Rhyme 0:f0de320e23ac 144 }
Rhyme 0:f0de320e23ac 145 }
Rhyme 0:f0de320e23ac 146 }
Rhyme 0:f0de320e23ac 147 for (i = 0 ; i < num_ave+2 ; i++ ) {
Rhyme 0:f0de320e23ac 148 for (c = 0 ; c < 3 ; c++ ) {
Rhyme 0:f0de320e23ac 149 if ((min[c] != i)&&(max[c] != i)) {
Rhyme 0:f0de320e23ac 150 l[c] += tmp[i*3+c] ;
Rhyme 0:f0de320e23ac 151 }
Rhyme 0:f0de320e23ac 152 }
Rhyme 0:f0de320e23ac 153 }
Rhyme 0:f0de320e23ac 154
Rhyme 0:f0de320e23ac 155 delete [] tmp ;
Rhyme 0:f0de320e23ac 156 for (c = 0 ; c < 3 ; c++ ) {
Rhyme 0:f0de320e23ac 157 v[c] = (uint16_t)(l[c] / num_ave) ;
Rhyme 0:f0de320e23ac 158 }
Rhyme 0:f0de320e23ac 159 // printf("=== average ===\n") ;
Rhyme 0:f0de320e23ac 160 // printf("%04x %04x %04x\n", v[0], v[1], v[2]) ;
Rhyme 0:f0de320e23ac 161 }
Rhyme 0:f0de320e23ac 162
Rhyme 0:f0de320e23ac 163 void edge_color::calibrate(uint16_t target[], uint16_t result[], int num_ave)
Rhyme 0:f0de320e23ac 164 {
Rhyme 0:f0de320e23ac 165 const uint16_t led_interval = 10 ; /* wait 10ms for LED */
Rhyme 0:f0de320e23ac 166 double denominator ;
Rhyme 0:f0de320e23ac 167 double numerator[3] ;
Rhyme 0:f0de320e23ac 168 double a,b,c,d,e,f,g,h,i ;
Rhyme 0:f0de320e23ac 169 uint16_t v[3], tmp[3] ;
Rhyme 0:f0de320e23ac 170 uint16_t L[3][3] ;
Rhyme 0:f0de320e23ac 171 int idx ;
Rhyme 0:f0de320e23ac 172
Rhyme 0:f0de320e23ac 173 printf("=== Calibrating Color Sensor %d ===\n", _id) ;
Rhyme 0:f0de320e23ac 174 for (idx = 0 ; idx < 3 ; idx++ ) {
Rhyme 0:f0de320e23ac 175 tmp[0] = tmp[1] = tmp[2] = 0 ;
Rhyme 0:f0de320e23ac 176 tmp[idx] = _probe ;
Rhyme 0:f0de320e23ac 177 setLEDs(tmp) ;
Rhyme 0:f0de320e23ac 178 wait_ms(led_interval) ;
Rhyme 0:f0de320e23ac 179 getAveColor(v, num_ave) ;
Rhyme 0:f0de320e23ac 180
Rhyme 0:f0de320e23ac 181 printf("R:%5d, G:%5d, B:%5d\n", v[0], v[1], v[2]) ;
Rhyme 0:f0de320e23ac 182 L[idx][0] = v[0] ;
Rhyme 0:f0de320e23ac 183 L[idx][1] = v[1] ;
Rhyme 0:f0de320e23ac 184 L[idx][2] = v[2] ;
Rhyme 0:f0de320e23ac 185 setLEDs(0, 0, 0) ; /* clear LEDs */
Rhyme 0:f0de320e23ac 186 }
Rhyme 0:f0de320e23ac 187
Rhyme 0:f0de320e23ac 188 printf("=== Initial Equation ===\n") ;
Rhyme 0:f0de320e23ac 189 for (idx = 0 ; idx < 3 ; idx++) {
Rhyme 0:f0de320e23ac 190 printf("%5d * R / %d + %5d * G / %d + %5d * B / %d = %d,\n",
Rhyme 0:f0de320e23ac 191 L[0][idx], _probe, L[1][idx], _probe, L[2][idx], _probe, target[idx]) ;
Rhyme 0:f0de320e23ac 192 }
Rhyme 0:f0de320e23ac 193
Rhyme 0:f0de320e23ac 194 a = L[0][0] ; b = L[1][0] ; c = L[2][0] ;
Rhyme 0:f0de320e23ac 195 d = L[0][1] ; e = L[1][1] ; f = L[2][1] ;
Rhyme 0:f0de320e23ac 196 g = L[0][2] ; h = L[1][2] ; i = L[2][2] ;
Rhyme 0:f0de320e23ac 197
Rhyme 0:f0de320e23ac 198 denominator = a * (f * h - e * i) + b * (d * i - f * g) + c * (e * g - d * h) ;
Rhyme 0:f0de320e23ac 199
Rhyme 0:f0de320e23ac 200 if (denominator != 0) {
Rhyme 0:f0de320e23ac 201 numerator[0] = (f * h - e * i) * target[0]
Rhyme 0:f0de320e23ac 202 + b * (i * target[1] - f * target[2])
Rhyme 0:f0de320e23ac 203 + c * (e * target[2] - h * target[1]) ;
Rhyme 0:f0de320e23ac 204
Rhyme 0:f0de320e23ac 205 numerator[1] = -((f * g - d * i) * target[0]
Rhyme 0:f0de320e23ac 206 + a * (i * target[1] - f * target[2])
Rhyme 0:f0de320e23ac 207 + c * (d * target[2] - g * target[1])) ;
Rhyme 0:f0de320e23ac 208
Rhyme 0:f0de320e23ac 209 numerator[2] = (e * g - d * h) * target[0]
Rhyme 0:f0de320e23ac 210 + a * (h * target[1] - e * target[2])
Rhyme 0:f0de320e23ac 211 + b * (d * target[2] - g * target[1]) ;
Rhyme 0:f0de320e23ac 212
Rhyme 0:f0de320e23ac 213 for (idx = 0 ; idx < 3 ; idx++ ) {
Rhyme 0:f0de320e23ac 214 _pwm[idx] = (uint16_t) ((_probe * numerator[idx]) / denominator) ;
Rhyme 0:f0de320e23ac 215 result[idx] = _pwm[idx] ;
Rhyme 0:f0de320e23ac 216 }
Rhyme 0:f0de320e23ac 217
Rhyme 0:f0de320e23ac 218 printf("R = %d [0x%04x] ", result[0], result[0]) ;
Rhyme 0:f0de320e23ac 219 printf("G = %d [0x%04x] ", result[1], result[1]) ;
Rhyme 0:f0de320e23ac 220 printf("B = %d [0x%04x] ", result[2], result[2]) ;
Rhyme 0:f0de320e23ac 221 printf("\n") ;
Rhyme 0:f0de320e23ac 222 printf("=== test ===\n") ;
Rhyme 0:f0de320e23ac 223 setLEDs(_pwm[0], _pwm[1], _pwm[2]) ;
Rhyme 0:f0de320e23ac 224 wait_ms(led_interval) ;
Rhyme 0:f0de320e23ac 225 getAveColor(v, num_ave) ;
Rhyme 0:f0de320e23ac 226 printf("R:%d, G:%d, B:%d\n", v[0], v[1], v[2]) ;
Rhyme 0:f0de320e23ac 227 } else {
Rhyme 0:f0de320e23ac 228 printf("calibration failed, pwm values were not updated\n") ;
Rhyme 0:f0de320e23ac 229 }
Rhyme 0:f0de320e23ac 230 }