Diff: main.cpp

Revision:

3:b65b394375b7

Parent:

2:c91ce1f091f5

Child:

4:104acd180bb1

--- a/main.cpp	Mon Nov 28 18:45:08 2016 +0000
+++ b/main.cpp	Tue Nov 29 03:37:35 2016 +0000
@@ -1,4 +1,29 @@
 #include "mbed.h"
+#include "LSM9DS1.h"
+#define PI 3.14159
+
+//IMU used for monitoring orientation
+// - wired using i2c in cookbook example here
+//   https://developer.mbed.org/components/LSM9DS1-IMU/
+LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
+
+//pc serial connection, for debugging purposes
+Serial pc(USBTX, USBRX);
+
+//initialize and calibrate IMU
+void initIMU(){
+    IMU.begin();
+    if (!IMU.begin()) {
+        pc.printf("Failed to communicate with LSM9DS1.\n");
+    }
+    IMU.calibrate(1);
+    IMU.calibrateMag(0);   
+}
+
+//initialization for all components
+void init(){
+    initIMU();    
+}
 
 // Sends a request to the server and waits for a path response
 char* getPathFromServer(){
@@ -10,12 +35,26 @@
 
 //gets the orientation from imu
 float getDirection(){
-    ///tests    
+    float sum = 0;
+    int target_samples = 100;
+    int num_samples = 0;
+    while(num_samples++ < target_samples){
+        while(!IMU.magAvailable(X_AXIS));
+            IMU.readMag();
+        float my = IMU.calcMag(IMU.my);
+        float mx = IMU.calcMag(IMU.mx);
+        float dir;
+        if (my == 0.0)
+            dir = (mx < 0.0) ? 180.0 : 0.0;
+        else
+            dir = atan2(mx, my)*360.0/(2.0*PI);
+        sum += dir;
+    }
+    return sum / target_samples;
 }
 
 //technologies for navigation may include
 // - Line following https://www.sparkfun.com/products/11769
-// - IMU https://developer.mbed.org/cookbook/Stinger-Robot-Library
 // - color sensor (indicates when end of movement is reached)
 void moveForward(){}
 void rotateLeft(){}
@@ -34,12 +73,15 @@
             case 'L': rotateLeft(); break;
             case 'R': rotateRight(); break;
             case 'U': armUp(); break;
-            case 'U': armDown(); break;
+            case 'D': armDown(); break;
         }    
     }
 }
 
+//main code
+//- repeatedly listens for paths and then executes them
 int main() {
+    init();
     while(true){
         char* path = getPathFromServer(); 
         executePath(path);