NA

Dependencies:   BMA180 mbed

main.cpp

Committer:
tedparrott6
Date:
2017-11-16
Revision:
0:c2fcb3c063f9

File content as of revision 0:c2fcb3c063f9:

#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);
       } 
    }