LPS.h

00001 #ifndef LPS_H
00002 #define LPS_H
00003 
00004 #include "mbed.h"
00005 
00006 typedef struct _3D_Vector{    
00007     float x;
00008     float y;
00009     float z;   
00010 } __3D_Vector;
00011 
00012 class LPS {
00013     public:
00014         // Eventually use this to setup pin interconnects to Basestation
00015         LPS(PinName MOSI, PinName MISO, PinName SCLK);
00016         ~LPS();
00017     
00018         void calibratePosition(float iCal, float dCal, float jCal);
00019     
00020         void setHome(_3D_Vector home);
00021         
00022         void updateLocation();
00023     
00024         float getCurrentX();
00025         float getCurrentY();
00026         float getCurrentZ();
00027         _3D_Vector getCurrentLocation();  
00028         
00029         _3D_Vector getBeacon_1_Location();
00030         _3D_Vector getBeacon_2_Location();
00031         _3D_Vector getBeacon_3_Location();    
00032         
00033         // debug functions 
00034         _3D_Vector getUnitX();
00035         _3D_Vector getUnitY();
00036         _3D_Vector getUnitZ();
00037         float geti();
00038         float getd();
00039         float getj();
00040     private:
00041         SPI _spi;
00042         
00043         // Bunch of storage locations for SPI transfers
00044         int received_1;
00045         int received_2;
00046         int received_3;
00047         /*
00048          The three displacemnet values corrosponding to the calibration positions
00049             - ALL MOST BE Z=0
00050             - ONE MUST BE ORIGIN (ALREADY ASSUMED)
00051             - ONE MUST BE ON X_AXIS
00052             
00053             Cal Point 1 is origin (0,0,0)
00054             theta_1 : x-coordinate of Cal Point 2
00055             theta_2 : x-coordinate of Cal Point 3
00056             theta_3 : y-coordinate of Cal Point 3
00057             
00058             i.e.
00059             Cal Point 1 : {0,0,0}
00060             Cal Point 2 : {theta_1,0,0}
00061             Cal Point 3 : {theta_2,theta_3,0}        
00062         */
00063         // i,d and j
00064         float i,d,j;
00065         
00066         // Unit Vector
00067         _3D_Vector unitX, unitY, unitZ;
00068         
00069         // The received distances from the beacons
00070         float beacon_1_distance;
00071         float beacon_2_distance;
00072         float beacon_3_distance;
00073         
00074         // The computed location of all three beacons
00075         _3D_Vector beacon_1_loc;
00076         _3D_Vector beacon_2_loc;
00077         _3D_Vector beacon_3_loc;
00078         
00079         // Home location
00080         _3D_Vector home;
00081         // Current location, two possible locations, still need to determine if i can narrow these down analytically,
00082         // currently, current_1 contains value closes to z = 0;
00083         _3D_Vector current_1;
00084         _3D_Vector current_2;
00085         
00086         void updateDistances(); 
00087         
00088         int fetchTimeOverSPI(int inst);
00089         
00090         // Special function used for reading in and storing all 9 distances needed for calibration, abuse _3D_Vector to optimise memory
00091         void updateCalibrationDistances();
00092         
00093         // Calculate all the necessary unitX, unitY, unitZ, i, d, and j values
00094         void calcUnitVectorsAndScalars();
00095         
00096 };
00097 
00098          
00099 // Calculates the coordinates based off of received timings/Calibration mode, values are dependent on mode (Calibrate or Acquire)
00100 // Passing by reference just saves a bit of memery (and time?)
00101 float calcX(float t1, float t2, float d);
00102 float calcY(float t1, float t3, float i, float j, float x);
00103 float calcZ(float t1, float x, float y);
00104 
00105 // Static functions that don't belong to the object
00106 _3D_Vector addFourVectors(_3D_Vector a, _3D_Vector b, _3D_Vector c, _3D_Vector d);
00107 _3D_Vector scaleVector(_3D_Vector a, float scale);
00108 _3D_Vector unitVector(_3D_Vector a);
00109 
00110 float dot_Product(_3D_Vector a, _3D_Vector b);
00111 float vectorMagnitude(_3D_Vector a);
00112 _3D_Vector cross_Product(_3D_Vector a, _3D_Vector b);  
00113 _3D_Vector subTwoVectors(_3D_Vector a, _3D_Vector b);
00114 #endif