main.cpp

00001 #include "mbed.h"
00002 #include "BMA180.h"
00003 #include "math.h"
00004 
00005 Serial pc(USBTX, USBRX); // tx, rx
00006 BMA180 my_BMA180(p5,p6,p7,p15,p16);
00007 double angle_x;
00008 double angle_y;
00009 double angle_z;
00010 double r1;
00011 double r2;
00012 double r3;
00013 float pi = 3.15159;
00014 
00015 
00016 int main()
00017     {
00018        my_BMA180.initBMA180();
00019        
00020        while(1)
00021        {
00022            int x_msb, y_msb, z_msb;
00023            char x_lsb, y_lsb, z_lsb;
00024            short ax, ay, az;
00025            float afx, afy, afz;
00026     
00027            x_lsb = my_BMA180.readReg(ACCXLSB);                              // Read X LSB register
00028            x_msb = my_BMA180.readReg(ACCXMSB);                              // Read X MSB register
00029            ax = (x_msb << 8) | x_lsb;                             // Concatinate X MSB and LSB
00030            ax = ax >> 2;                                          // Remove unused first 2 LSB (16 bits to 14 bits)
00031            afx = (float)ax*3/16384;                               
00032     
00033            y_lsb = my_BMA180.readReg(ACCYLSB);                              // Read Y LSB register
00034            y_msb = my_BMA180.readReg(ACCYMSB);                              // Read Y MSB register
00035            ay = (y_msb << 8) | y_lsb;                             // Concatinate Y MSB and LSB
00036            ay = ay >> 2;                                          // Remove unused first 2 LSB
00037            afy = (float)ay*3/16384;
00038                                 
00039            z_lsb = my_BMA180.readReg(ACCZLSB);                              // Read Z LSB register
00040            z_msb = my_BMA180.readReg(ACCZMSB);                              // Read Z MSB register
00041            az = (z_msb << 8) | z_lsb;                             // Concatinate Z MSB and LSB
00042            az = az >> 2;                                          // Remove unused first 2 LSB
00043            afz = (float)az*3/16384; 
00044     
00045            r1 =   afx / (sqrt(pow(afy,2) + pow(afz,2)));
00046            r2 =   afy / (sqrt(pow(afx,2) + pow(afz,2)));
00047            r3 =   afz / (sqrt(pow(afy,2) + pow(afx,2)));                
00048     
00049            angle_x = atan(r1)*180/pi;
00050            angle_y = atan(r2)*180/pi;
00051            angle_z = atan(r3)*180/pi;
00052     
00053            pc.printf("\n\rX: %05f Y: %05f Z: %05f", angle_x, angle_y, angle_z);
00054        } 
00055     }