File content as of revision 4:c8bab10bc897:

#include "mbed.h"
#include "HIDScope.h"
#include "BiQuad.h"
#include "math.h"

AnalogIn emg1(A0);

HIDScope scope(6);
Ticker ticker;

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
//BiQuad bq1( 4.97164e-02, 9.94328e-02, 4.97164e-02, -3.04503e-01, 6.36149e-02 );
//BiQuad bq2( 1.00000e+00, -2.00000e+00, 1.00000e+00, -3.87549e-01, 4.72280e-01 );
//BiQuad bq3( 1.00000e+00, 2.00000e+00, 1.00000e+00, -1.95093e+00, 9.51645e-01 );
//BiQuad bq4( 1.00000e+00, -2.00000e+00, 1.00000e+00, -1.97996e+00, 9.80645e-01 );
//BiQuad bq5( 9.97389e-01, -1.97771e+00, 9.97389e-01, -1.97771e+00, 9.94778e-01 );

volatile float TOTAL_SAMPLE_SUM = 0;
volatile long NUMBER_SAMPLES = 0;

const int numEmgCache = 50;
float emgCache[numEmgCache]; //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];
    }
    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) {
    float sum = 0;
    for (int i = numEmgCache - 2; i >= 0; i--) {
        emgCache[i+1] = emgCache[i];
        sum += emgCache[i];
    }
    emgCache[0] = newValue;
    sum += newValue;
    return sum / numEmgCache;
}

float sum(float array[], int size) {
    float sum = 0;
    for (int i = 0; i < size; i++) {
        sum += array[i];
    }
    return sum;
}

float mean(float array[], int size) {
    return sum(array, size) / size;
}

float meanSquare(float array[], int size) {
    float naam[size];
    for(int i = 0; i < size; i++) {
        naam[i] = pow(array[i], 2);
    }
    return sum(naam, size) / size;
}

float variance(float array[], int size, float avg) {  
    float squaredDifferences[size];
    for (int i = 0; i < size; i++) {
        float difference = array[i] - avg;
        squaredDifferences[i] = difference*difference;
    }
    return mean(squaredDifferences, size);
}

float standardDeviation(float array[], int size, float avg) {
    return sqrt(variance(array, size, avg));
}

int decide(float value, float threshold) {
    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 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);
//    
//    float blocksize = sizeof(emgCache);
//        float rmsWithoutSquareRoot = meanSquare(emgCache, blocksize);
//        float rms = sqrt(rmsWithoutSquareRoot);
//        scope.set(3, rms);


    //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));
    //scope.set(4, decide(filtered, 0.4f));
    //scope.set(5, decide(filtered, 0.5f));
    
    scope.send();
}

int main()
{
   //bqc.add( &bq1 ).add( &bq2 );//.add( &bq3 ).add( &bq4 ).add( &bq5 );
   bqc.add( &bq1 );
   ticker.attach(&tick, 0.002);
   while (true);
}