ss
Dependencies: WS2812 PixelArray Adafruit_GFX
IRreflection.cpp@0:27e31cadeb36, 2019-06-15 (annotated)
- Committer:
- eunsong
- Date:
- Sat Jun 15 13:09:31 2019 +0000
- Revision:
- 0:27e31cadeb36
yes
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
eunsong | 0:27e31cadeb36 | 1 | #include "IRreflection.h" |
eunsong | 0:27e31cadeb36 | 2 | |
eunsong | 0:27e31cadeb36 | 3 | |
eunsong | 0:27e31cadeb36 | 4 | |
eunsong | 0:27e31cadeb36 | 5 | // Base class data member initialization (called by derived class init()) |
eunsong | 0:27e31cadeb36 | 6 | |
eunsong | 0:27e31cadeb36 | 7 | TRSensors::TRSensors(PinName MOSI,PinName MISO, PinName CLK, PinName CS) : _spi(MOSI,MISO,CLK), _spi_cs(CS) |
eunsong | 0:27e31cadeb36 | 8 | |
eunsong | 0:27e31cadeb36 | 9 | { |
eunsong | 0:27e31cadeb36 | 10 | |
eunsong | 0:27e31cadeb36 | 11 | _spi.format(16,0); |
eunsong | 0:27e31cadeb36 | 12 | |
eunsong | 0:27e31cadeb36 | 13 | _spi.frequency(2000000); |
eunsong | 0:27e31cadeb36 | 14 | |
eunsong | 0:27e31cadeb36 | 15 | _spi_cs = 1; |
eunsong | 0:27e31cadeb36 | 16 | |
eunsong | 0:27e31cadeb36 | 17 | _numSensors = NUMSENSORS; |
eunsong | 0:27e31cadeb36 | 18 | |
eunsong | 0:27e31cadeb36 | 19 | } |
eunsong | 0:27e31cadeb36 | 20 | |
eunsong | 0:27e31cadeb36 | 21 | |
eunsong | 0:27e31cadeb36 | 22 | |
eunsong | 0:27e31cadeb36 | 23 | |
eunsong | 0:27e31cadeb36 | 24 | |
eunsong | 0:27e31cadeb36 | 25 | |
eunsong | 0:27e31cadeb36 | 26 | |
eunsong | 0:27e31cadeb36 | 27 | void TRSensors::AnalogRead(int *sensor_values) |
eunsong | 0:27e31cadeb36 | 28 | |
eunsong | 0:27e31cadeb36 | 29 | { |
eunsong | 0:27e31cadeb36 | 30 | |
eunsong | 0:27e31cadeb36 | 31 | int i,j,ch; |
eunsong | 0:27e31cadeb36 | 32 | |
eunsong | 0:27e31cadeb36 | 33 | int values[] = {0,0,0,0,0,0}; |
eunsong | 0:27e31cadeb36 | 34 | |
eunsong | 0:27e31cadeb36 | 35 | |
eunsong | 0:27e31cadeb36 | 36 | |
eunsong | 0:27e31cadeb36 | 37 | for(j = 0;j < _numSensors + 1;j++) |
eunsong | 0:27e31cadeb36 | 38 | |
eunsong | 0:27e31cadeb36 | 39 | { |
eunsong | 0:27e31cadeb36 | 40 | |
eunsong | 0:27e31cadeb36 | 41 | ch =j; |
eunsong | 0:27e31cadeb36 | 42 | |
eunsong | 0:27e31cadeb36 | 43 | _spi_cs = 0; |
eunsong | 0:27e31cadeb36 | 44 | |
eunsong | 0:27e31cadeb36 | 45 | wait_us(2); |
eunsong | 0:27e31cadeb36 | 46 | |
eunsong | 0:27e31cadeb36 | 47 | values[j] = _spi.write(ch<<12); |
eunsong | 0:27e31cadeb36 | 48 | |
eunsong | 0:27e31cadeb36 | 49 | _spi_cs = 1; |
eunsong | 0:27e31cadeb36 | 50 | |
eunsong | 0:27e31cadeb36 | 51 | wait_us(21); |
eunsong | 0:27e31cadeb36 | 52 | |
eunsong | 0:27e31cadeb36 | 53 | values[j] = (values[j]>>6); |
eunsong | 0:27e31cadeb36 | 54 | |
eunsong | 0:27e31cadeb36 | 55 | } |
eunsong | 0:27e31cadeb36 | 56 | |
eunsong | 0:27e31cadeb36 | 57 | |
eunsong | 0:27e31cadeb36 | 58 | |
eunsong | 0:27e31cadeb36 | 59 | for(i = 0;i < _numSensors;i++) |
eunsong | 0:27e31cadeb36 | 60 | |
eunsong | 0:27e31cadeb36 | 61 | { |
eunsong | 0:27e31cadeb36 | 62 | |
eunsong | 0:27e31cadeb36 | 63 | sensor_values[i] = values[i+1]; |
eunsong | 0:27e31cadeb36 | 64 | |
eunsong | 0:27e31cadeb36 | 65 | } |
eunsong | 0:27e31cadeb36 | 66 | |
eunsong | 0:27e31cadeb36 | 67 | |
eunsong | 0:27e31cadeb36 | 68 | |
eunsong | 0:27e31cadeb36 | 69 | } |
eunsong | 0:27e31cadeb36 | 70 | |
eunsong | 0:27e31cadeb36 | 71 | |
eunsong | 0:27e31cadeb36 | 72 | |
eunsong | 0:27e31cadeb36 | 73 | // Reads the sensors 10 times and uses the results for |
eunsong | 0:27e31cadeb36 | 74 | |
eunsong | 0:27e31cadeb36 | 75 | // calibration. The sensor values are not returned; instead, the |
eunsong | 0:27e31cadeb36 | 76 | |
eunsong | 0:27e31cadeb36 | 77 | // maximum and minimum values found over time are stored internally |
eunsong | 0:27e31cadeb36 | 78 | |
eunsong | 0:27e31cadeb36 | 79 | // and used for the readCalibrated() method. |
eunsong | 0:27e31cadeb36 | 80 | |
eunsong | 0:27e31cadeb36 | 81 | void TRSensors::calibrate_init(int *calibratedMin, int *calibratedMax) |
eunsong | 0:27e31cadeb36 | 82 | |
eunsong | 0:27e31cadeb36 | 83 | { |
eunsong | 0:27e31cadeb36 | 84 | |
eunsong | 0:27e31cadeb36 | 85 | for(int i=0;i<_numSensors;i++) |
eunsong | 0:27e31cadeb36 | 86 | |
eunsong | 0:27e31cadeb36 | 87 | { |
eunsong | 0:27e31cadeb36 | 88 | |
eunsong | 0:27e31cadeb36 | 89 | calibratedMin[i] = 1023; |
eunsong | 0:27e31cadeb36 | 90 | |
eunsong | 0:27e31cadeb36 | 91 | calibratedMax[i] = 0; |
eunsong | 0:27e31cadeb36 | 92 | |
eunsong | 0:27e31cadeb36 | 93 | } |
eunsong | 0:27e31cadeb36 | 94 | |
eunsong | 0:27e31cadeb36 | 95 | } |
eunsong | 0:27e31cadeb36 | 96 | |
eunsong | 0:27e31cadeb36 | 97 | void TRSensors::calibrate(int *sensor_values, int *calibratedMin, int *calibratedMax) |
eunsong | 0:27e31cadeb36 | 98 | |
eunsong | 0:27e31cadeb36 | 99 | { |
eunsong | 0:27e31cadeb36 | 100 | |
eunsong | 0:27e31cadeb36 | 101 | int i=0; |
eunsong | 0:27e31cadeb36 | 102 | |
eunsong | 0:27e31cadeb36 | 103 | int j=0; |
eunsong | 0:27e31cadeb36 | 104 | |
eunsong | 0:27e31cadeb36 | 105 | |
eunsong | 0:27e31cadeb36 | 106 | |
eunsong | 0:27e31cadeb36 | 107 | for(j=0;j<10;j++) |
eunsong | 0:27e31cadeb36 | 108 | |
eunsong | 0:27e31cadeb36 | 109 | { |
eunsong | 0:27e31cadeb36 | 110 | |
eunsong | 0:27e31cadeb36 | 111 | AnalogRead(sensor_values); |
eunsong | 0:27e31cadeb36 | 112 | |
eunsong | 0:27e31cadeb36 | 113 | for(i=0;i<_numSensors;i++) |
eunsong | 0:27e31cadeb36 | 114 | |
eunsong | 0:27e31cadeb36 | 115 | { |
eunsong | 0:27e31cadeb36 | 116 | |
eunsong | 0:27e31cadeb36 | 117 | // set the max we found THIS time |
eunsong | 0:27e31cadeb36 | 118 | |
eunsong | 0:27e31cadeb36 | 119 | if(j == 0 || max_sensor_values[i] < sensor_values[i]) |
eunsong | 0:27e31cadeb36 | 120 | |
eunsong | 0:27e31cadeb36 | 121 | max_sensor_values[i] = sensor_values[i]; |
eunsong | 0:27e31cadeb36 | 122 | |
eunsong | 0:27e31cadeb36 | 123 | |
eunsong | 0:27e31cadeb36 | 124 | |
eunsong | 0:27e31cadeb36 | 125 | // set the min we found THIS time |
eunsong | 0:27e31cadeb36 | 126 | |
eunsong | 0:27e31cadeb36 | 127 | if(j == 0 || min_sensor_values[i] > sensor_values[i]) |
eunsong | 0:27e31cadeb36 | 128 | |
eunsong | 0:27e31cadeb36 | 129 | min_sensor_values[i] = sensor_values[i]; |
eunsong | 0:27e31cadeb36 | 130 | |
eunsong | 0:27e31cadeb36 | 131 | } |
eunsong | 0:27e31cadeb36 | 132 | |
eunsong | 0:27e31cadeb36 | 133 | } |
eunsong | 0:27e31cadeb36 | 134 | |
eunsong | 0:27e31cadeb36 | 135 | |
eunsong | 0:27e31cadeb36 | 136 | |
eunsong | 0:27e31cadeb36 | 137 | // record the min and max calibration values |
eunsong | 0:27e31cadeb36 | 138 | |
eunsong | 0:27e31cadeb36 | 139 | for(i=0;i<_numSensors;i++) |
eunsong | 0:27e31cadeb36 | 140 | |
eunsong | 0:27e31cadeb36 | 141 | { |
eunsong | 0:27e31cadeb36 | 142 | |
eunsong | 0:27e31cadeb36 | 143 | if(min_sensor_values[i] > calibratedMax[i]) |
eunsong | 0:27e31cadeb36 | 144 | |
eunsong | 0:27e31cadeb36 | 145 | calibratedMax[i] = min_sensor_values[i]; |
eunsong | 0:27e31cadeb36 | 146 | |
eunsong | 0:27e31cadeb36 | 147 | if(max_sensor_values[i] < calibratedMin[i]) |
eunsong | 0:27e31cadeb36 | 148 | |
eunsong | 0:27e31cadeb36 | 149 | calibratedMin[i] = max_sensor_values[i]; |
eunsong | 0:27e31cadeb36 | 150 | |
eunsong | 0:27e31cadeb36 | 151 | } |
eunsong | 0:27e31cadeb36 | 152 | |
eunsong | 0:27e31cadeb36 | 153 | } |
eunsong | 0:27e31cadeb36 | 154 | |
eunsong | 0:27e31cadeb36 | 155 | |
eunsong | 0:27e31cadeb36 | 156 | |
eunsong | 0:27e31cadeb36 | 157 | |
eunsong | 0:27e31cadeb36 | 158 | |
eunsong | 0:27e31cadeb36 | 159 | // Returns values calibrated to a value between 0 and 1000, where |
eunsong | 0:27e31cadeb36 | 160 | |
eunsong | 0:27e31cadeb36 | 161 | // 0 corresponds to the minimum value read by calibrate() and 1000 |
eunsong | 0:27e31cadeb36 | 162 | |
eunsong | 0:27e31cadeb36 | 163 | // corresponds to the maximum value. Calibration values are |
eunsong | 0:27e31cadeb36 | 164 | |
eunsong | 0:27e31cadeb36 | 165 | // stored separately for each sensor, so that differences in the |
eunsong | 0:27e31cadeb36 | 166 | |
eunsong | 0:27e31cadeb36 | 167 | // sensors are accounted for automatically. |
eunsong | 0:27e31cadeb36 | 168 | |
eunsong | 0:27e31cadeb36 | 169 | void TRSensors::readCalibrated(int *sensor_values, int *calibratedMin, int *calibratedMax) |
eunsong | 0:27e31cadeb36 | 170 | |
eunsong | 0:27e31cadeb36 | 171 | { |
eunsong | 0:27e31cadeb36 | 172 | |
eunsong | 0:27e31cadeb36 | 173 | int i; |
eunsong | 0:27e31cadeb36 | 174 | |
eunsong | 0:27e31cadeb36 | 175 | |
eunsong | 0:27e31cadeb36 | 176 | |
eunsong | 0:27e31cadeb36 | 177 | // read the needed values |
eunsong | 0:27e31cadeb36 | 178 | |
eunsong | 0:27e31cadeb36 | 179 | AnalogRead(sensor_values); |
eunsong | 0:27e31cadeb36 | 180 | |
eunsong | 0:27e31cadeb36 | 181 | |
eunsong | 0:27e31cadeb36 | 182 | |
eunsong | 0:27e31cadeb36 | 183 | for(i=0;i<_numSensors;i++) |
eunsong | 0:27e31cadeb36 | 184 | |
eunsong | 0:27e31cadeb36 | 185 | { |
eunsong | 0:27e31cadeb36 | 186 | |
eunsong | 0:27e31cadeb36 | 187 | int denominator; |
eunsong | 0:27e31cadeb36 | 188 | |
eunsong | 0:27e31cadeb36 | 189 | |
eunsong | 0:27e31cadeb36 | 190 | |
eunsong | 0:27e31cadeb36 | 191 | denominator = calibratedMax[i] - calibratedMin[i]; |
eunsong | 0:27e31cadeb36 | 192 | |
eunsong | 0:27e31cadeb36 | 193 | |
eunsong | 0:27e31cadeb36 | 194 | |
eunsong | 0:27e31cadeb36 | 195 | int x = 0; |
eunsong | 0:27e31cadeb36 | 196 | |
eunsong | 0:27e31cadeb36 | 197 | |
eunsong | 0:27e31cadeb36 | 198 | |
eunsong | 0:27e31cadeb36 | 199 | |
eunsong | 0:27e31cadeb36 | 200 | |
eunsong | 0:27e31cadeb36 | 201 | if(denominator != 0){ |
eunsong | 0:27e31cadeb36 | 202 | |
eunsong | 0:27e31cadeb36 | 203 | if(((signed int)sensor_values[i] - (signed int)calibratedMin[i])<0){ |
eunsong | 0:27e31cadeb36 | 204 | |
eunsong | 0:27e31cadeb36 | 205 | x = 0;} |
eunsong | 0:27e31cadeb36 | 206 | |
eunsong | 0:27e31cadeb36 | 207 | else { |
eunsong | 0:27e31cadeb36 | 208 | |
eunsong | 0:27e31cadeb36 | 209 | x = ((sensor_values[i] - calibratedMin[i])*1000/denominator); |
eunsong | 0:27e31cadeb36 | 210 | |
eunsong | 0:27e31cadeb36 | 211 | } |
eunsong | 0:27e31cadeb36 | 212 | |
eunsong | 0:27e31cadeb36 | 213 | } |
eunsong | 0:27e31cadeb36 | 214 | |
eunsong | 0:27e31cadeb36 | 215 | |
eunsong | 0:27e31cadeb36 | 216 | |
eunsong | 0:27e31cadeb36 | 217 | if( x > 1000) x = 1000; |
eunsong | 0:27e31cadeb36 | 218 | |
eunsong | 0:27e31cadeb36 | 219 | |
eunsong | 0:27e31cadeb36 | 220 | |
eunsong | 0:27e31cadeb36 | 221 | sensor_values[i] = x; |
eunsong | 0:27e31cadeb36 | 222 | |
eunsong | 0:27e31cadeb36 | 223 | |
eunsong | 0:27e31cadeb36 | 224 | |
eunsong | 0:27e31cadeb36 | 225 | |
eunsong | 0:27e31cadeb36 | 226 | |
eunsong | 0:27e31cadeb36 | 227 | } |
eunsong | 0:27e31cadeb36 | 228 | |
eunsong | 0:27e31cadeb36 | 229 | |
eunsong | 0:27e31cadeb36 | 230 | |
eunsong | 0:27e31cadeb36 | 231 | } |
eunsong | 0:27e31cadeb36 | 232 | |
eunsong | 0:27e31cadeb36 | 233 | |
eunsong | 0:27e31cadeb36 | 234 | |
eunsong | 0:27e31cadeb36 | 235 | |
eunsong | 0:27e31cadeb36 | 236 | |
eunsong | 0:27e31cadeb36 | 237 | |
eunsong | 0:27e31cadeb36 | 238 | |
eunsong | 0:27e31cadeb36 | 239 | // Operates the same as read calibrated, but also returns an |
eunsong | 0:27e31cadeb36 | 240 | |
eunsong | 0:27e31cadeb36 | 241 | // estimated position of the robot with respect to a line. The |
eunsong | 0:27e31cadeb36 | 242 | |
eunsong | 0:27e31cadeb36 | 243 | // estimate is made using a weighted average of the sensor indices |
eunsong | 0:27e31cadeb36 | 244 | |
eunsong | 0:27e31cadeb36 | 245 | // multiplied by 1000, so that a return value of 0 indicates that |
eunsong | 0:27e31cadeb36 | 246 | |
eunsong | 0:27e31cadeb36 | 247 | // the line is directly below sensor 0, a return value of 1000 |
eunsong | 0:27e31cadeb36 | 248 | |
eunsong | 0:27e31cadeb36 | 249 | // indicates that the line is directly below sensor 1, 2000 |
eunsong | 0:27e31cadeb36 | 250 | |
eunsong | 0:27e31cadeb36 | 251 | // indicates that it's below sensor 2000, etc. Intermediate |
eunsong | 0:27e31cadeb36 | 252 | |
eunsong | 0:27e31cadeb36 | 253 | // values indicate that the line is between two sensors. The |
eunsong | 0:27e31cadeb36 | 254 | |
eunsong | 0:27e31cadeb36 | 255 | // formula is: |
eunsong | 0:27e31cadeb36 | 256 | |
eunsong | 0:27e31cadeb36 | 257 | // |
eunsong | 0:27e31cadeb36 | 258 | |
eunsong | 0:27e31cadeb36 | 259 | // 0*value0 + 1000*value1 + 2000*value2 + ... |
eunsong | 0:27e31cadeb36 | 260 | |
eunsong | 0:27e31cadeb36 | 261 | // -------------------------------------------- |
eunsong | 0:27e31cadeb36 | 262 | |
eunsong | 0:27e31cadeb36 | 263 | // value0 + value1 + value2 + ... |
eunsong | 0:27e31cadeb36 | 264 | |
eunsong | 0:27e31cadeb36 | 265 | // |
eunsong | 0:27e31cadeb36 | 266 | |
eunsong | 0:27e31cadeb36 | 267 | // By default, this function assumes a dark line (high values) |
eunsong | 0:27e31cadeb36 | 268 | |
eunsong | 0:27e31cadeb36 | 269 | // surrounded by white (low values). If your line is light on |
eunsong | 0:27e31cadeb36 | 270 | |
eunsong | 0:27e31cadeb36 | 271 | // black, set the optional second argument white_line to true. In |
eunsong | 0:27e31cadeb36 | 272 | |
eunsong | 0:27e31cadeb36 | 273 | // this case, each sensor value will be replaced by (1000-value) |
eunsong | 0:27e31cadeb36 | 274 | |
eunsong | 0:27e31cadeb36 | 275 | // before the averaging. |
eunsong | 0:27e31cadeb36 | 276 | |
eunsong | 0:27e31cadeb36 | 277 | |
eunsong | 0:27e31cadeb36 | 278 | |
eunsong | 0:27e31cadeb36 | 279 | int TRSensors::readLine(int *sensor_values , int *calibratedMin, int *calibratedMax, int *online, char white_line) |
eunsong | 0:27e31cadeb36 | 280 | |
eunsong | 0:27e31cadeb36 | 281 | { |
eunsong | 0:27e31cadeb36 | 282 | |
eunsong | 0:27e31cadeb36 | 283 | char i, on_line = 0; |
eunsong | 0:27e31cadeb36 | 284 | |
eunsong | 0:27e31cadeb36 | 285 | long int avg; // this is for the weighted total, which is long |
eunsong | 0:27e31cadeb36 | 286 | |
eunsong | 0:27e31cadeb36 | 287 | // before division |
eunsong | 0:27e31cadeb36 | 288 | |
eunsong | 0:27e31cadeb36 | 289 | int sum; // this is for the denominator which is <= 64000 |
eunsong | 0:27e31cadeb36 | 290 | |
eunsong | 0:27e31cadeb36 | 291 | static int last_value=0; // assume initially that the line is left. |
eunsong | 0:27e31cadeb36 | 292 | |
eunsong | 0:27e31cadeb36 | 293 | |
eunsong | 0:27e31cadeb36 | 294 | |
eunsong | 0:27e31cadeb36 | 295 | readCalibrated(sensor_values, calibratedMin, calibratedMax); |
eunsong | 0:27e31cadeb36 | 296 | |
eunsong | 0:27e31cadeb36 | 297 | |
eunsong | 0:27e31cadeb36 | 298 | |
eunsong | 0:27e31cadeb36 | 299 | online[0] = 0; |
eunsong | 0:27e31cadeb36 | 300 | |
eunsong | 0:27e31cadeb36 | 301 | avg = 0; |
eunsong | 0:27e31cadeb36 | 302 | |
eunsong | 0:27e31cadeb36 | 303 | sum = 0; |
eunsong | 0:27e31cadeb36 | 304 | |
eunsong | 0:27e31cadeb36 | 305 | |
eunsong | 0:27e31cadeb36 | 306 | |
eunsong | 0:27e31cadeb36 | 307 | for(i=0;i<_numSensors;i++) { |
eunsong | 0:27e31cadeb36 | 308 | |
eunsong | 0:27e31cadeb36 | 309 | |
eunsong | 0:27e31cadeb36 | 310 | |
eunsong | 0:27e31cadeb36 | 311 | long int value = sensor_values[i]; |
eunsong | 0:27e31cadeb36 | 312 | |
eunsong | 0:27e31cadeb36 | 313 | |
eunsong | 0:27e31cadeb36 | 314 | |
eunsong | 0:27e31cadeb36 | 315 | //if(!white_line) value = 1000-value; |
eunsong | 0:27e31cadeb36 | 316 | |
eunsong | 0:27e31cadeb36 | 317 | if(white_line){ |
eunsong | 0:27e31cadeb36 | 318 | |
eunsong | 0:27e31cadeb36 | 319 | value = 1000-value; |
eunsong | 0:27e31cadeb36 | 320 | |
eunsong | 0:27e31cadeb36 | 321 | //sensor_values[i] = (1000 - sensor_values[i])/100; |
eunsong | 0:27e31cadeb36 | 322 | |
eunsong | 0:27e31cadeb36 | 323 | } |
eunsong | 0:27e31cadeb36 | 324 | |
eunsong | 0:27e31cadeb36 | 325 | |
eunsong | 0:27e31cadeb36 | 326 | |
eunsong | 0:27e31cadeb36 | 327 | //sensor_values[i] = value; |
eunsong | 0:27e31cadeb36 | 328 | |
eunsong | 0:27e31cadeb36 | 329 | // keep track of whether we see the line at all |
eunsong | 0:27e31cadeb36 | 330 | |
eunsong | 0:27e31cadeb36 | 331 | if(value > 300) { |
eunsong | 0:27e31cadeb36 | 332 | |
eunsong | 0:27e31cadeb36 | 333 | online[0] = 1; |
eunsong | 0:27e31cadeb36 | 334 | |
eunsong | 0:27e31cadeb36 | 335 | } |
eunsong | 0:27e31cadeb36 | 336 | |
eunsong | 0:27e31cadeb36 | 337 | |
eunsong | 0:27e31cadeb36 | 338 | |
eunsong | 0:27e31cadeb36 | 339 | // only average in values that are above a noise threshold |
eunsong | 0:27e31cadeb36 | 340 | |
eunsong | 0:27e31cadeb36 | 341 | |
eunsong | 0:27e31cadeb36 | 342 | if(value > 30) { |
eunsong | 0:27e31cadeb36 | 343 | |
eunsong | 0:27e31cadeb36 | 344 | |
eunsong | 0:27e31cadeb36 | 345 | |
eunsong | 0:27e31cadeb36 | 346 | avg += value*(1000*i);//(value/100)*(1000*i) // * sqrt(100,i); |
eunsong | 0:27e31cadeb36 | 347 | |
eunsong | 0:27e31cadeb36 | 348 | sum += value; //(value/100); |
eunsong | 0:27e31cadeb36 | 349 | |
eunsong | 0:27e31cadeb36 | 350 | } |
eunsong | 0:27e31cadeb36 | 351 | |
eunsong | 0:27e31cadeb36 | 352 | } |
eunsong | 0:27e31cadeb36 | 353 | |
eunsong | 0:27e31cadeb36 | 354 | /* |
eunsong | 0:27e31cadeb36 | 355 | |
eunsong | 0:27e31cadeb36 | 356 | if(!on_line) |
eunsong | 0:27e31cadeb36 | 357 | |
eunsong | 0:27e31cadeb36 | 358 | { |
eunsong | 0:27e31cadeb36 | 359 | |
eunsong | 0:27e31cadeb36 | 360 | // If it last read to the left of center, return 0. |
eunsong | 0:27e31cadeb36 | 361 | |
eunsong | 0:27e31cadeb36 | 362 | if(last_value < (_numSensors-1)*1000/2) |
eunsong | 0:27e31cadeb36 | 363 | |
eunsong | 0:27e31cadeb36 | 364 | return 0; |
eunsong | 0:27e31cadeb36 | 365 | |
eunsong | 0:27e31cadeb36 | 366 | |
eunsong | 0:27e31cadeb36 | 367 | |
eunsong | 0:27e31cadeb36 | 368 | // If it last read to the right of center, return the max. |
eunsong | 0:27e31cadeb36 | 369 | |
eunsong | 0:27e31cadeb36 | 370 | else |
eunsong | 0:27e31cadeb36 | 371 | |
eunsong | 0:27e31cadeb36 | 372 | return (_numSensors-1)*1000; |
eunsong | 0:27e31cadeb36 | 373 | |
eunsong | 0:27e31cadeb36 | 374 | } |
eunsong | 0:27e31cadeb36 | 375 | |
eunsong | 0:27e31cadeb36 | 376 | */ |
eunsong | 0:27e31cadeb36 | 377 | |
eunsong | 0:27e31cadeb36 | 378 | |
eunsong | 0:27e31cadeb36 | 379 | |
eunsong | 0:27e31cadeb36 | 380 | last_value = avg/sum; |
eunsong | 0:27e31cadeb36 | 381 | |
eunsong | 0:27e31cadeb36 | 382 | |
eunsong | 0:27e31cadeb36 | 383 | |
eunsong | 0:27e31cadeb36 | 384 | // if(sum == 0) last_value = 0; |
eunsong | 0:27e31cadeb36 | 385 | |
eunsong | 0:27e31cadeb36 | 386 | |
eunsong | 0:27e31cadeb36 | 387 | |
eunsong | 0:27e31cadeb36 | 388 | return last_value; |
eunsong | 0:27e31cadeb36 | 389 | |
eunsong | 0:27e31cadeb36 | 390 | } |
eunsong | 0:27e31cadeb36 | 391 | |
eunsong | 0:27e31cadeb36 | 392 | |
eunsong | 0:27e31cadeb36 | 393 | |
eunsong | 0:27e31cadeb36 | 394 | int TRSensors::sqrt(int m, int k){ |
eunsong | 0:27e31cadeb36 | 395 | |
eunsong | 0:27e31cadeb36 | 396 | int temp = m; |
eunsong | 0:27e31cadeb36 | 397 | |
eunsong | 0:27e31cadeb36 | 398 | if(k == 0) m = 1; |
eunsong | 0:27e31cadeb36 | 399 | |
eunsong | 0:27e31cadeb36 | 400 | else{ |
eunsong | 0:27e31cadeb36 | 401 | |
eunsong | 0:27e31cadeb36 | 402 | for(int i=0; i<(k-1); i++){ |
eunsong | 0:27e31cadeb36 | 403 | |
eunsong | 0:27e31cadeb36 | 404 | m = temp *m; |
eunsong | 0:27e31cadeb36 | 405 | |
eunsong | 0:27e31cadeb36 | 406 | } |
eunsong | 0:27e31cadeb36 | 407 | |
eunsong | 0:27e31cadeb36 | 408 | } |
eunsong | 0:27e31cadeb36 | 409 | |
eunsong | 0:27e31cadeb36 | 410 | return m; |
eunsong | 0:27e31cadeb36 | 411 | |
eunsong | 0:27e31cadeb36 | 412 | } |