Ted Parrott
/
AccelerometerTest
NA
main.cpp@0:c2fcb3c063f9, 2017-11-16 (annotated)
- Committer:
- tedparrott6
- Date:
- Thu Nov 16 14:56:18 2017 +0000
- Revision:
- 0:c2fcb3c063f9
NA
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
tedparrott6 | 0:c2fcb3c063f9 | 1 | #include "mbed.h" |
tedparrott6 | 0:c2fcb3c063f9 | 2 | #include "BMA180.h" |
tedparrott6 | 0:c2fcb3c063f9 | 3 | #include "math.h" |
tedparrott6 | 0:c2fcb3c063f9 | 4 | |
tedparrott6 | 0:c2fcb3c063f9 | 5 | Serial pc(USBTX, USBRX); // tx, rx |
tedparrott6 | 0:c2fcb3c063f9 | 6 | BMA180 my_BMA180(p5,p6,p7,p15,p16); |
tedparrott6 | 0:c2fcb3c063f9 | 7 | double angle_x; |
tedparrott6 | 0:c2fcb3c063f9 | 8 | double angle_y; |
tedparrott6 | 0:c2fcb3c063f9 | 9 | double angle_z; |
tedparrott6 | 0:c2fcb3c063f9 | 10 | double r1; |
tedparrott6 | 0:c2fcb3c063f9 | 11 | double r2; |
tedparrott6 | 0:c2fcb3c063f9 | 12 | double r3; |
tedparrott6 | 0:c2fcb3c063f9 | 13 | float pi = 3.15159; |
tedparrott6 | 0:c2fcb3c063f9 | 14 | |
tedparrott6 | 0:c2fcb3c063f9 | 15 | |
tedparrott6 | 0:c2fcb3c063f9 | 16 | int main() |
tedparrott6 | 0:c2fcb3c063f9 | 17 | { |
tedparrott6 | 0:c2fcb3c063f9 | 18 | my_BMA180.initBMA180(); |
tedparrott6 | 0:c2fcb3c063f9 | 19 | |
tedparrott6 | 0:c2fcb3c063f9 | 20 | while(1) |
tedparrott6 | 0:c2fcb3c063f9 | 21 | { |
tedparrott6 | 0:c2fcb3c063f9 | 22 | int x_msb, y_msb, z_msb; |
tedparrott6 | 0:c2fcb3c063f9 | 23 | char x_lsb, y_lsb, z_lsb; |
tedparrott6 | 0:c2fcb3c063f9 | 24 | short ax, ay, az; |
tedparrott6 | 0:c2fcb3c063f9 | 25 | float afx, afy, afz; |
tedparrott6 | 0:c2fcb3c063f9 | 26 | |
tedparrott6 | 0:c2fcb3c063f9 | 27 | x_lsb = my_BMA180.readReg(ACCXLSB); // Read X LSB register |
tedparrott6 | 0:c2fcb3c063f9 | 28 | x_msb = my_BMA180.readReg(ACCXMSB); // Read X MSB register |
tedparrott6 | 0:c2fcb3c063f9 | 29 | ax = (x_msb << 8) | x_lsb; // Concatinate X MSB and LSB |
tedparrott6 | 0:c2fcb3c063f9 | 30 | ax = ax >> 2; // Remove unused first 2 LSB (16 bits to 14 bits) |
tedparrott6 | 0:c2fcb3c063f9 | 31 | afx = (float)ax*3/16384; |
tedparrott6 | 0:c2fcb3c063f9 | 32 | |
tedparrott6 | 0:c2fcb3c063f9 | 33 | y_lsb = my_BMA180.readReg(ACCYLSB); // Read Y LSB register |
tedparrott6 | 0:c2fcb3c063f9 | 34 | y_msb = my_BMA180.readReg(ACCYMSB); // Read Y MSB register |
tedparrott6 | 0:c2fcb3c063f9 | 35 | ay = (y_msb << 8) | y_lsb; // Concatinate Y MSB and LSB |
tedparrott6 | 0:c2fcb3c063f9 | 36 | ay = ay >> 2; // Remove unused first 2 LSB |
tedparrott6 | 0:c2fcb3c063f9 | 37 | afy = (float)ay*3/16384; |
tedparrott6 | 0:c2fcb3c063f9 | 38 | |
tedparrott6 | 0:c2fcb3c063f9 | 39 | z_lsb = my_BMA180.readReg(ACCZLSB); // Read Z LSB register |
tedparrott6 | 0:c2fcb3c063f9 | 40 | z_msb = my_BMA180.readReg(ACCZMSB); // Read Z MSB register |
tedparrott6 | 0:c2fcb3c063f9 | 41 | az = (z_msb << 8) | z_lsb; // Concatinate Z MSB and LSB |
tedparrott6 | 0:c2fcb3c063f9 | 42 | az = az >> 2; // Remove unused first 2 LSB |
tedparrott6 | 0:c2fcb3c063f9 | 43 | afz = (float)az*3/16384; |
tedparrott6 | 0:c2fcb3c063f9 | 44 | |
tedparrott6 | 0:c2fcb3c063f9 | 45 | r1 = afx / (sqrt(pow(afy,2) + pow(afz,2))); |
tedparrott6 | 0:c2fcb3c063f9 | 46 | r2 = afy / (sqrt(pow(afx,2) + pow(afz,2))); |
tedparrott6 | 0:c2fcb3c063f9 | 47 | r3 = afz / (sqrt(pow(afy,2) + pow(afx,2))); |
tedparrott6 | 0:c2fcb3c063f9 | 48 | |
tedparrott6 | 0:c2fcb3c063f9 | 49 | angle_x = atan(r1)*180/pi; |
tedparrott6 | 0:c2fcb3c063f9 | 50 | angle_y = atan(r2)*180/pi; |
tedparrott6 | 0:c2fcb3c063f9 | 51 | angle_z = atan(r3)*180/pi; |
tedparrott6 | 0:c2fcb3c063f9 | 52 | |
tedparrott6 | 0:c2fcb3c063f9 | 53 | pc.printf("\n\rX: %05f Y: %05f Z: %05f", angle_x, angle_y, angle_z); |
tedparrott6 | 0:c2fcb3c063f9 | 54 | } |
tedparrott6 | 0:c2fcb3c063f9 | 55 | } |