Diff: main.cpp

Revision:

0:ebfdfa4739aa

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Jul 16 18:42:18 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;
+ double pitchAcc;
+ double rollAcc;
+ double pitch = 0;
+ double roll = 0;
+ double yaw = 0;
+ Timer t;
+ 
+ int main() {
+     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("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);
+    t.start();
+    while(1)
+    {
+        imu.setmode(OPERATION_MODE_AMG);
+        imu.get_accel();
+        imu.get_gyro();
+
+        pitch = atan2 (imu.accel.y ,( sqrt ((imu.accel.x * imu.accel.x) + (imu.accel.z * imu.accel.z))));
+        roll = atan2(-imu.accel.x ,( sqrt((imu.accel.y * imu.accel.y) + (imu.accel.z * imu.accel.z))));
+
+
+        roll = roll*57.3;
+        pitch = pitch*57.3;
+
+        yaw = (yaw - t.read()*imu.gyro.z);
+        t.reset();
+        if(yaw > 360)
+            yaw -= 360;
+        if(yaw < 0)
+            yaw += 360;
+
+        pc.printf("Euler angles: x %f\ty %f\t z %f \r\n\n", pitch, roll, yaw);
+    }
+}
\ No newline at end of file