Changjun Park / Mbed 2 deprecated ECE4180Project--Slave2

slave2

Dependencies:   PinDetect dsp mbed-rtos mbed

Phase_Finder.h@2:919431d2e1a7, 2016-04-25 (annotated)

Committer:
cpark308
Date:
Mon Apr 25 21:42:00 2016 +0000
Revision:
2:919431d2e1a7
Child:
3:18d0a7397658
mbed slave2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cpark308 2:919431d2e1a7 1 #include <mbed.h>
cpark308 2:919431d2e1a7 2 using namespace std;
cpark308 2:919431d2e1a7 3 const int SAMPLE_LENGTH = 251;
cpark308 2:919431d2e1a7 4 const int PEAKS = 4;
cpark308 2:919431d2e1a7 5
cpark308 2:919431d2e1a7 6 /** A
cpark308 2:919431d2e1a7 7 * }
cpark308 2:919431d2e1a7 8 * }
cpark308 2:919431d2e1a7 9 * @endcode
cpark308 2:919431d2e1a7 10 */
cpark308 2:919431d2e1a7 11
cpark308 2:919431d2e1a7 12 class Phase_Finder {
cpark308 2:919431d2e1a7 13
cpark308 2:919431d2e1a7 14 public:
cpark308 2:919431d2e1a7 15 /** Create a
cpark308 2:919431d2e1a7 16 *
cpark308 2:919431d2e1a7 17 * @param _pin mbed AnalogIn pin where the analog output of sensor is connected
cpark308 2:919431d2e1a7 18 *
cpark308 2:919431d2e1a7 19 * @note Supported types of sensors:
cpark308 2:919431d2e1a7 20 */
cpark308 2:919431d2e1a7 21 Phase_Finder(int sampleRate, float frequency);
cpark308 2:919431d2e1a7 22 float estimate(float samples[], int leng);
cpark308 2:919431d2e1a7 23
cpark308 2:919431d2e1a7 24
cpark308 2:919431d2e1a7 25 private:
cpark308 2:919431d2e1a7 26 float est_Phase();
cpark308 2:919431d2e1a7 27 void est_Max(float samples[]);
cpark308 2:919431d2e1a7 28 float wavelength;
cpark308 2:919431d2e1a7 29 float frequency;
cpark308 2:919431d2e1a7 30 int sampleRate;
cpark308 2:919431d2e1a7 31 int indices1[PEAKS];
cpark308 2:919431d2e1a7 32 int length;
cpark308 2:919431d2e1a7 33 int peaks;
cpark308 2:919431d2e1a7 34 float phase;
cpark308 2:919431d2e1a7 35
cpark308 2:919431d2e1a7 36 };
cpark308 2:919431d2e1a7 37 Phase_Finder::Phase_Finder(int nsampleRate, float freq) : sampleRate(nsampleRate), frequency(freq)
cpark308 2:919431d2e1a7 38 {
cpark308 2:919431d2e1a7 39 //wavelength = (338.4/freq);
cpark308 2:919431d2e1a7 40 }
cpark308 2:919431d2e1a7 41
cpark308 2:919431d2e1a7 42 void Phase_Finder::est_Max(float samples1[]) {
cpark308 2:919431d2e1a7 43
cpark308 2:919431d2e1a7 44
cpark308 2:919431d2e1a7 45 for (int j = 0; j<peaks; j++) {
cpark308 2:919431d2e1a7 46 float max = 0;
cpark308 2:919431d2e1a7 47
cpark308 2:919431d2e1a7 48 for (int i = j*ceil(sampleRate/frequency); i< (j+1)*ceil(sampleRate/frequency); i++) {
cpark308 2:919431d2e1a7 49 if (max < samples1[i]) {
cpark308 2:919431d2e1a7 50 max = samples1[i];
cpark308 2:919431d2e1a7 51 indices1[j] = i;
cpark308 2:919431d2e1a7 52 }
cpark308 2:919431d2e1a7 53 }
cpark308 2:919431d2e1a7 54 if (indices1[j] - ceil(sampleRate / frequency)/2 >= 0) {
cpark308 2:919431d2e1a7 55 for (int i = -ceil(sampleRate / frequency)/2; i< ceil(sampleRate/frequency) / 2; i++) {
cpark308 2:919431d2e1a7 56 samples1[indices1[j] + i] = 0;
cpark308 2:919431d2e1a7 57 }
cpark308 2:919431d2e1a7 58 }
cpark308 2:919431d2e1a7 59 else {
cpark308 2:919431d2e1a7 60 for (int i = 0; i< indices1[j] + ceil(sampleRate / frequency) / 2; i++) {
cpark308 2:919431d2e1a7 61 samples1[indices1[j] + i] = 0;
cpark308 2:919431d2e1a7 62 }
cpark308 2:919431d2e1a7 63 }
cpark308 2:919431d2e1a7 64
cpark308 2:919431d2e1a7 65 }
cpark308 2:919431d2e1a7 66 }
cpark308 2:919431d2e1a7 67
cpark308 2:919431d2e1a7 68
cpark308 2:919431d2e1a7 69 float Phase_Finder::est_Phase() {
cpark308 2:919431d2e1a7 70 float avgDist = 0;
cpark308 2:919431d2e1a7 71 float ph;
cpark308 2:919431d2e1a7 72 for (int i = 0; i<peaks - 1; i++){
cpark308 2:919431d2e1a7 73 avgDist += indices1[i] - ceil(sampleRate/frequency*i);
cpark308 2:919431d2e1a7 74 }
cpark308 2:919431d2e1a7 75 avgDist = avgDist / (peaks- 1);
cpark308 2:919431d2e1a7 76 ph = avgDist / float(sampleRate) * float(frequency) *float(360);
cpark308 2:919431d2e1a7 77 return ph;
cpark308 2:919431d2e1a7 78 }
cpark308 2:919431d2e1a7 79
cpark308 2:919431d2e1a7 80 float Phase_Finder::estimate(float sampl[], int leng) {
cpark308 2:919431d2e1a7 81 length = leng;
cpark308 2:919431d2e1a7 82 peaks = floor(frequency / sampleRate*length);
cpark308 2:919431d2e1a7 83 est_Max(sampl);
cpark308 2:919431d2e1a7 84 return est_Phase();
cpark308 2:919431d2e1a7 85
cpark308 2:919431d2e1a7 86 }