Diff: main.cpp

Revision:

4:c8bab10bc897

Parent:

3:d4b8692e6697

--- a/main.cpp	Mon Oct 31 15:17:10 2016 +0000
+++ b/main.cpp	Tue Nov 01 14:26:40 2016 +0000
@@ -8,7 +8,9 @@
 HIDScope scope(6);
 Ticker ticker;
 
-//BiQuadChain bqc;
+BiQuadChain bqc;
+BiQuad bq1( 9.93756e-01, -1.89024e+00, 9.93756e-01, -1.89024e+00, 9.87512e-01 );
+
 //Bandpass butterworth filter + Notch butterworth filter.
 //Bandpass: 10 --- 500 Hz
 //Nothc:    50 +- 2 Hz
@@ -24,9 +26,6 @@
 const int numEmgCache = 50;
 float emgCache[numEmgCache]; //sorted from new to old;
 
-const int numAvgCache = 50;
-float averageCache[numAvgCache]; //sorted from new to old;
-
 void addFirst(float newValue, float array[], int size) {
     for (int i = size - 2; i >= 0; i--) {
         array[i+1] = array[i];
@@ -34,6 +33,16 @@
     array[0] = newValue;
 }
 
+float average(float newValue) {
+    float sum = 0;
+    for (int i = numEmgCache - 2; i >= 0; i--) {
+        sum += emgCache[i];
+    }
+   // emgCache[0] = newValue;
+    sum += newValue;
+    return sum / numEmgCache;
+}
+
 //shifts the array by adding the new emg value up front.
 //returns the new calculated average
 float movingAverage(float newValue) {
@@ -60,10 +69,11 @@
 }
 
 float meanSquare(float array[], int size) {
+    float naam[size];
     for(int i = 0; i < size; i++) {
-        array[i] *= array[i];
+        naam[i] = pow(array[i], 2);
     }
-    return sum(array, size) / size;
+    return sum(naam, size) / size;
 }
 
 float variance(float array[], int size, float avg) {  
@@ -83,35 +93,46 @@
     return value < threshold ? 0 : 1;
 }
 
+float rectifier(float value, float avg) {
+    return fabs(value - 0.5)*2;
+    //return (avg + fabs(value - avg));
+}
 
 void tick() {
     float emg = emg1.read();
     scope.set(0, emg);
-    float filtered = movingAverage( fabs(emg) );
-    scope.set(1, filtered);
+    
+    float avg = average(fabs(emg));   
+    
+    float rect = rectifier(emg, avg);
+    scope.set(1, rect);
+    
+    float filtered = movingAverage( rect );
+    scope.set(2, filtered);
+    
+    
+//    filtered = movingAverage( fabs(emg) );
+//    scope.set(3, filtered);
     
     TOTAL_SAMPLE_SUM += emg;
-    NUMBER_SAMPLES++;
-    
-    addFirst(filtered, averageCache, numAvgCache);
-    float avg = TOTAL_SAMPLE_SUM / NUMBER_SAMPLES; //TODO calibrate this value.
-    float stdDev = standardDeviation(averageCache, numAvgCache, avg);
-    scope.set(2, stdDev);
+    NUMBER_SAMPLES++;    
     
-    float blocksize = 1000;
-    if ((sizeof(averageCache)) > blocksize) {
-        float rmsWithoutSquareRoot = meanSquare(averageCache, blocksize);
-        float rms = sqrt(rmsWithoutSquareRoot);
-        scope.set(3, rms);
-    } else {
-        float rmsWithoutSquareRoot = meanSquare(averageCache, ( sizeof(averageCache)));
-        float rms = sqrt(rmsWithoutSquareRoot);
-        scope.set(3, rms);
+//    addFirst(filtered, averageCache, numAvgCache);
+//    float avg = TOTAL_SAMPLE_SUM / NUMBER_SAMPLES; //TODO calibrate this value.
+//    float stdDev = standardDeviation(averageCache, numAvgCache, avg);
+//    scope.set(2, stdDev);
+//    
+//    float blocksize = sizeof(emgCache);
+//        float rmsWithoutSquareRoot = meanSquare(emgCache, blocksize);
+//        float rms = sqrt(rmsWithoutSquareRoot);
+//        scope.set(3, rms);
 
-    }    
 
-    //float filtered = bqc.step( emg );
-    //scope.set(1, filtered);
+    //float notch = bqc.step( emg );
+    int threshold = decide(filtered ,0.2);
+    scope.set(3, threshold);
+    //float rectified = 
+    
     
     //scope.set(2, decide(filtered, 0.2f));
     //scope.set(3, decide(filtered, 0.3f));
@@ -124,7 +145,7 @@
 int main()
 {
    //bqc.add( &bq1 ).add( &bq2 );//.add( &bq3 ).add( &bq4 ).add( &bq5 );
-   
-   ticker.attach(&tick, 0.005);
+   bqc.add( &bq1 );
+   ticker.attach(&tick, 0.002);
    while (true);
 }
\ No newline at end of file