sensing well - need to debug oor

PseudoSensorControl/PseudoSensorControl.ino

@@ -2,7 +2,7 @@
 #include "IndSensorMap.hpp"
 #include "PseudoSensorControl.hpp"
 
-float refs[4] = {10.83,10.83,10.83,10.83};
+float refs[4] = {14,14,14,14};
 
 Constants repelling = {10000, 0, 50000};
 Constants attracting = {10000, 0, 50000};
@@ -16,6 +16,10 @@ K_MAP consts = {repelling, attracting};
 // Might be useful for things like jitter or lag.
 #define sampling_rate 1000 // Hz
 
+// EMA filter alpha value (all sensors use same alpha)
+#define alphaVal 0.3f
+
+
 // ABOVE THIS LINE IS TUNING VALUES ONLY, BELOW IS ACTUAL CODE.
 
 unsigned long tprior;
@@ -32,6 +36,11 @@ int ON = 0;
 void setup() {
   Serial.begin(57600);
 
+  indL.alpha = alphaVal;
+  indR.alpha = alphaVal;
+  indF.alpha = alphaVal;
+  indB.alpha = alphaVal;
+
   tprior = micros();
   for (PinPair& mc : pinMap) {
     pinMode(mc.dir, OUTPUT);

lib/IndSensorMap.cpp

@@ -9,8 +9,11 @@ IndSensorMap ind2Map = {-9.824360913609562, 871.4744633266955, 0.290936623509330
 IndSensorMap ind3Map = {-13.8907146886418, 990.6824637304771, 0.16376005385006073, -0.07513804021312243, 0.1772655198934789};
 
 // IndSensor class implementation
-IndSensor::IndSensor(IndSensorMap calibration, uint8_t analogPin)
-  : consts(calibration), pin(analogPin), oor(false) {}
+IndSensor::IndSensor(IndSensorMap calibration, uint8_t analogPin, float emaAlpha)
+  : consts(calibration), pin(analogPin), alpha(emaAlpha), oor(false), filteredRaw(0) {
+  // Initialize filtered value with first reading
+  filteredRaw = analogRead(pin);
+}
 
 // Convert raw analog reading to millimeters using sensor calibration
 float IndSensor::toMM(uint16_t raw) {
@@ -19,8 +22,14 @@ float IndSensor::toMM(uint16_t raw) {
 
 // Read sensor directly from pin and convert to millimeters
 float IndSensor::readMM() {
-  analog = analogRead(pin);
-  oor = (analog == 0 || analog > 870);  // Update out-of-range flag
+  uint16_t raw = constrain(analogRead(pin), 0, 900);
+  Serial.println(raw);
+  
+  // Exponential moving average filter
+  filteredRaw = alpha * raw + (1.0f - alpha) * filteredRaw;
+  
+  analog = (uint16_t)filteredRaw;
+  oor = (analog == 0 || analog > 870);
   mmVal = toMM(analog);
   return mmVal;
 }

lib/IndSensorMap.hpp

@@ -17,15 +17,17 @@ class IndSensor {
     bool oor;
     float mmVal;
     uint16_t analog;
+    float alpha; // EMA smoothing factor: 0-1, lower = more smoothing
 
     // Constructor
-    IndSensor(IndSensorMap calibration, uint8_t analogPin);
+    IndSensor(IndSensorMap calibration, uint8_t analogPin, float emaAlpha = 0.3f);
     // Read sensor directly from pin and convert to millimeters
     float readMM();
 
   private:
     IndSensorMap consts;
     uint8_t pin;
+    float filteredRaw;
 
     // helper function to convert analog reading to millimeters
     float toMM(uint16_t raw);

lib/PseudoSensorControl.cpp

@@ -83,13 +83,26 @@ void PseudoSensorController::report() {
   Serial.print(Back.mmVal);
   Serial.print("mm,\n");
 
+  Serial.print("OOR - Left: ");
+  Serial.print(Left.oor);
+  Serial.print(", Right: ");
+  Serial.print(Right.oor);
+  Serial.print(", Front: ");
+  Serial.print(Front.oor);
+  Serial.print(", Back: ");
+  Serial.print(Back.oor);
+  Serial.print(",\n");
+
+  Serial.print("Overall OOR: ");
+  Serial.println(oor);
+
   Serial.print("PWMS - FL_PWM: ");
   Serial.print(PWMs[0]);
   Serial.print(", FR_PWM: ");
   Serial.print(PWMs[1]);
-  Serial.print("BL_PWM: ");
+  Serial.print(", BL_PWM: ");
   Serial.print(PWMs[2]);
-  Serial.print("BR_PWM: ");
+  Serial.print(", BR_PWM: ");
   Serial.print(PWMs[3]);
   Serial.print("\n");
 }