Revision 0:adae25491b93, committed 2016-04-18

Comitter:

mikeb

Date:

Mon Apr 18 01:14:36 2016 +0000

Child:

1:4fceb43e2dd3

Commit message:

1st Draft. Some accuracy errors when close to sensor axis. ; Needs a filter and a method to process the 2 sensors. ; Filter is a must.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AoA_Est.h	Mon Apr 18 01:14:36 2016 +0000
@@ -0,0 +1,128 @@
+
+#include "mbed.h"
+#include <cmath>
+
+const int MAX_SENSORS = 10;
+const int ITERATIONS = 2;
+/** A
+*      }
+* }
+* @endcode
+*/
+
+class AoA_Est {
+
+public:
+    /** Create a
+    *
+    * @param _pin mbed AnalogIn pin where the analog output of sensor is connected
+    *
+    * @note Supported types of sensors:
+    */
+    AoA_Est(int numOfSensors, int xPassed[], int yPassed[], float freq);
+    float estimate(float phases[], float amp[]);
+    bool calibrate();
+
+
+    int confidence;
+
+private:
+    float estimate_Theoretical(float phases[], float amp[]);
+    float estimate_Calibrated(float phases[], float amp[]);
+    void comparative_Phases(float phas[]);
+    float angle_Resolver();
+    float distanceFinder(float phase);
+    int sensors;
+
+    int x[MAX_SENSORS];
+    int y[MAX_SENSORS];
+    float phases[MAX_SENSORS - 1];
+    float sensorSep[MAX_SENSORS];
+    float sensorAngles[MAX_SENSORS];
+    float amplitudes[MAX_SENSORS];
+    int z[2];
+    float ambigAngles[2][MAX_SENSORS];
+    float wavelength;
+
+};
+AoA_Est::AoA_Est(int numOfSensors, int xPassed[], int yPassed[], float freq) : sensors(numOfSensors)
+{
+    wavelength = (338.4 / freq)*1000;
+    for (short i = 0; i < sensors-1; i++) {
+        x[i] = xPassed[i];
+        y[i] = yPassed[i];
+        sensorSep[i] = sqrt(float(xPassed[i] * x[i]) + float(yPassed[i] * y[i]));
+        sensorAngles[i] = atan2(float(yPassed[i]), float(xPassed[i]))*180/3.1415926535;
+    }
+}
+
+float AoA_Est::distanceFinder(float phase) {
+    return phase / 360 * wavelength;
+}
+
+float AoA_Est::estimate_Theoretical(float phases[], float amp[]) {
+    float distance = 0;
+    float angle = 0;
+
+    for (int i = 0; i < sensors-1; i++) {
+        distance = distanceFinder(phases[i]);
+        angle = acos(distance / sensorSep[i])*180/3.1415923535;
+        ambigAngles[0][i] = sensorAngles[i] - angle; //Potentially swap +/-
+        ambigAngles[1][i] = sensorAngles[i] + angle;
+    //  if (distance > 0) {
+            //ambigAngles[0][i] = (int(ambigAngles[0][i]) + 180) % 360;  //Check
+            //ambigAngles[1][i] = (int(ambigAngles[1][i]) + 180) % 360; //Not sure
+    //      ambigAngles[0][i] = angle - sensorAngles[i];
+    //      ambigAngles[1][i] = sensorAngles[i] + angle;
+    //  }
+        ambigAngles[0][i] = (ambigAngles[0][i] < 0) ? ambigAngles[0][i] + 360 : ambigAngles[0][i];
+        ambigAngles[1][i] = (ambigAngles[1][i] < 0) ? ambigAngles[1][i] + 360 : ambigAngles[1][i];
+    }
+    angle = angle_Resolver();
+    return angle;
+}
+
+float AoA_Est::angle_Resolver() {
+    float angle = ambigAngles[0][0];
+    float avg = angle;
+    bool flag = false;
+    confidence = 0;
+    for (short i = 1; i <= ITERATIONS; i++) {
+        if (abs(angle - ambigAngles[0][i]) < 30) {
+            angle = ambigAngles[0][i];
+            avg += ambigAngles[0][i];
+            confidence++;
+        }
+        else if (abs(angle - ambigAngles[1][i]) < 30) {
+            angle = ambigAngles[1][i];
+            avg += ambigAngles[1][i];
+            confidence++;
+        }
+        else if (i == 1 && flag == false) {
+            angle = ambigAngles[1][0];
+            avg = angle;
+            i = 0;
+            flag = true;
+        }
+
+    }
+
+    return avg / (ITERATIONS);//change when comute the other way
+}
+
+void AoA_Est :: comparative_Phases(float phas[]) {
+    for (int i = 0; i < sensors - 1; i++) {
+        phases[i] = phas[0] - phas[i + 1];
+
+    }
+
+}
+
+float AoA_Est::estimate(float phas[], float amplitudes[]) {
+    float angle;
+    comparative_Phases(phas);
+    angle = estimate_Theoretical(phases, amplitudes);
+
+    return angle;
+
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Phase_Finder.h	Mon Apr 18 01:14:36 2016 +0000
@@ -0,0 +1,86 @@
+#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[]) {
+
+    
+    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();
+
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Apr 18 01:14:36 2016 +0000
@@ -0,0 +1,15 @@
+#include "mbed.h"
+#include "AoA_Est.h"
+#include "Phase_Finder.h"
+
+int main() {
+    int x[2] = { 95, -82 };
+    int y[2] = { 77, -110 };
+    Phase_Finder phase(50000, 900);
+    Phase_Finder phase2(50000, 900);
+    Phase_Finder phase3(50000, 900);
+    AoA_Est AoA(2, x, y, 900); 
+    while(1) {
+        
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Mon Apr 18 01:14:36 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/99a22ba036c9
\ No newline at end of file