Diff: FrequencyFinder.cpp

Revision:

0:78c2ee8ea962

diff -r 000000000000 -r 78c2ee8ea962 FrequencyFinder.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FrequencyFinder.cpp	Sun Nov 25 23:10:33 2012 +0000
@@ -0,0 +1,107 @@
+#include "FrequencyFinder.h"
+
+using namespace std;
+
+FrequencyFinder::FrequencyFinder(PinName input) : _signal_in(input), _dc_offset(p18) {
+    index=0;
+    peak=0;
+    frequency_final=0;
+}
+
+FrequencyFinder::~FrequencyFinder() {
+    ticker.detach();
+}
+
+float FrequencyFinder::find_frequency() {
+    initialize_array();
+    ticker.attach_us(this, &FrequencyFinder::get_data,500);
+
+    wait(1);
+    ticker.detach();
+
+    copy_data();
+
+    vRealFFT(signal_array,size);
+
+    take_abs();
+
+    int peak = find_peak();
+
+    float frequency_final=((float)peak)/4.1;
+
+    return frequency_final;
+}
+
+void FrequencyFinder::get_data() {
+    if (index<size) {
+        signal_array[index]=_signal_in.read();//-0.5;
+        index++;
+    }
+}
+
+void FrequencyFinder::take_abs() {
+//takes the absolute value of the signal
+    for (int j=0; j<size; j++) {
+        signal_array[j]=abs(signal_array[j]);
+        if (j<200)//~48Hz
+            signal_array[j]=0;
+        else if (j<400)//~97Hz
+            signal_array[j]=signal_array[j]*3;
+        else if (j<550)//~
+            signal_array[j]=signal_array[j]*6;
+        else if (j<900)//~219Hz
+            signal_array[j]=signal_array[j]*10;
+        else if (j<1200)//292Hz
+            signal_array[j]=signal_array[j]*12;
+        else
+            signal_array[j]=signal_array[j]*30;
+    }
+}
+
+int FrequencyFinder::find_peak() {
+    int i=0;
+
+    for (int j=0; j<size; j++) {
+        if ((signal_array[i]-signal_array[i-1])>100 ) {
+            while (
+                (signal_array[i]<=signal_array[i+1]) &&
+                (signal_array[i]<=signal_array[i+2]) &&
+                (signal_array[i]<=signal_array[i+3]) &&
+                (signal_array[i]<=signal_array[i+4])) {
+                i++;
+            }
+            return FrequencyFinder::find_max(i);
+        }
+        i++;
+    }
+    return FrequencyFinder::find_max(i);
+}
+
+int FrequencyFinder::find_max(int position) {
+    int index1=position-20;
+    int index2=position+20;
+    float max=signal_array[index1];
+    int max_index=0;
+
+    for (int a=index1; a<index2; a++) {
+        if (signal_array[a]>max) {
+            max=signal_array[a];
+            max_index=a;
+        }
+    }
+
+    return max_index;
+}
+
+void FrequencyFinder::initialize_array() {
+    index=0;
+    for (int w=0; w<size; w++) {
+        signal_array[w]=0;
+    }
+}
+
+void FrequencyFinder::copy_data() {
+    for (int i=0; i<size/2; i++) {
+        signal_array[i+(size/2)]=signal_array[i];
+    }
+}
\ No newline at end of file