File content as of revision 6:7f6e2d293154:

#include "signalnumber.h"
#include "mbed.h"


//Constants for mean value
const int n = 500;                   //Window size for the mean value
float emg0_filtered[n] = {}, emg2_filtered[n] = {}, emg4_filtered[n]={}; 
int count1 = 0, count2 = 0, count3 = 0;  //Counters for the mean values
int countx = 0, county = 0.0, countz = 0.0;     //Counters for the signalnumbers
float meanL = 0.0, meanR = 0.0, meanM = 0.0;    //Internal function calculation variables

//Constants EMG switch
const float LeftMin = 0.20;
const float LeftMax = 0.50;
int NumberL = 0, NumberR =0;


SignalNumber::SignalNumber(void)
{

}

//Determine the Left mean value
//"n" is the amount off samples over which the mean value is calculated
//"ipnut" is the inputsignal (filtered analog in)
float SignalNumber::getmeanLeft(const int n, float input){
    emg0_filtered[count1] = input;
    count1++;
    if (count1 == n){
        count1 = 0;
    }
    float sum_math = 0.0;
    for (int m=0 ; m<n ; m++ ){
        sum_math = sum_math + emg0_filtered[m];
    }
    float mean_math = sum_math/n;
    return mean_math;
}

//Determine the Right mean value
//"n" is the amount off samples over which the mean value is calculated
//"ipnut" is the inputsignal (filtered analog in)
float SignalNumber::getmeanRight(const int n, float input){
    emg2_filtered[count2] = input;
    count2++;
    if (count2 == n){
        count2 = 0;
    }
    float sum_math = 0.0;
    for (int m=0 ; m<n ; m++ ){
        sum_math = sum_math + emg2_filtered[m];
    }
    float mean_math = sum_math/n;
    return mean_math;
}

/*
//Determine the Mode mean value
//"n" is the amount off samples over which the mean value is calculated
//"ipnut" is the inputsignal (filtered analog in)
float SignalNumber::getmeanMode(const int n, float input){
    emg4_filtered[count3] = input;
    count3++;
    if (count3 == n){
        count3 = 0;
    }
    float sum_math = 0.0;
    for (int m=0 ; m<n ; m++ ){
        sum_math = sum_math + emg4_filtered[m];
    }
    float mean_math = sum_math/n;
    return mean_math;
}
*/

// Determine the offset by calculating the mean value for "n" samples
float SignalNumber::calibrate(const int n, float input){
    float offset = getmeanLeft(n, input);
    return offset; 
} 

int SignalNumber::getnumberLeft(const int n, const int action, float input){
    meanL = getmeanLeft(n, input);
    //Check first case, standstill
    if( meanL < LeftMin ) {
        if (county < action){
            meanL = getmeanLeft(n, input);
            if(meanL < LeftMin){
                county++;
            }
            else{
                county = 0;
                NumberL = 0;
            } 
        }
        else{
            NumberL = 0;
            county = 0;
        }
    }
     //Check second case, move slow  
     else if(meanL >= LeftMin and meanL < LeftMax){
        if (county <action){
            meanL= getmeanLeft(n, input);
            if(meanL >= LeftMin and meanL < LeftMax){
                county++;
            }
            else{
                county=0;
                NumberL=0;
            } 
        }
        else{
            NumberL = 1;
            county=0;
        }
    }
    //Check third case, move fast
     else if( meanL>=LeftMax) {
        if (county <action){
            meanL= getmeanLeft(n, input);
            if(meanL>=LeftMax){
                county++;
            }
            else{
                county=0;
                NumberL=0;
            } 
        }
        else{
            NumberL = 2;
            county=0;
        }
    }
    //If not working output zero
    else{
        county=0;
        NumberL =0;
    }
    return NumberL;
}       

int SignalNumber::getnumberRight(const int n, const int action, float input){
    meanR= getmeanRight(n, input);
    //Check first case, standstill
    if( meanR< LeftMin ) {
        if (countx <action){
            meanR= getmeanRight(n, input);
            if(meanR< LeftMin){
                countx++;
            }
            else{
                countx=0;
                NumberR=0;
            } 
        }
        else{
            NumberR = 0;
            countx=0;
        }
    }
     //Check second case, move slow 
     else if(meanR >= LeftMin and meanR < LeftMax){
        if (countx <action){
            meanR= getmeanRight(n, input);
            if(meanR >= LeftMin and meanR < LeftMax){
                countx++;
            }
            else{
                countx=0;
                NumberR=0;
            } 
        }
        else{
            NumberR = 1;
            countx=0;
        }
    }
    //Check third case, move fast
     else if( meanR >=LeftMax ) {
        if (countx <action){
            meanR= getmeanRight(n, input);
            if( meanR >=LeftMax ){
                countx++;
            }
            else{
                countx=0;
                NumberR=0;
            } 
        }
        else{
            NumberR = 2;
            countx=0;
        }
    }
    //If not working output zero
    else{
        countx=0;
        NumberR =0;
    }
    return NumberR;
}