150 lines
6.0 KiB
C++
150 lines
6.0 KiB
C++
|
#include "IndSensorLUT.hpp"
|
||
|
|
#include <Arduino.h>
|
||
|
|
#include <avr/pgmspace.h>
|
||
|
|
#include "ADC.hpp"
|
||
|
|
|
||
|
|
// LUT active range: 4.0..18.0 mm at 0.1 mm resolution (141 entries each).
|
||
|
|
// Generated from A0Calibration/data/Sensor{0,1,2,5}.xlsx via linear interpolation
|
||
|
|
// onto a uniform mm grid; strictly monotonic increasing in ADC.
|
||
|
|
|
||
|
|
static const uint16_t ind0LUT[141] PROGMEM = {
|
||
|
|
67, 71, 74, 77, 81, 85, 99, 102, 108, 113,
|
||
|
|
117, 120, 125, 135, 141, 151, 154, 158, 165, 170,
|
||
|
|
181, 194, 196, 207, 212, 218, 223, 233, 234, 250,
|
||
|
|
265, 267, 279, 284, 291, 296, 301, 309, 327, 334,
|
||
|
|
346, 350, 357, 360, 366, 375, 379, 387, 411, 421,
|
||
|
|
422, 429, 434, 441, 451, 462, 470, 480, 487, 488,
|
||
|
|
497, 502, 505, 515, 523, 530, 537, 545, 546, 559,
|
||
|
|
566, 570, 575, 587, 592, 596, 601, 612, 618, 625,
|
||
|
|
628, 632, 636, 640, 646, 653, 658, 663, 668, 672,
|
||
|
|
678, 684, 686, 688, 691, 699, 703, 708, 709, 711,
|
||
|
|
715, 719, 722, 726, 729, 733, 737, 739, 743, 747,
|
||
|
|
748, 751, 754, 759, 760, 763, 766, 769, 770, 772,
|
||
|
|
775, 776, 779, 781, 783, 785, 788, 791, 792, 794,
|
||
|
|
796, 797, 798, 800, 802, 803, 806, 807, 808, 810,
|
||
|
|
811
|
||
|
|
};
|
||
|
|
|
||
|
|
static const uint16_t ind1LUT[141] PROGMEM = {
|
||
|
|
64, 68, 72, 77, 80, 87, 93, 96, 100, 109,
|
||
|
|
114, 121, 127, 133, 139, 146, 152, 164, 169, 176,
|
||
|
|
183, 192, 200, 207, 217, 224, 234, 242, 251, 259,
|
||
|
|
270, 280, 289, 299, 310, 319, 329, 337, 348, 358,
|
||
|
|
363, 377, 386, 395, 407, 416, 428, 437, 445, 453,
|
||
|
|
462, 475, 485, 497, 505, 515, 527, 532, 547, 553,
|
||
|
|
564, 573, 582, 591, 603, 611, 619, 626, 634, 644,
|
||
|
|
653, 658, 667, 675, 683, 691, 700, 704, 713, 721,
|
||
|
|
728, 735, 742, 748, 755, 762, 768, 774, 781, 786,
|
||
|
|
793, 798, 804, 809, 815, 820, 826, 830, 835, 840,
|
||
|
|
844, 849, 854, 859, 863, 867, 870, 874, 878, 881,
|
||
|
|
887, 890, 894, 896, 899, 902, 905, 909, 911, 914,
|
||
|
|
916, 921, 923, 926, 929, 932, 934, 937, 940, 942,
|
||
|
|
944, 946, 949, 951, 953, 955, 957, 959, 961, 962,
|
||
|
|
965
|
||
|
|
};
|
||
|
|
|
||
|
|
static const uint16_t ind2LUT[141] PROGMEM = {
|
||
|
|
58, 60, 65, 73, 76, 79, 85, 90, 94, 97,
|
||
|
|
101, 105, 114, 121, 126, 131, 136, 142, 148, 153,
|
||
|
|
163, 172, 178, 183, 188, 194, 202, 216, 226, 231,
|
||
|
|
241, 246, 253, 263, 271, 276, 284, 293, 306, 313,
|
||
|
|
318, 326, 337, 345, 353, 361, 369, 387, 394, 400,
|
||
|
|
406, 413, 420, 426, 434, 443, 450, 462, 467, 468,
|
||
|
|
475, 481, 486, 494, 501, 510, 518, 525, 531, 536,
|
||
|
|
542, 546, 556, 561, 567, 572, 577, 582, 588, 594,
|
||
|
|
599, 606, 612, 615, 623, 627, 631, 634, 638, 641,
|
||
|
|
646, 654, 657, 660, 664, 668, 672, 675, 679, 684,
|
||
|
|
687, 691, 694, 698, 701, 704, 706, 708, 711, 714,
|
||
|
|
716, 719, 722, 725, 727, 728, 732, 735, 737, 739,
|
||
|
|
740, 743, 745, 746, 748, 750, 751, 752, 755, 758,
|
||
|
|
760, 761, 762, 764, 765, 768, 769, 770, 771, 772,
|
||
|
|
773
|
||
|
|
};
|
||
|
|
|
||
|
|
static const uint16_t ind5LUT[141] PROGMEM = {
|
||
|
|
44, 47, 50, 54, 59, 62, 64, 69, 72, 77,
|
||
|
|
81, 86, 90, 97, 101, 104, 113, 118, 122, 127,
|
||
|
|
132, 139, 147, 154, 159, 167, 175, 180, 188, 193,
|
||
|
|
201, 212, 222, 228, 234, 239, 253, 258, 265, 273,
|
||
|
|
285, 292, 300, 307, 315, 323, 335, 349, 355, 364,
|
||
|
|
371, 375, 384, 392, 399, 407, 418, 424, 433, 440,
|
||
|
|
448, 455, 464, 472, 482, 488, 494, 501, 509, 520,
|
||
|
|
529, 536, 541, 548, 554, 558, 564, 574, 579, 586,
|
||
|
|
591, 598, 607, 611, 617, 624, 627, 632, 638, 646,
|
||
|
|
652, 657, 661, 665, 670, 675, 682, 687, 691, 695,
|
||
|
|
698, 703, 709, 712, 716, 719, 722, 728, 731, 735,
|
||
|
|
739, 743, 747, 749, 752, 754, 758, 761, 764, 768,
|
||
|
|
770, 773, 775, 777, 781, 784, 787, 789, 791, 792,
|
||
|
|
795, 797, 799, 801, 803, 805, 807, 808, 810, 813,
|
||
|
|
814
|
||
|
|
};
|
||
|
|
|
||
|
|
const IndSensorLUT ind0LUTCal = { ind0LUT, 141, 4.0f, 0.1f };
|
||
|
|
const IndSensorLUT ind1LUTCal = { ind1LUT, 141, 4.0f, 0.1f };
|
||
|
|
const IndSensorLUT ind2LUTCal = { ind2LUT, 141, 4.0f, 0.1f };
|
||
|
|
const IndSensorLUT ind5LUTCal = { ind5LUT, 141, 4.0f, 0.1f };
|
||
|
|
|
||
|
|
IndSensorL::IndSensorL(IndSensorLUT calibration, uint8_t analogPin, float emaAlpha)
|
||
|
|
: oor(false), oorDir(0), mmVal(0.0f), analog(0), alpha(emaAlpha),
|
||
|
|
cal(calibration), pin(analogPin), filteredRaw(0.0f) {
|
||
|
|
filteredRaw = analogRead(pin);
|
||
|
|
}
|
||
|
|
|
||
|
|
// Binary search the monotonic ADC table for `raw`, then linearly interpolate mm.
|
||
|
|
// LUT lives in Flash (PROGMEM); all reads go through pgm_read_word().
|
||
|
|
// Sets oor + oorDir when raw falls outside the calibrated range.
|
||
|
|
float IndSensorL::toMM(uint16_t raw) {
|
||
|
|
const uint16_t* t = cal.adcTable;
|
||
|
|
const uint16_t n = cal.length;
|
||
|
|
|
||
|
|
uint16_t t0 = pgm_read_word(&t[0]);
|
||
|
|
uint16_t tEnd = pgm_read_word(&t[n - 1]);
|
||
|
|
|
||
|
|
if (raw <= t0) {
|
||
|
|
oor = true;
|
||
|
|
oorDir = -1; // ADC below table → gap below mmMin → too close
|
||
|
|
return cal.mmMin;
|
||
|
|
}
|
||
|
|
if (raw >= tEnd) {
|
||
|
|
oor = true;
|
||
|
|
oorDir = +1; // ADC above table → gap above mmMax → too far
|
||
|
|
return cal.mmMin + (n - 1) * cal.mmStep;
|
||
|
|
}
|
||
|
|
|
||
|
|
uint16_t lo = 0, hi = n - 1;
|
||
|
|
while (hi - lo > 1) {
|
||
|
|
uint16_t mid = (lo + hi) >> 1;
|
||
|
|
if (pgm_read_word(&t[mid]) <= raw) lo = mid;
|
||
|
|
else hi = mid;
|
||
|
|
}
|
||
|
|
|
||
|
|
uint16_t vLo = pgm_read_word(&t[lo]);
|
||
|
|
uint16_t vHi = pgm_read_word(&t[hi]);
|
||
|
|
float frac = (float)(raw - vLo) / (float)(vHi - vLo);
|
||
|
|
return cal.mmMin + ((float)lo + frac) * cal.mmStep;
|
||
|
|
}
|
||
|
|
|
||
|
|
float IndSensorL::readMM() {
|
||
|
|
uint8_t index = pin - A0;
|
||
|
|
index = (index > 3) ? index - 2 : index;
|
||
|
|
uint16_t raw;
|
||
|
|
|
||
|
|
ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
|
||
|
|
raw = adc_results[index];
|
||
|
|
}
|
||
|
|
|
||
|
|
filteredRaw = alpha * raw + (1.0f - alpha) * filteredRaw;
|
||
|
|
analog = (uint16_t)filteredRaw;
|
||
|
|
|
||
|
|
oor = false;
|
||
|
|
oorDir = 0;
|
||
|
|
mmVal = toMM(analog);
|
||
|
|
return mmVal;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Face → sensor pin/LUT mapping (assigned by user).
|
||
|
|
IndSensorL indF(ind2LUTCal, A5);
|
||
|
|
IndSensorL indL(ind1LUTCal, A1);
|
||
|
|
IndSensorL indR(ind0LUTCal, A0);
|
||
|
|
IndSensorL indB(ind5LUTCal, A4);
|