1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
|
/*
LabUI ESP32 Firmware v1.0
─────────────────────────────────────────────────────────────────────────
Author: Adam Shatila
Co-Author: Christian Kolset
Upload this sketch to your ESP32 with a TE SP1 spring potensiometer.
Required library: esp32 by Espressif Systems
─────────────────────────────────────────────────────────────────────────
*/
const int sensorPin = A0;
const unsigned long sampleInterval = 2000;
unsigned long lastSampleTime = 0;
//Calibration
float calibration_factor = 1.0;
float calibration_offset;
// 2 point calibration used for scaling factor
bool calibrationMODE = false;
float measuredRange[2] = {0, 150};
float readingRange[2] = {260.26, 421.3};
// 2nd-order Butterworth low-pass (biquad, direct form II transposed)
float butterworth_cutoffHz = 0.5;
const float butterworth_sampleHz = 1000000.0 / sampleInterval;
float butter_b0, butter_b1, butter_b2, butter_a1, butter_a2;
float butter_z1 = 0.0, butter_z2 = 0.0;
void setup() {
Serial.begin(115200);
while (!Serial) {
delay(10);
}
analogReadResolution(12);
if (calibrationMODE == false){
computeCalibrationParameters(measuredRange, readingRange);
}
computeButterworthCoeffs(butterworth_cutoffHz, butterworth_sampleHz);
}
void loop() {
handleSerialCommands();
unsigned long currentTime = micros();
if (currentTime - lastSampleTime >= sampleInterval) {
lastSampleTime = currentTime;
uint32_t millivolts = analogReadMilliVolts(sensorPin);
float displacement = convert_mV_to_mm(millivolts);
float filtered_displacement = butterworthLowPass(displacement);
float volts = filtered_displacement;
// Output the voltage to the serial monitor with 3 decimal places
//Serial.print(displacement, 3);
//Serial.print(", ");
Serial.println(filtered_displacement, 2);
//Serial.println(millivolts);
}
}
float convert_mV_to_mm(float mVolts){
// Reads mV from sensor and output in mm from lower limit
float voltage = mVolts / 1000.0;
float raw_val = voltage * (635.0 / 2.863);
float volts = raw_val * (635.0 / 637.0);
return calibration_factor * volts + calibration_offset;
}
void computeCalibrationParameters(float x[2], float y[2]){
calibration_factor = (x[1] - x[0]) / (y[1] - y[0]);
calibration_offset = x[0] - calibration_factor * y[0];
}
void computeButterworthCoeffs(float cutoffHz, float sampleHz) {
// 2nd-order Butterworth LPF via bilinear transform
float omega = tan(PI * cutoffHz / sampleHz);
float omega2 = omega * omega;
float sqrt2 = 1.41421356f;
float a0 = omega2 + sqrt2 * omega + 1.0f;
butter_b0 = omega2 / a0;
butter_b1 = 2.0f * butter_b0;
butter_b2 = butter_b0;
butter_a1 = 2.0f * (omega2 - 1.0f) / a0;
butter_a2 = (omega2 - sqrt2 * omega + 1.0f) / a0;
}
float butterworthLowPass(float newSample) {
// Direct form II transposed biquad
float output = butter_b0 * newSample + butter_z1;
butter_z1 = butter_b1 * newSample - butter_a1 * output + butter_z2;
butter_z2 = butter_b2 * newSample - butter_a2 * output;
return output;
}
void handleSerialCommands() {
// "C:<cutoffHz>\n" sets Butterworth cutoff frequency, e.g. "C:15.0\n"
if (Serial.available() > 0) {
String line = Serial.readStringUntil('\n');
line.trim();
if (line.startsWith("C:")) {
float newCutoff = line.substring(2).toFloat();
if (newCutoff > 0.0 && newCutoff < butterworth_sampleHz / 2.0) {
butterworth_cutoffHz = newCutoff;
computeButterworthCoeffs(butterworth_cutoffHz, butterworth_sampleHz);
butter_z1 = 0.0;
butter_z2 = 0.0;
}
}
}
}
|
