1 files changed, 66 insertions, 12 deletions

diff --git a/api_layers/firmware/esp32_stringPot/esp32_stringPot.ino b/api_layers/firmware/esp32_stringPot/esp32_stringPot.ino
index a2efc64..9edda7b 100644
--- a/api_layers/firmware/esp32_stringPot/esp32_stringPot.ino
+++ b/api_layers/firmware/esp32_stringPot/esp32_stringPot.ino
@@ -14,10 +14,21 @@
 const int sensorPin = A0;
 const unsigned long sampleInterval = 2000;
 unsigned long lastSampleTime = 0;
-const double lowPassFilter_alpha = 0.00675;
-const unsigned long calibration_factor = 1.07517;
-const long calibration_offset = -267.25;
-float filtered_displacement = 0.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() {
 
@@ -28,9 +39,15 @@ void setup() {
   }
 
   analogReadResolution(12);
+  if (calibrationMODE == false){
+    computeCalibrationParameters(measuredRange, readingRange);
+  }
+  computeButterworthCoeffs(butterworth_cutoffHz, butterworth_sampleHz);
 }
 
 void loop() {
+  handleSerialCommands();
+
   unsigned long currentTime = micros();
 
   if (currentTime - lastSampleTime >= sampleInterval) {
@@ -38,13 +55,13 @@ void loop() {
 
     uint32_t millivolts = analogReadMilliVolts(sensorPin);
     float displacement = convert_mV_to_mm(millivolts);
-    filtered_displacement = lowPassFilter(displacement, filtered_displacement, lowPassFilter_alpha);
+    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, 3);
+    Serial.println(filtered_displacement, 2);
     //Serial.println(millivolts);
   }
 }
@@ -53,14 +70,51 @@ 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 inverted_val = 650.0 - raw_val;
-  //float volts = inverted_val * (635.0 / 637.0);
   float volts = raw_val * (635.0 / 637.0);
-
   return calibration_factor * volts + calibration_offset;
 }
 
-float lowPassFilter(float newSample, float prevOutput, float alpha) {
-  // Simple exponential low-pass filter (EMA) formula
-  return alpha * newSample + (1.0 - alpha) * prevOutput;
+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;
+      }
+    }
+  }
 }