Changjun Park / Mbed 2 deprecated ECE4180Project--Slave2

slave2

Dependencies:   PinDetect dsp mbed-rtos mbed

Phase_Finder.h@4:e97105ea8f9e, 2016-04-27 (annotated)

Committer:
cpark308
Date:
Wed Apr 27 21:20:20 2016 +0000
Revision:
4:e97105ea8f9e
Parent:
3:18d0a7397658 
Slave 2 code, with auto sampling and dsp tweaks

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 4:e97105ea8f9e 3 const int SAMPLE_LENGTH = 251;
cpark308 4:e97105ea8f9e 4 const int PEAKS = 4;
cpark308 3:18d0a7397658 5
cpark308 2:919431d2e1a7 6 /** A
cpark308 2:919431d2e1a7 7 * }
cpark308 2:919431d2e1a7 8 * }
cpark308 2:919431d2e1a7 9 * @endcode
cpark308 2:919431d2e1a7 10 */
cpark308 3:18d0a7397658 11
cpark308 2:919431d2e1a7 12 class Phase_Finder {
cpark308 3:18d0a7397658 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 3:18d0a7397658 23
cpark308 3:18d0a7397658 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 3:18d0a7397658 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 3:18d0a7397658 41
cpark308 2:919431d2e1a7 42 void Phase_Finder::est_Max(float samples1[]) {
cpark308 3:18d0a7397658 43 float change = 0;
cpark308 3:18d0a7397658 44
cpark308 3:18d0a7397658 45 for (int i = 2; i < length - 1; i++) {
cpark308 3:18d0a7397658 46 change = abs(samples1[i - 2] - samples1[i - 1]);
cpark308 3:18d0a7397658 47 if (abs(samples1[i] - samples1[i-1]) > change*4.5)
cpark308 3:18d0a7397658 48 samples1[i] = (samples1[i - 1] + samples1[i + 1]) / 2;
cpark308 3:18d0a7397658 49 }
cpark308 2:919431d2e1a7 50
cpark308 2:919431d2e1a7 51 for (int j = 0; j<peaks; j++) {
cpark308 2:919431d2e1a7 52 float max = 0;
cpark308 2:919431d2e1a7 53
cpark308 2:919431d2e1a7 54 for (int i = j*ceil(sampleRate/frequency); i< (j+1)*ceil(sampleRate/frequency); i++) {
cpark308 2:919431d2e1a7 55 if (max < samples1[i]) {
cpark308 2:919431d2e1a7 56 max = samples1[i];
cpark308 2:919431d2e1a7 57 indices1[j] = i;
cpark308 2:919431d2e1a7 58 }
cpark308 2:919431d2e1a7 59 }
cpark308 2:919431d2e1a7 60 if (indices1[j] - ceil(sampleRate / frequency)/2 >= 0) {
cpark308 2:919431d2e1a7 61 for (int i = -ceil(sampleRate / frequency)/2; i< ceil(sampleRate/frequency) / 2; i++) {
cpark308 2:919431d2e1a7 62 samples1[indices1[j] + i] = 0;
cpark308 2:919431d2e1a7 63 }
cpark308 2:919431d2e1a7 64 }
cpark308 2:919431d2e1a7 65 else {
cpark308 2:919431d2e1a7 66 for (int i = 0; i< indices1[j] + ceil(sampleRate / frequency) / 2; i++) {
cpark308 2:919431d2e1a7 67 samples1[indices1[j] + i] = 0;
cpark308 2:919431d2e1a7 68 }
cpark308 2:919431d2e1a7 69 }
cpark308 2:919431d2e1a7 70
cpark308 2:919431d2e1a7 71 }
cpark308 2:919431d2e1a7 72 }
cpark308 3:18d0a7397658 73
cpark308 3:18d0a7397658 74
cpark308 2:919431d2e1a7 75 float Phase_Finder::est_Phase() {
cpark308 2:919431d2e1a7 76 float avgDist = 0;
cpark308 2:919431d2e1a7 77 float ph;
cpark308 2:919431d2e1a7 78 for (int i = 0; i<peaks - 1; i++){
cpark308 2:919431d2e1a7 79 avgDist += indices1[i] - ceil(sampleRate/frequency*i);
cpark308 2:919431d2e1a7 80 }
cpark308 2:919431d2e1a7 81 avgDist = avgDist / (peaks- 1);
cpark308 2:919431d2e1a7 82 ph = avgDist / float(sampleRate) * float(frequency) *float(360);
cpark308 2:919431d2e1a7 83 return ph;
cpark308 2:919431d2e1a7 84 }
cpark308 3:18d0a7397658 85
cpark308 2:919431d2e1a7 86 float Phase_Finder::estimate(float sampl[], int leng) {
cpark308 2:919431d2e1a7 87 length = leng;
cpark308 2:919431d2e1a7 88 peaks = floor(frequency / sampleRate*length);
cpark308 2:919431d2e1a7 89 est_Max(sampl);
cpark308 2:919431d2e1a7 90 return est_Phase();
cpark308 3:18d0a7397658 91
cpark308 2:919431d2e1a7 92 }