Diff: main.cpp

Revision:

1:4fb66612cb9c

Parent:

0:c9ec02922858

--- a/main.cpp	Mon Jul 16 18:40:15 2018 +0000
+++ b/main.cpp	Fri Aug 03 22:00:54 2018 +0000
@@ -5,7 +5,7 @@
  #define PI 3.141593
  
  Serial pc(USBTX,USBRX);
- BNO055 imu(PTE25, PTE24);         // SDA, SCL 
+ BNO055 imu(D14, D15);         // SDA, SCL 
  
  double y;
  double x;
@@ -14,51 +14,89 @@
  double result;
 // BNO055_ID_INF_TypeDef bno055_id_inf;
 // BNO055_EULER_TypeDef  euler_angles;
+
+ double getAngleToNorth (double x, double y, double result) {
+        if (y > 0) {
+            return 90.0 - atan(result) * 180/PI;  
+        }      
+        else if (y < 0) {
+            return  270.0 - atan(result) * 180/PI;
+        }
+        else if (y == 0 && x <= 0) {
+            return 180;
+        }
+        else if (y == 0 && x > 0) {
+            return 0;
+        }
+} 
  
- 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");
+void getValues() {
+    imu.get_accel();
+    imu.get_gyro();
+    imu.get_mag();
+}
+ 
+void printInfo() {
+    //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);
+}
+ 
+void setup() {
+    imu.reset();
     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;
+}
+
+void printAccel() {
+    pc.printf("acceleration: x %f\ty %f\tz %f \r\n", imu.accel.x, imu.accel.y + .25, imu.accel.z);
+}
+
+void printGyro() {
+    pc.printf("gyroscope: x %f\ty %f\tz %f \r\n", imu.gyro.x, imu.gyro.y, imu.gyro.z);
+}
+
+void printMag() {
+    pc.printf("magnometer { x: %d\t y: %d\t z: %d }\r\n", imu.mag.x, imu.mag.y, imu.mag.z);
+}
+
+void printValues() {
+    printAccel();
+    printGyro();
+    printMag();
+}
+
+void checkAvail() {
+    while (imu.check() == 0){
+        pc.printf("Bosch BNO055 is NOT available!!\r\n");
+        wait_ms(200);
+    }
+    
+    pc.printf("Bosch Sensortec BNO055 available \r\n");//" __DATE__ "/" __TIME__ "\r\n");
+}
+
+int main() {
+
+    setup();
+    //pc.printf("Bosch Sensortec BNO055 test program on \r\n");//" __DATE__ "/" __TIME__ "\r\n"); 
+    checkAvail();
+    printInfo();
+
+    while (1) {
+        getValues();
+        printMag();
         
-        result = x/y;
+        result = (double) imu.mag.x / imu.mag.y;
+        angleToNorth = getAngleToNorth(imu.mag.x, imu.mag.y, result);      
         
-        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);
+        pc.printf("it is pointing %f angle from north \r\n\n", angleToNorth); 
+        wait_us(200);
     }
-}
\ No newline at end of file
+}