File content as of revision 8:aaf5cde0aa0a:

#include <mbed.h>
using namespace std;
const int SAMPLE_LENGTH = 251;
const int PEAKS = 4;

/** A
*      }
* }
* @endcode
*/

class Phase_Finder {

public:
    /** Create a
    *
    * @param _pin mbed AnalogIn pin where the analog output of sensor is connected
    *
    * @note Supported types of sensors:
    */
    Phase_Finder(int sampleRate, float frequency);
    float estimate(float samples[], int leng);


private:
    float est_Phase();
    void est_Max(float samples[]);
    float wavelength;
    float frequency;
    int sampleRate;
    int indices1[PEAKS];
    int length;
    int peaks;
    float phase;

};
Phase_Finder::Phase_Finder(int nsampleRate, float freq) : sampleRate(nsampleRate), frequency(freq)
{
    //wavelength = (338.4/freq);
}

void Phase_Finder::est_Max(float samples1[]) {
    float change = 0;
    
    for (int i = 2; i < length - 1; i++) {
        change = abs(samples1[i - 2] - samples1[i - 1]);
        if (abs(samples1[i] - samples1[i-1]) > change*4.5)
            samples1[i] = (samples1[i - 1] + samples1[i + 1]) / 2;
    }
    
    for (int j = 0; j<peaks; j++) {
        float max = 0;
        
        for (int i = j*ceil(sampleRate/frequency); i< (j+1)*ceil(sampleRate/frequency); i++) {
            if (max < samples1[i]) {
                max = samples1[i];
                indices1[j] = i;
            }
        }
        if (indices1[j] - ceil(sampleRate / frequency)/2 >= 0) {
            for (int i = -ceil(sampleRate / frequency)/2; i< ceil(sampleRate/frequency) / 2; i++) {
                samples1[indices1[j] + i] = 0;
            }
        }
        else {
            for (int i = 0; i< indices1[j] + ceil(sampleRate / frequency) / 2; i++) {
                samples1[indices1[j] + i] = 0;
            }
        }
        
    }
}


float Phase_Finder::est_Phase() {
    float avgDist = 0;
    float ph;
    for (int i = 0; i<peaks - 1; i++){
        avgDist += indices1[i] - ceil(sampleRate/frequency*i);
    }
    avgDist = avgDist / (peaks- 1);
        ph = avgDist / float(sampleRate) * float(frequency) *float(360);
        return ph;
}

float Phase_Finder::estimate(float  sampl[], int leng) {
    length = leng;
    peaks = floor(frequency / sampleRate*length);
    est_Max(sampl);
        return est_Phase();

}