Functional filter but not actually working because it's way too slow for 1kHz sampling rate as yet.

Author: finnkuusisto

Devlpr.cpp

@@ -3,8 +3,9 @@
 
 Devlpr::Devlpr()
 {
-    bufInd = 0;
-    emgPin = A0;
+    bufInd = BUFSIZE - 1;
+    emgPin = A5;
+    emgRunningSum = 0;
 }
 
 void Devlpr::tick()
@@ -16,14 +17,14 @@ void Devlpr::tick()
     // accrue micros on the micros since last run
     microsSinceEMG += microsDelta;
     // now see if enough time has passed to run this bad boy
-    // keep in mind that the schedule is in millis vs micros
     if (microsSinceEMG > MICROS_SCHED_EMG) {
         readEMG();
         // and update micros since
         microsSinceEMG = 0;
         // NOTE do we want to do some sort of remainder on
         // NOTE the micros - ie do we want to play catch up
         // NOTE or just make a best effort to run on sched?
+        dummy = lastValueFiltered();
     }
     // just pretend no time has passed since function start
     lastTickMicros = currMicros;
@@ -34,13 +35,45 @@ int Devlpr::lastValue()
     return buf[bufInd];
 }
 
+float filt[155] = {0.001513386020877483, 0.0065815188379678286, 0.002386245075165573, 0.007566874896062961, 0.0019103137662135668, 0.006992501751352943, -4.495537256296272e-05, 0.0051834687785310895, -0.002648935758060934, 0.0033582848135394524, -0.004403527605449877, 0.0030360303136534005, -0.003970335570216464, 0.005088770411351987, -0.0010939546033377785, 0.009000196239377003, 0.0029797542078670355, 0.012873573885430926, 0.0059781068429677255, 0.014341550635307414, 0.005839078055311389, 0.011962177185195152, 0.0020540826800157686, 0.00628910346542397, -0.0037297389859886764, -0.00012797925377133632, -0.008360410071453508, -0.0039928957513214345, -0.008878628957170818, -0.0031739484856050243, -0.004358329258205972, 0.0018195934592311605, 0.0032359982373611025, 0.007774757946784205, 0.009842353743397264, 0.010355335400477728, 0.01147309137873063, 0.006565601478880059, 0.006608133637858301, -0.0032987826122814258, -0.002619853769121824, -0.01545390682749638, -0.011297979146828542, -0.024490682105516394, -0.01435098160629302, -0.026385833993863368, -0.009505783044341567, -0.02096941545317657, 0.0011335663648171434, -0.01233293740414986, 0.01195399467829908, -0.006767237737413448, 0.016825048377839883, -0.009258440470070626, 0.012608322724518551, -0.020500324958948334, 0.0011602156609500842, -0.03620786400461252, -0.011438446353649424, -0.04927237222277001, -0.018137619347845493, -0.053662062433447046, -0.014898493047407273, -0.047894076917303914, -0.0031289843896549954, -0.0360514341092553, 0.010898270201266994, -0.025659125374167382, 0.019426167695941163, -0.023527141221818995, 0.017546298160676748, -0.031878568213663964, 0.006034030764155489, -0.04698185315200764, -0.008967266965349297, -0.06112055411706036, -0.01929407495907671, 0.9363988033051267, -0.019294074959076694, -0.06112055411706037, -0.008967266965349314, -0.04698185315200765, 0.006034030764155483, -0.03187856821366395, 0.01754629816067676, -0.023527141221819026, 0.019426167695941157, -0.025659125374167382, 0.010898270201266975, -0.03605143410925534, -0.003128984389654987, -0.04789407691730391, -0.014898493047407264, -0.053662062433447046, -0.018137619347845493, -0.04927237222277002, -0.011438446353649422, -0.036207864004612525, 0.0011602156609500855, -0.020500324958948334, 0.012608322724518554, -0.00925844047007063, 0.01682504837783989, -0.006767237737413443, 0.011953994678299076, -0.012332937404149856, 0.0011335663648171465, -0.020969415453176564, -0.009505783044341567, -0.026385833993863354, -0.014350981606293018, -0.02449068210551641, -0.011297979146828535, -0.01545390682749638, -0.002619853769121825, -0.0032987826122814244, 0.006608133637858307, 0.006565601478880057, 0.011473091378730633, 0.010355335400477728, 0.009842353743397265, 0.007774757946784199, 0.0032359982373610994, 0.0018195934592311596, -0.004358329258205973, -0.003173948485605027, -0.008878628957170814, -0.003992895751321437, -0.008360410071453508, -0.0001279792537713359, -0.0037297389859886764, 0.00628910346542397, 0.00205408268001577, 0.011962177185195154, 0.005839078055311389, 0.014341550635307412, 0.0059781068429677255, 0.012873573885430931, 0.0029797542078670333, 0.009000196239377003, -0.0010939546033377785, 0.005088770411351986, -0.003970335570216465, 0.003036030313653403, -0.004403527605449876, 0.0033582848135394537, -0.002648935758060934, 0.005183468778531092, -4.4955372562961925e-05, 0.0069925017513529425, 0.0019103137662135673, 0.007566874896062961, 0.002386245075165573, 0.00658151883796783, 0.0015133860208774836};
+float Devlpr::lastValueFiltered()
+{
+    // the buffer is the same size as the filter so filtered data
+    // will be roughly len/2 ticks in the past
+    float ret = 0.0;
+    int currInd = bufInd + 1; // start at the tail
+    // the filter is also symmetric, so convolve=linear combination
+    for (int i = 0; i < BUFSIZE; i++) {
+        // check the current circular buffer index
+        if (currInd >= BUFSIZE) { // faster than modulo
+            currInd = 0;
+        }
+        // do the multiply/add
+        ret = ret + (filt[i] * buf[currInd]);
+        // and update the current buffer index for next
+        currInd++;
+    }
+    return ret;
+}
+
+float Devlpr::windowAvg()
+{
+    return emgRunningSum / float(BUFSIZE);
+}
+
 void Devlpr::readEMG()
 {
-    buf[bufInd] = analogRead(emgPin);
-    Serial.println(buf[bufInd]);
+    // want bufInd to sit on the most recent value until tick
+    // so update bufInd before the read
     bufInd++;
     // circular buffer
-    if (bufInd > BUFSIZE) {
+    if (bufInd >= BUFSIZE) { // surprisingly faster than mod
         bufInd = 0;
     }
+    // read our new value
+    emgVal = analogRead(emgPin);
+    // before replacing the prev tail value though, update running sum
+    emgRunningSum = emgRunningSum - buf[bufInd] + emgVal;
+    // now replace
+    buf[bufInd] = emgVal;
 }

Devlpr.h

@@ -9,17 +9,22 @@ class Devlpr
         Devlpr();
         void tick();
         int lastValue();
+        float lastValueFiltered();
+        float windowAvg();
+        float dummy;
     private:
         // general buffer bookkeeping
-        static const byte BUFSIZE = 129;
+        static const byte BUFSIZE = 155; // size of filter
         byte bufInd;
         int buf[BUFSIZE];
         unsigned long lastTickMicros = 0;
         // emg tracking
         unsigned long MICROS_SCHED_EMG = 1000;
-        void readEMG();
-        int emgPin;
         unsigned long microsSinceEMG;
+        unsigned long emgRunningSum;
+        int emgPin;
+        int emgVal;
+        void readEMG();
 };
 
 #endif