Diff: main.cpp

Revision:

0:c2fcb3c063f9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Nov 16 14:56:18 2017 +0000
@@ -0,0 +1,55 @@
+#include "mbed.h"
+#include "BMA180.h"
+#include "math.h"
+
+Serial pc(USBTX, USBRX); // tx, rx
+BMA180 my_BMA180(p5,p6,p7,p15,p16);
+double angle_x;
+double angle_y;
+double angle_z;
+double r1;
+double r2;
+double r3;
+float pi = 3.15159;
+
+
+int main()
+    {
+       my_BMA180.initBMA180();
+       
+       while(1)
+       {
+           int x_msb, y_msb, z_msb;
+           char x_lsb, y_lsb, z_lsb;
+           short ax, ay, az;
+           float afx, afy, afz;
+    
+           x_lsb = my_BMA180.readReg(ACCXLSB);                              // Read X LSB register
+           x_msb = my_BMA180.readReg(ACCXMSB);                              // Read X MSB register
+           ax = (x_msb << 8) | x_lsb;                             // Concatinate X MSB and LSB
+           ax = ax >> 2;                                          // Remove unused first 2 LSB (16 bits to 14 bits)
+           afx = (float)ax*3/16384;                               
+    
+           y_lsb = my_BMA180.readReg(ACCYLSB);                              // Read Y LSB register
+           y_msb = my_BMA180.readReg(ACCYMSB);                              // Read Y MSB register
+           ay = (y_msb << 8) | y_lsb;                             // Concatinate Y MSB and LSB
+           ay = ay >> 2;                                          // Remove unused first 2 LSB
+           afy = (float)ay*3/16384;
+                                
+           z_lsb = my_BMA180.readReg(ACCZLSB);                              // Read Z LSB register
+           z_msb = my_BMA180.readReg(ACCZMSB);                              // Read Z MSB register
+           az = (z_msb << 8) | z_lsb;                             // Concatinate Z MSB and LSB
+           az = az >> 2;                                          // Remove unused first 2 LSB
+           afz = (float)az*3/16384; 
+    
+           r1 =   afx / (sqrt(pow(afy,2) + pow(afz,2)));
+           r2 =   afy / (sqrt(pow(afx,2) + pow(afz,2)));
+           r3 =   afz / (sqrt(pow(afy,2) + pow(afx,2)));                
+    
+           angle_x = atan(r1)*180/pi;
+           angle_y = atan(r2)*180/pi;
+           angle_z = atan(r3)*180/pi;
+    
+           pc.printf("\n\rX: %05f Y: %05f Z: %05f", angle_x, angle_y, angle_z);
+       } 
+    }
\ No newline at end of file