Yelfie / LocalPositionSystem

Dependents:   TDP_main_BartFork TDP_main TDP_main TDP_main_fork

Files at this revision

API Documentation at this revision

Comitter:
Joseph_Penikis
Date:
Thu Mar 12 22:12:18 2015 +0000
Parent:
2:21ca29888540
Commit message:
Implemented ISP control, not tested...;

Changed in this revision

LPS.cpp Show annotated file Show diff for this revision Revisions of this file
LPS.h Show annotated file Show diff for this revision Revisions of this file
diff -r 21ca29888540 -r dd68ac680416 LPS.cpp
--- a/LPS.cpp	Wed Mar 11 01:20:56 2015 +0000
+++ b/LPS.cpp	Thu Mar 12 22:12:18 2015 +0000
@@ -2,11 +2,23 @@
 
 // Defines the delay experienced between transmitting IR and Ultrasonic pulse
 #define PING_OFFSET 0
-#define SPEED_OF_SOUND 1.0f
+#define SPEED_OF_SOUND 330.f
 // Used to rectify the issue of measured spheres not intersecting, may not be needed, but available anyway
 #define SWELL_VALUE 0
 
-LPS::LPS() {}
+// Defines for instructions on SPI link, instructions corrpsond to distances under ~20mm, way too close to ever be picked up...
+#define START_CAL 0xFF
+#define REQUEST_T1 0xFE
+#define REQUEST_T2 0xFD 
+#define REQUEST_T3 0xFC
+#define NEXT_CAL_POINT 0xFB
+#define UPPER_BYTE 0x01
+
+LPS::LPS(PinName MOSI, PinName MISO, PinName SCLK) : _spi(MOSI, MISO, SCLK) {
+    // Set up SPI interface for AVR
+    _spi.format(8,3);
+    _spi.frequency(1000000);
+}
 LPS::~LPS() {}
 
 _3D_Vector LPS::getUnitX() { return unitX; }
@@ -88,37 +100,68 @@
     // Calibration complete!!!!!!!
 }
 
-void LPS::updateCalDistances(bool isCalibrate) {
-    // This will be the "socket" for talking to the base station
-    // TODO: Implement...
+int LPS::fetchTimeOverSPI(int instr) {
+     
+    // Loop while avr has not prepared data, i.e. still returning an instruction not data. TODO: Make a fetch only if available option to save on processor time
+    do {
+        received_2 = _spi.write(instr + UPPER_BYTE);    
+    } while (received_2 & 0x80);
     
-    // Use isCalibrate to handle interaction code to allow user to relocate to next calibration point
+    // I have received some data back, the avr is setup to create an identical packet to check data is correct
+    // Send this data back to AVR (Not really used here, but better than using defined instructions)
+    do {
+        // Each time this loop data_1 shifted to r_1, data_2 loaded into r_2, i.e. r_2 ALWAYS has the most recent value
+        received_1 = received_2;
+        received_2 = _spi.write(received_1);
+    } while (received_1 != received_2);
+    // Valid data got (UPPER BYTE)
     
-    // Just dummy values for testing purposes
-    beacon_1_distance = (11.55f - PING_OFFSET) * SPEED_OF_SOUND + SWELL_VALUE;
-    beacon_2_distance = (21.095f - PING_OFFSET) * SPEED_OF_SOUND + SWELL_VALUE;
-    beacon_3_distance = (15.395f - PING_OFFSET) * SPEED_OF_SOUND + SWELL_VALUE;  
+    do {
+        received_3 = _spi.write(instr);    
+    } while (received_3 == instr); 
+    do {
+        // Each time this loop data_1 shifted to r_1, data_2 loaded into r_3, i.e. r_3 ALWAYS has the most recent value
+        received_1 = received_3;
+        received_3 = _spi.write(received_1);
+    } while (received_1 != received_3);
+    // Valid data got (LOWER BYTE)
+    
+    // Generate an integer from these and return
+    return (received_2 << 8) + received_3;
 }
 
 void LPS::updateDistances() {
-    updateCalDistances(false);
+    // This will be the "socket" for talking to the base station    
+    /*
+    beacon_1_distance = (fetchTimeOverSPI(REQUEST_T1) - PING_OFFSET) * SPEED_OF_SOUND + SWELL_VALUE;
+    beacon_2_distance = (fetchTimeOverSPI(REQUEST_T2) - PING_OFFSET) * SPEED_OF_SOUND + SWELL_VALUE;
+    beacon_3_distance = (fetchTimeOverSPI(REQUEST_T3) - PING_OFFSET) * SPEED_OF_SOUND + SWELL_VALUE;
+    */
+    
+    // Just dummy values for testing purposes
+    beacon_1_distance = 11.55f;
+    beacon_2_distance = 21.095f;
+    beacon_3_distance = 15.395f;  
 }
 
 void LPS::updateCalibrationDistances() {
+    // Tell base station to enter calibration mode
+    _spi.write(START_CAL);   
+    
     // Reuse existing function to constrain external accessor code
-    updateCalDistances(true);  
+    updateDistances();  
     
     beacon_2_loc.x = 14.14213562f;//beacon_1_distance;     // 1_t1
     beacon_2_loc.y = 22.49444376f;//beacon_2_distance;     // 1_t2
     beacon_2_loc.z = 12.36931688f;//beacon_3_distance;     // 1_t3
     
-    updateCalDistances(true); 
+    updateDistances(); 
     
     beacon_3_loc.x = 14.45683229f;//beacon_1_distance;     // 2_t1
     beacon_3_loc.y = 21.47091055f;//beacon_2_distance;     // 2_t2
     beacon_3_loc.z = 12.24744871f;//beacon_3_distance;     // 2_t3
     
-    updateCalDistances(true);
+    updateDistances();
     
     beacon_1_distance = 16.673332f;
     beacon_2_distance = 22.36067477f;
diff -r 21ca29888540 -r dd68ac680416 LPS.h
--- a/LPS.h	Wed Mar 11 01:20:56 2015 +0000
+++ b/LPS.h	Thu Mar 12 22:12:18 2015 +0000
@@ -12,7 +12,7 @@
 class LPS {
     public:
         // Eventually use this to setup pin interconnects to Basestation
-        LPS();
+        LPS(PinName MOSI, PinName MISO, PinName SCLK);
         ~LPS();
     
         void calibratePosition(float iCal, float dCal, float jCal);
@@ -38,6 +38,12 @@
         float getd();
         float getj();
     private:
+        SPI _spi;
+        
+        // Bunch of storage locations for SPI transfers
+        int received_1;
+        int received_2;
+        int received_3;
         /*
          The three displacemnet values corrosponding to the calibration positions
             - ALL MOST BE Z=0
@@ -56,6 +62,7 @@
         */
         // i,d and j
         float i,d,j;
+        
         // Unit Vector
         _3D_Vector unitX, unitY, unitZ;
         
@@ -76,9 +83,10 @@
         _3D_Vector current_1;
         _3D_Vector current_2;
         
-        void updateCalDistances(bool isCalibrate);
         void updateDistances(); 
         
+        int fetchTimeOverSPI(int inst);
+        
         // Special function used for reading in and storing all 9 distances needed for calibration, abuse _3D_Vector to optimise memory
         void updateCalibrationDistances();