Diff: main.cpp

Revision:

0:c9ec02922858

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Jul 16 18:40:15 2018 +0000
@@ -0,0 +1,64 @@
+ #include    "mbed.h"
+ #include    "math.h"
+ #include    "BNO055.h"
+ 
+ #define PI 3.141593
+ 
+ Serial pc(USBTX,USBRX);
+ BNO055 imu(PTE25, PTE24);         // SDA, SCL 
+ 
+ double y;
+ double x;
+ double z;
+ double angleToNorth;
+ double result;
+// BNO055_ID_INF_TypeDef bno055_id_inf;
+// BNO055_EULER_TypeDef  euler_angles;
+ 
+ int main() {
+//     setup();
+     imu.reset();
+     pc.printf("Bosch Sensortec BNO055 test program on \r\n");//" __DATE__ "/" __TIME__ "\r\n");
+     while (imu.check() == 0){
+         pc.printf("Bosch BNO055 is NOT available!!\r\n");
+         wait(.5);
+     }
+ //   pc.printf("Bosch Sensortec BNO055 available \r\n");//" __DATE__ "/" __TIME__ "\r\n");
+    pc.printf("BNO055 found\r\n\r\n");
+    pc.printf("Chip          ID: %0z\r\n",imu.ID.id);
+    pc.printf("Accelerometer ID: %0z\r\n",imu.ID.accel);
+    pc.printf("Gyroscope     ID: %0z\r\n",imu.ID.gyro);
+    pc.printf("Magnetometer  ID: %0z\r\n\r\n",imu.ID.mag);
+    pc.printf("Firmware version v%d.%0d\r\n",imu.ID.sw[0],imu.ID.sw[1]);
+    pc.printf("Bootloader version v%d\r\n\r\n",imu.ID.bootload);
+    imu.set_accel_units(MPERSPERS);
+    imu.setmode(OPERATION_MODE_AMG);
+    imu.read_calibration_data();
+    imu.write_calibration_data();
+    imu.set_angle_units(DEGREES);
+    while(1)
+    {
+        imu.setmode(OPERATION_MODE_AMG);
+        imu.get_accel();
+        pc.printf("acceleration: x %f\ty %f\tz %f \r\n", imu.accel.x, imu.accel.y+.25, imu.accel.z);
+        imu.get_gyro();
+        pc.printf("gyroscope: x %f\ty %f\tz %f \r\n", imu.gyro.x, imu.gyro.y, imu.gyro.z);
+        imu.get_mag();
+        pc.printf("magnometer: x %f\ty %f\t %f  \r\n", imu.mag.x, imu.mag.y, imu.mag.z);
+        x = imu.mag.x;
+        y = imu.mag.y;
+        
+        result = x/y;
+        
+        if(imu.mag.y>0)
+            angleToNorth = 90.0 - atan(result)*180/PI;        
+        else if(imu.mag.y<0)
+            angleToNorth = 270.0 - atan(result)*180/PI;
+        else if(y == 0 && x <= 0)
+            angleToNorth = 180;
+        else if(y == 0 && x > 0)
+            angleToNorth = 0;        
+        pc.printf("it is pointing %f angle from north \r\n", angleToNorth); 
+        wait(1);
+    }
+}
\ No newline at end of file