File content as of revision 2:d59911284528:

#include "mbed.h"
#include "math.h"
#include "filter1.h"

#include <stdlib.h>
#include <string.h>

DigitalOut led(LED_RED);        //deklarace led
DigitalOut led2(LED_GREEN);
DigitalOut led3(LED_BLUE);

Serial pc(USBTX, USBRX);        //deklarace seriove linky a analogovych vstupu
AnalogIn   ain(A0);
AnalogIn   otoc(A5);

const int N=8000;               //pocet vzorku ucici faze (fvz je 1kHz)
const int M=100;                //pocet vzorku pro kontinualni mereni

float filter1_coefficients[20] =//definice filtru s IIR
{
    0.47949725793876763, -0.9589945158775353, 0.47949725793876763, 1.9426378322419233, -0.9435968961395514,// b0, b1, b2, a1, a2
    1, -2, 1, 1.9752697957988141, -0.9762449562056803,// b0, b1, b2, a1, a2
    1, 2, 1, -1.998839289707791, -0.9988396842695652,// b0, b1, b2, a1, a2
    2, 4, 2, -1.9995188301139024, -0.9995192248043052// b0, b1, b2, a1, a2

};

const int inputBufferSize = 1;           // delky IO bufferu v B + alokace
const int outputBufferSize = 1;          
float inputBuffer[inputBufferSize], outputBuffer[outputBufferSize];    

int main()
{
float emg4abs[N];               //absolutni hodnota emg4 pro ucici fazi 8s                                                         
float emg4[M];                  //namerene data 0,1s, emg4 = biceps
float emg4obal[N-M];            //obalka emg4abs

float emg3abs[N];               //to same pro emg3, emg3 = predlokti
float emg3[M];
float emg3obal[N-M];

float Maktual[M];               //aktualni okno pro vypocet obalky
float meze1;                    //meze pro vyhodnoceni emg3
float meze2;                    //meze pro vyhodnoceni emg4
float meze1zaloha;              //pomocna promenna
float emg4krit;                 //zdali je emg4 nad mezi

uint32_t cyklus=0;                //cyklus mereni
float sum4=0;                   //suma pro vypocet obalky emg4
float sum3=0;                   //suma pro vypocet obalky emg3
float s1[N];                    //abs. hodnota obalky pro vypocet meze
float maxs1[200];               //200 submaxim abs. hodnoty obalky
float obal4;                    //aktualni hodnota obalky emg4 behem kontinualniho mereni
float obal3;                    //aktualni hodnota obalky emg3 behem kontinualniho mereni
float data4[M];                 //namerena data se kteryma se dale pracuje na vyhodnoceni emg4
float data3[M];                 //namerena data se kteryma se dale pracuje na vyhodnoceni emg3

uint32_t procenta=0;            //aktualni pozice koncetiny v zavislosti na emg3 a emg4, emg4 kodovano mod1, emg3 mod1000
float zvysenimeze=3;            //nastaveni modifikace meze pro otoceni (emg3)

int compare (const void * a, const void * b);  //nastaveni pro quicksort

float vzorek[1];        //promenna do ktere se uklada hodnota analog. vstupu
int nProcessedSamples;  //pocet vzorku po filtraci
filter1Type *filter = filter1_create();  //deklarace filtru
  
//########################################################
//## ucici phase emg4 - biceps 8 vterin ... cca 4 flexy ##
//########################################################
led = 1;                //zhasnuti led
led2 =1;
led3 = 1;
pc.baud(115200);        //nastaveni baudrate

for(int i=0;i<(N);i++){          //ulozeni hodnoty do vzorek, filtrace a abs. hodnota
    vzorek[0] = ain.read_u16();
    nProcessedSamples = filter1_filterBlock( filter, vzorek, outputBuffer, inputBufferSize ); 
    emg4abs[i]=abs(outputBuffer[0]);
    wait(0.001f);                //fvz = 1kHz
}
led = 0;                //roznuti cervene led        

for(int i=0;i<N-M;i++){ //smycka pro vypocet obalky emg4 ucici faze
    for(int j=0;j<M;j++){
        Maktual[j]=emg4abs[i+j];  //nacteni do okna M vzorku (default 100)
    }
    qsort(Maktual,M,sizeof(float),compare); //serazeni od nejmensiho vzorku v okne po nejvetsi
    for(int j=(M/2);j<M;j++){               
        sum4=sum4+Maktual[j];    //secteni hornich defaultne 50 vzorku okna
    }
    emg4obal[i]=sum4/(M/2);      //vydeleni poctem defaultne 50 vzorku okna
    sum4=0;                      //tj. prumerna hodnota horni poloviny hodnot okna tvori obalku
}

for (int j=0;j<N-M;j++){
    emg4obal[j]=emg4obal[j]+1;                  //pricteni 1 pro omezeni hodnot mensich nez 1
    emg4obal[j]=emg4obal[j]*emg4obal[j];        //umocneni obalky (vsechny hodnoty se tak zvysi ptze jsou vetsi nez 1)
}
qsort(emg4obal,N-M,sizeof(float),compare);      //serazeni od min po max

meze2=0;                                        //inicializace meze pro biceps
for(int j=20;j<220;j++){                        //vyber 20. az 220. maxima
    maxs1[j-20]=emg4obal[(N-M)-(1*j)-1];        //a ulozeni do maxs1
    meze2=meze2+(maxs1[j-20]);                  //suma 200 submaxim
}      
meze2=meze2/200/4;      //podeleni poctem maxim a snizeni na ctvrtinu = definice prahu

//######################################################
//## ucici phase emg3 - otoc 8 vterin ... cca 4 flexy ##
//######################################################
led = 0;        //led signalizujici konec ucici faze pro biceps                                    
wait(1); 
led = 1;        //a start ucici faze pro predlokti (led zhasne)

for(int i=0;i<(N);i++){
    vzorek[0] = otoc.read_u16();
    nProcessedSamples = filter1_filterBlock( filter, vzorek, outputBuffer, inputBufferSize ); 
    emg3abs[i]=abs(outputBuffer[0]);
    wait(0.001f); 
}
led = 0;          //konec nacitani pro prodlokti

for(int i=0;i<N-M;i++){    
    for(int j=0;j<M;j++){
        Maktual[j]=emg3abs[i+j];  
    }
    qsort(Maktual,M,sizeof(float),compare);
    for(int j=(M/2);j<M;j++){
        sum3=sum3+Maktual[j];
    }
    emg3obal[i]=sum3/(M/2);
    sum3=0;
}

for (int j=0;j<N-M;j++){
    emg3obal[j]=emg3obal[j]+1;
    emg3obal[j]=emg3obal[j]*emg3obal[j];
}
qsort(emg3obal,N-M,sizeof(float),compare);
meze1=0;
for(int j=20;j<220;j++){
    maxs1[j-20]=emg3obal[(N-M)-(1*j)-1]; 
    meze1=meze1+(maxs1[j-20]);
}      
meze1=meze1/200/4;                
   
led=1;                            //zhasnuti led indikujici start mereni
//####################################################
//## kriterium emg4 - biceps, kriterium emg3 - otoc ##
//####################################################

for(int i=0;i<M;i++){
    vzorek[0]=ain.read_u16();
    nProcessedSamples = filter1_filterBlock( filter, vzorek, outputBuffer, inputBufferSize ); 
    emg4[i]=abs(outputBuffer[0]); 
    
    vzorek[0]=otoc.read_u16();
    nProcessedSamples = filter1_filterBlock( filter, vzorek, outputBuffer, inputBufferSize ); 
    emg3[i]=abs(outputBuffer[0]); 
    wait(0.001f); 
   
}
for (int i=0;i<M;i++){
    data4[i]=emg4[i];       //ulozeni do data4 a data3 ktere se quicksortuje
    data3[i]=emg3[i];
}
while(1)                    //nekonecna smycka mereni
{
    qsort(data4,M,sizeof(float),compare);      //serazeni data4 od min po max
    qsort(data3,M,sizeof(float),compare);      //serazeni data3 od min po max
    for(int j=(M/2);j<M;j++){
        sum4=sum4+data4[j];                    //secteni horni poloviny dat
        sum3=sum3+data3[j];
    }
    obal4=sum4/(M/2);                          //prumer horni poloviny dat
    obal4=obal4+1;                                      
    obal4=obal4*obal4;                         //mocninne zvyseni dat
    sum4=0;                                    //reset sumy
    
    obal3=sum3/(M/2);                          //to same s predloktim
    obal3=obal3+1;
    obal3=obal3*obal3;
    sum3=0;
    
    if(meze2<obal4){            //jestlize bude hodnota obalky vetsi nez meze2               
        emg4krit=1;             //kriterium pro biceps = 1 ... ruka se zveda
        if(procenta<400){       //jestlize neni na max hodnote 400 pak se zvedne
           procenta++;  
        }
        led2=0;                 //zelena led signalizujici zvedani
    }else{
        emg4krit=0;
        if(procenta>0){         //jestlize neni na min hodnote 0 pak klesne
           procenta--;
        }  
        led2=1;                 //zelena led signalizuje pokles
    }
    
    meze1zaloha=meze1;          //zalohuje se puvodni meze
    if(emg4krit==1){            //jestlize se ruka zveda                 
        meze1=meze1*zvysenimeze;//a zvysi se prah (defaultne 3*)                       
    }
    
    if(meze1<obal3){            //jestlize bude hodnota obalky vetsi nez meze1     
        if(procenta<400400){    //jestlize neni na max hodnote
           procenta=procenta+1000; //tak se zvysi (mod 1000)
        }   
        led3=0;                 //modra led signalizujici otoceni   
    }else{
        led3=1;  
        if(procenta>400){      //jestlize neni na min hodnote mod 1000 0 pak klesne
           procenta=procenta-1000;
        }  
    }
    meze1=meze1zaloha;         //navrat na puvodni mez                  
    
    if (cyklus % 10 == 1){           //po seriove lince se posila kazda 10. hodnota
        pc.printf("%d\n",procenta);  //vypis procent po seriove lince
    } 
       
    for (int i=0;i<M-1;i++){   //posun v okne o 1 dozadu      
        emg4[i]=emg4[i+1];
        emg3[i]=emg3[i+1];
    }
    
    vzorek[0]=ain.read_u16();  //ulozeni vyfiltrovane hodnoty na posledni index okna
    nProcessedSamples = filter1_filterBlock( filter, vzorek, outputBuffer, inputBufferSize ); 
    emg4[M-1]=abs(outputBuffer[0]);
    vzorek[0]=otoc.read_u16();
    nProcessedSamples = filter1_filterBlock( filter, vzorek, outputBuffer, inputBufferSize ); 
    emg3[M-1]=abs(outputBuffer[0]); 
    wait(0.001f); 
    
    cyklus++;                  //dalsi cyklus     
    
    for (int i=0;i<M;i++){     //ulozeni do data4 a data3 ktere se quicksortuje
        data4[i]=emg4[i];
        data3[i]=emg3[i];
    }
}
}

int compare (const void * a, const void * b)      //definice fce pro quicksort
{
  return ( *(int*)a - *(int*)b );
}
                                                  //fce pro definice filtru s iir
filter1Type *filter1_create( void )     
{
    filter1Type *result = (filter1Type *)malloc( sizeof( filter1Type ) );   // Allocate memory for the object
    filter1_init( result );                                         // Initialize it
    return result;                                                              // Return the result
}

void filter1_destroy( filter1Type *pObject )
{
    free( pObject );
}

 void filter1_init( filter1Type * pThis )
{
    filter1_reset( pThis );

}

 void filter1_reset( filter1Type * pThis )
{
    memset( &pThis->state, 0, sizeof( pThis->state ) ); // Reset state to 0
    pThis->output = 0;                                  // Reset output

}

 int filter1_filterBlock( filter1Type * pThis, float * pInput, float * pOutput, unsigned int count )
{
    filter1_executionState executionState;          // The executionState structure holds call data, minimizing stack reads and writes 
    if( ! count ) return 0;                         // If there are no input samples, return immediately
    executionState.pInput = pInput;                 // Pointers to the input and output buffers that each call to filterBiquad() will use
    executionState.pOutput = pOutput;               // - pInput and pOutput can be equal, allowing reuse of the same memory.
    executionState.count = count;                   // The number of samples to be processed
    executionState.pState = pThis->state;                   // Pointer to the biquad's internal state and coefficients. 
    executionState.pCoefficients = filter1_coefficients;    // Each call to filterBiquad() will advance pState and pCoefficients to the next biquad

    // The 1st call to filter1_filterBiquad() reads from the caller supplied input buffer and writes to the output buffer.
    // The remaining calls to filterBiquad() recycle the same output buffer, so that multiple intermediate buffers are not required.

    filter1_filterBiquad( &executionState );        // Run biquad #0
    executionState.pInput = executionState.pOutput;         // The remaining biquads will now re-use the same output buffer.

    filter1_filterBiquad( &executionState );        // Run biquad #1

    filter1_filterBiquad( &executionState );        // Run biquad #2

    filter1_filterBiquad( &executionState );        // Run biquad #3

    // At this point, the caller-supplied output buffer will contain the filtered samples and the input buffer will contain the unmodified input samples.  
    return count;       // Return the number of samples processed, the same as the number of input samples

}

 void filter1_filterBiquad( filter1_executionState * pExecState )
{
    // Read state variables
    float w0, x0;
    float w1 = pExecState->pState[0];
    float w2 = pExecState->pState[1];

    // Read coefficients into work registers
    float b0 = *(pExecState->pCoefficients++);
    float b1 = *(pExecState->pCoefficients++);
    float b2 = *(pExecState->pCoefficients++);
    float a1 = *(pExecState->pCoefficients++);
    float a2 = *(pExecState->pCoefficients++);

    // Read source and target pointers
    float *pInput  = pExecState->pInput;
    float *pOutput = pExecState->pOutput;
    short count = pExecState->count;
    float accumulator;

    // Loop for all samples in the input buffer
    while( count-- )
    {
        // Read input sample
        x0 = *(pInput++);
    
        // Run feedback part of filter
        accumulator  = w2 * a2;
        accumulator += w1 * a1;
        accumulator += x0 ;

        w0 = accumulator ;
    
        // Run feedforward part of filter
        accumulator  = w0 * b0;
        accumulator += w1 * b1;
        accumulator += w2 * b2;

        w2 = w1;        // Shuffle history buffer
        w1 = w0;

        // Write output
        *(pOutput++) = accumulator ;
    }

    // Write state variables
    *(pExecState->pState++) = w1;
    *(pExecState->pState++) = w2;
}