diff --git a/PseudoSensorControl/PseudoSensorControl.ino b/PseudoSensorControl/PseudoSensorControl.ino index 1d17bc0..321aa46 100644 --- a/PseudoSensorControl/PseudoSensorControl.ino +++ b/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); diff --git a/lib/IndSensorMap.cpp b/lib/IndSensorMap.cpp index e8080fd..e1b6e9b 100644 --- a/lib/IndSensorMap.cpp +++ b/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; } diff --git a/lib/IndSensorMap.hpp b/lib/IndSensorMap.hpp index 29815ce..93d16b0 100644 --- a/lib/IndSensorMap.hpp +++ b/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); diff --git a/lib/PseudoSensorControl.cpp b/lib/PseudoSensorControl.cpp index 18f7f94..46af5cd 100644 --- a/lib/PseudoSensorControl.cpp +++ b/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"); } \ No newline at end of file