NA
Dependencies: BMA180_1 L3G4200D mbed
main.cpp@0:51b08ae8fc55, 2017-11-16 (annotated)
- Committer:
- tedparrott6
- Date:
- Thu Nov 16 14:58:51 2017 +0000
- Revision:
- 0:51b08ae8fc55
NA
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
tedparrott6 | 0:51b08ae8fc55 | 1 | #include "mbed.h" |
tedparrott6 | 0:51b08ae8fc55 | 2 | #include "BMA180.h" |
tedparrott6 | 0:51b08ae8fc55 | 3 | #include "math.h" |
tedparrott6 | 0:51b08ae8fc55 | 4 | #include "L3G4200D.h" |
tedparrott6 | 0:51b08ae8fc55 | 5 | |
tedparrott6 | 0:51b08ae8fc55 | 6 | Serial pc(USBTX, USBRX); |
tedparrott6 | 0:51b08ae8fc55 | 7 | L3G4200D gyro(p9,p10); |
tedparrott6 | 0:51b08ae8fc55 | 8 | BMA180 my_BMA180(p5,p6,p7,p15,p16); |
tedparrott6 | 0:51b08ae8fc55 | 9 | |
tedparrott6 | 0:51b08ae8fc55 | 10 | float data_x[2]; |
tedparrott6 | 0:51b08ae8fc55 | 11 | float data_y[2]; |
tedparrott6 | 0:51b08ae8fc55 | 12 | float angle_x; |
tedparrott6 | 0:51b08ae8fc55 | 13 | float angle_x_a; |
tedparrott6 | 0:51b08ae8fc55 | 14 | float angle_y; |
tedparrott6 | 0:51b08ae8fc55 | 15 | float angle_y_a; |
tedparrott6 | 0:51b08ae8fc55 | 16 | float r1; |
tedparrott6 | 0:51b08ae8fc55 | 17 | float r2; |
tedparrott6 | 0:51b08ae8fc55 | 18 | float pi = 3.14159; |
tedparrott6 | 0:51b08ae8fc55 | 19 | float speed_x; |
tedparrott6 | 0:51b08ae8fc55 | 20 | float speed_y; |
tedparrott6 | 0:51b08ae8fc55 | 21 | float g[3]; |
tedparrott6 | 0:51b08ae8fc55 | 22 | float output_angle_x; |
tedparrott6 | 0:51b08ae8fc55 | 23 | float output_angle_y; |
tedparrott6 | 0:51b08ae8fc55 | 24 | int output_angle_d_x; |
tedparrott6 | 0:51b08ae8fc55 | 25 | int output_angle_d_y; |
tedparrott6 | 0:51b08ae8fc55 | 26 | float force_mag; |
tedparrott6 | 0:51b08ae8fc55 | 27 | Timer t; |
tedparrott6 | 0:51b08ae8fc55 | 28 | float dt; |
tedparrott6 | 0:51b08ae8fc55 | 29 | |
tedparrott6 | 0:51b08ae8fc55 | 30 | int main() |
tedparrott6 | 0:51b08ae8fc55 | 31 | { |
tedparrott6 | 0:51b08ae8fc55 | 32 | |
tedparrott6 | 0:51b08ae8fc55 | 33 | angle_x = 0; |
tedparrott6 | 0:51b08ae8fc55 | 34 | angle_y = 0; |
tedparrott6 | 0:51b08ae8fc55 | 35 | t.start(); |
tedparrott6 | 0:51b08ae8fc55 | 36 | while(1) { |
tedparrott6 | 0:51b08ae8fc55 | 37 | gyro.read(g); // Read and Interpret Gyro Data |
tedparrott6 | 0:51b08ae8fc55 | 38 | speed_x = ((g[1])*0.07) - 1.12; |
tedparrott6 | 0:51b08ae8fc55 | 39 | speed_y = ((g[0])*0.07) - 0.595; |
tedparrott6 | 0:51b08ae8fc55 | 40 | |
tedparrott6 | 0:51b08ae8fc55 | 41 | int x_msb, y_msb, z_msb; |
tedparrott6 | 0:51b08ae8fc55 | 42 | char x_lsb, y_lsb, z_lsb; |
tedparrott6 | 0:51b08ae8fc55 | 43 | short ax, ay, az; |
tedparrott6 | 0:51b08ae8fc55 | 44 | float afx, afy, afz; |
tedparrott6 | 0:51b08ae8fc55 | 45 | |
tedparrott6 | 0:51b08ae8fc55 | 46 | x_lsb = my_BMA180.readReg(ACCXLSB); // Read X LSB register |
tedparrott6 | 0:51b08ae8fc55 | 47 | x_msb = my_BMA180.readReg(ACCXMSB); // Read X MSB register |
tedparrott6 | 0:51b08ae8fc55 | 48 | ax = (x_msb << 8) | x_lsb; // Concatinate X MSB and LSB |
tedparrott6 | 0:51b08ae8fc55 | 49 | ax = ax >> 2; // Remove unused first 2 LSB (16 bits to 14 bits) |
tedparrott6 | 0:51b08ae8fc55 | 50 | afx = (float)ax*3/16384; |
tedparrott6 | 0:51b08ae8fc55 | 51 | |
tedparrott6 | 0:51b08ae8fc55 | 52 | y_lsb = my_BMA180.readReg(ACCYLSB); // Read Y LSB register |
tedparrott6 | 0:51b08ae8fc55 | 53 | y_msb = my_BMA180.readReg(ACCYMSB); // Read Y MSB register |
tedparrott6 | 0:51b08ae8fc55 | 54 | ay = (y_msb << 8) | y_lsb; // Concatinate Y MSB and LSB |
tedparrott6 | 0:51b08ae8fc55 | 55 | ay = ay >> 2; // Remove unused first 2 LSB |
tedparrott6 | 0:51b08ae8fc55 | 56 | afy = (float)ay*3/16384; |
tedparrott6 | 0:51b08ae8fc55 | 57 | |
tedparrott6 | 0:51b08ae8fc55 | 58 | z_lsb = my_BMA180.readReg(ACCZLSB); // Read Z LSB register |
tedparrott6 | 0:51b08ae8fc55 | 59 | z_msb = my_BMA180.readReg(ACCZMSB); // Read Z MSB register |
tedparrott6 | 0:51b08ae8fc55 | 60 | az = (z_msb << 8) | z_lsb; // Concatinate Z MSB and LSB |
tedparrott6 | 0:51b08ae8fc55 | 61 | az = az >> 2; // Remove unused first 2 LSB |
tedparrott6 | 0:51b08ae8fc55 | 62 | afz = (float)az*3/16384; |
tedparrott6 | 0:51b08ae8fc55 | 63 | |
tedparrott6 | 0:51b08ae8fc55 | 64 | r1 = afx / (sqrt(pow(afy,2) + pow(afz,2))); // Determine X component of gravity force |
tedparrott6 | 0:51b08ae8fc55 | 65 | r2 = afy / (sqrt(pow(afx,2) + pow(afz,2))); // Determine Y component of gravity force |
tedparrott6 | 0:51b08ae8fc55 | 66 | angle_x_a = atan(r1)*180/pi; // Determine X, Y angles |
tedparrott6 | 0:51b08ae8fc55 | 67 | angle_y_a = atan(r2)*180/pi; |
tedparrott6 | 0:51b08ae8fc55 | 68 | |
tedparrott6 | 0:51b08ae8fc55 | 69 | force_mag = abs(az) + abs(ay) + abs(ax); // Check force magnitude to reduce effects of shocks/vibration |
tedparrott6 | 0:51b08ae8fc55 | 70 | |
tedparrott6 | 0:51b08ae8fc55 | 71 | |
tedparrott6 | 0:51b08ae8fc55 | 72 | angle_x = 0.98*(angle_x + speed_x*0.005) + 0.02*angle_x_a; |
tedparrott6 | 0:51b08ae8fc55 | 73 | output_angle_x = angle_x; |
tedparrott6 | 0:51b08ae8fc55 | 74 | output_angle_d_x = output_angle_x; |
tedparrott6 | 0:51b08ae8fc55 | 75 | |
tedparrott6 | 0:51b08ae8fc55 | 76 | angle_y = 0.98*(angle_y + speed_y*0.005) + 0.02*angle_y_a; |
tedparrott6 | 0:51b08ae8fc55 | 77 | output_angle_y = angle_y; |
tedparrott6 | 0:51b08ae8fc55 | 78 | output_angle_d_y = output_angle_y; |
tedparrott6 | 0:51b08ae8fc55 | 79 | |
tedparrott6 | 0:51b08ae8fc55 | 80 | dt = t.read_us(); // timer to determine dt |
tedparrott6 | 0:51b08ae8fc55 | 81 | //pc.printf("\n\rTime: %05f", dt); |
tedparrott6 | 0:51b08ae8fc55 | 82 | t.reset(); |
tedparrott6 | 0:51b08ae8fc55 | 83 | |
tedparrott6 | 0:51b08ae8fc55 | 84 | //pc.printf("$%i %i;", output_angle_d_x, output_angle_d_y); //Print values for serial reader app |
tedparrott6 | 0:51b08ae8fc55 | 85 | pc.printf("%d\n", output_angle_d_x); //Print values for MATLAB |
tedparrott6 | 0:51b08ae8fc55 | 86 | //pc.printf("\n\rSpeed: %05f",speed); |
tedparrott6 | 0:51b08ae8fc55 | 87 | |
tedparrott6 | 0:51b08ae8fc55 | 88 | } |
tedparrott6 | 0:51b08ae8fc55 | 89 | } |