Ted Parrott
/
AccelerometerTest
NA
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); } }