![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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?
User | Revision | Line number | New 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 | } |