Diff: main.cpp

Revision:

3:70f624c5fe26

Parent:

2:fcf6f5901ee6

Child:

4:18e9f16cc53c

--- a/main.cpp	Sun Apr 10 21:23:17 2016 +0000
+++ b/main.cpp	Sun Apr 10 22:01:21 2016 +0000
@@ -1,11 +1,65 @@
 #include "mbed.h"
 #include "rtos.h"
+#include "LSM9DS1.h"
+#define DECLINATION -4.94
+#define PI 3.14159
+
 
 AnalogIn infraredR(p20); //Right infrared distance sensor
 AnalogIn infraredL(p19); //Left infrared distance sensor
 AnalogIn infraredF(p17); //Front infrared distance sensor
+Serial pc(USBTX, USBRX);
 
 float leftDist, rightDist, frontDist; //Distances from robot to obstacles
+float xaccel, yaccel, zaccel;   //Acceleration in the x, y, and z directions
+float xmag, ymag, zmag, head;         //Magnetic readings
+
+void printAttitude(float ax, float ay, float az, float mx, float my, float mz)
+{
+    float roll = atan2(ay, az);
+    float pitch = atan2(-ax, sqrt(ay * ay + az * az));
+// touchy trig stuff to use arctan to get compass heading (scale is 0..360)
+    mx = -mx;
+    float heading;
+    if (my == 0.0)
+        heading = (mx < 0.0) ? 180.0 : 0.0;
+    else
+        heading = atan2(mx, my)*360.0/(2.0*PI);
+    //pc.printf("heading atan=%f \n\r",heading);
+    heading -= DECLINATION; //correct for geo location
+    if(heading>180.0) heading = heading - 360.0;
+    else if(heading<-180.0) heading = 360.0 + heading;
+    else if(heading<0.0) heading = 360.0  + heading;
+
+
+    // Convert everything from radians to degrees:
+    //heading *= 180.0 / PI;
+    pitch *= 180.0 / PI;
+    roll  *= 180.0 / PI;
+
+    //pc.printf("Pitch: %f,    Roll: %f degress\n\r",pitch,roll);
+    //pc.printf("Magnetic Heading: %f degress\n\r",heading);
+    head = heading;
+}
+
+void IMU_thread(void const *args) {
+    LSM9DS1 IMU(p28, p27, 0xD6, 0x3C);
+    IMU.calibrate(1);
+    IMU.calibrateMag(0);
+    while(1) {
+        while(!IMU.accelAvailable());
+        IMU.readAccel();
+        while(!IMU.magAvailable(X_AXIS));
+        IMU.readMag();
+        xaccel = IMU.calcAccel(IMU.ax);
+        yaccel = IMU.calcAccel(IMU.ay);
+        zaccel = IMU.calcAccel(IMU.az);
+        xmag = IMU.calcMag(IMU.mx);
+        ymag = IMU.calcMag(IMU.my);
+        zmag = IMU.calcMag(IMU.mz);
+        Thread::wait(500); //Wait 1/2 second
+    }    
+}
 
 void left_thread(void const *args) {
     while (1) {