Diff: LPS.h

Revision:

1:b71a1c39d263

Child:

2:21ca29888540

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LPS.h	Tue Mar 10 00:02:29 2015 +0000
@@ -0,0 +1,107 @@
+#ifndef LPS_H
+#define LPS_H
+
+#include "mbed.h"
+
+typedef struct _3D_Vector{    
+    float x;
+    float y;
+    float z;   
+} __3D_Vector;
+
+class LPS {
+    public:
+        // Eventually use this to setup pin interconnects to Basestation
+        LPS();
+        ~LPS();
+    
+        void calibratePosition(float iCal, float dCal, float jCal);
+    
+        void setHome(_3D_Vector home);
+        
+        void updateLocation();
+    
+        float getCurrentX();
+        float getCurrentY();
+        float getCurrentZ();
+        _3D_Vector getCurrentLocation();  
+        
+        _3D_Vector getBeacon_1_Location();
+        _3D_Vector getBeacon_2_Location();
+        _3D_Vector getBeacon_3_Location();    
+        
+        // debug functions 
+        _3D_Vector getUnitX();
+        _3D_Vector getUnitY();
+        _3D_Vector getUnitZ();
+        float geti();
+        float getd();
+        float getj();
+    private:
+        /*
+         The three displacemnet values corrosponding to the calibration positions
+            - ALL MOST BE Z=0
+            - ONE MUST BE ORIGIN (ALREADY ASSUMED)
+            - ONE MUST BE ON X_AXIS
+            
+            Cal Point 1 is origin (0,0,0)
+            theta_1 : x-coordinate of Cal Point 2
+            theta_2 : x-coordinate of Cal Point 3
+            theta_3 : y-coordinate of Cal Point 3
+            
+            i.e.
+            Cal Point 1 : {0,0,0}
+            Cal Point 2 : {theta_1,0,0}
+            Cal Point 3 : {theta_2,theta_3,0}        
+        */
+        // i,d and j
+        float i,d,j;
+        // Unit Vector
+        _3D_Vector unitX, unitY, unitZ;
+        
+        // The received distances from the beacons
+        float beacon_1_distance;
+        float beacon_2_distance;
+        float beacon_3_distance;
+        
+        // The computed location of all three beacons
+        _3D_Vector beacon_1_loc;
+        _3D_Vector beacon_2_loc;
+        _3D_Vector beacon_3_loc;
+        
+        // Home location
+        _3D_Vector home;
+        // Current location, two possible locations, still need to determine if i can narrow these down analytically,
+        // currently, current_1 contains value closes to z = 0;
+        _3D_Vector current_1;
+        _3D_Vector current_2;
+        
+        void updateCalDistances(bool isCalibrate);
+        void updateDistances(); 
+        
+        // Special function used for reading in and storing all 9 distances needed for calibration, abuse _3D_Vector to optimise memory
+        void updateCalibrationDistances();
+        
+        
+                
+        // Calculates the coordinates based off of received timings/Calibration mode, values are dependent on mode (Calibrate or Acquire)
+        // Passing by reference just saves a bit of memery (and time?)
+        float calcX(float t1, float t2, float d);
+        float calcY(float t1, float t3, float i, float j, float x);
+        float calcZ(float t1, float x, float y);
+        
+        // Calculate all the necessary unitX, unitY, unitZ, i, d, and j values
+        void calcUnitVectorsAndScalars();
+        
+};
+
+// Static functions that don't belong to the object
+_3D_Vector addFourVectors(_3D_Vector a, _3D_Vector b, _3D_Vector c, _3D_Vector d);
+_3D_Vector scaleVector(_3D_Vector a, float scale);
+_3D_Vector unitVector(_3D_Vector a);
+
+float dot_Product(_3D_Vector a, _3D_Vector b);
+float vectorMagnitude(_3D_Vector a);
+_3D_Vector cross_Product(_3D_Vector a, _3D_Vector b);  
+_3D_Vector subTwoVectors(_3D_Vector a, _3D_Vector b);
+#endif