File content as of revision 8:9098c6b1897b:

#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] = {}; 
int count1 = 0, count2 = 0;  //Counters for the mean values
int countx = 0, county = 0.0;     //Counters for the signalnumbers
float meanL = 0.0, meanR = 0.0;    //Internal function calculation variables

//Constants EMG switch
const float MinTreshold = 0.40;
const float MaxTreshold = 0.750;
int SpeedL = 0, SpeedR =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 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::getspeedLeft(const int n, const int action, float input){
    meanL = getmeanLeft(n, input);
    //Check first case, standstill
    if( meanL < MinTreshold ) {
        if (county < action){
            meanL = getmeanLeft(n, input);
            if(meanL < MinTreshold){
                county++;
            }
            else{
                county = 0;
                SpeedL = 0;
            } 
        }
        else{
            SpeedL = 0;
            county = 0;
        }
    }
     //Check second case, move slow  
     else if(meanL >= MinTreshold and meanL < MaxTreshold){
        if (county <action){
            meanL= getmeanLeft(n, input);
            if(meanL >= MinTreshold and meanL < MaxTreshold){
                county++;
            }
            else{
                county=0;
                SpeedL=0;
            } 
        }
        else{
            SpeedL = 1;
            county=0;
        }
    }
    //Check third case, move fast
     else if( meanL>=MaxTreshold) {
        if (county <action){
            meanL= getmeanLeft(n, input);
            if(meanL>=MaxTreshold){
                county++;
            }
            else{
                county=0;
                SpeedL=0;
            } 
        }
        else{
            SpeedL = 2;
            county=0;
        }
    }
    //If not working output zero
    else{
        county=0;
        SpeedL =0;
    }
    return SpeedL;
}       

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