![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
NA
Dependencies: BMA180_1 L3G4200D mbed
Revision 0:51b08ae8fc55, committed 2017-11-16
- Comitter:
- tedparrott6
- Date:
- Thu Nov 16 14:58:51 2017 +0000
- Commit message:
- NA
Changed in this revision
diff -r 000000000000 -r 51b08ae8fc55 BMA180.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BMA180.lib Thu Nov 16 14:58:51 2017 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/tedparrott6/code/BMA180_1/#333b3cfb4b89
diff -r 000000000000 -r 51b08ae8fc55 L3G4200D.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/L3G4200D.lib Thu Nov 16 14:58:51 2017 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/tedparrott6/code/L3G4200D/#d99fca91fe6f
diff -r 000000000000 -r 51b08ae8fc55 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Nov 16 14:58:51 2017 +0000 @@ -0,0 +1,89 @@ +#include "mbed.h" +#include "BMA180.h" +#include "math.h" +#include "L3G4200D.h" + +Serial pc(USBTX, USBRX); +L3G4200D gyro(p9,p10); +BMA180 my_BMA180(p5,p6,p7,p15,p16); + +float data_x[2]; +float data_y[2]; +float angle_x; +float angle_x_a; +float angle_y; +float angle_y_a; +float r1; +float r2; +float pi = 3.14159; +float speed_x; +float speed_y; +float g[3]; +float output_angle_x; +float output_angle_y; +int output_angle_d_x; +int output_angle_d_y; +float force_mag; +Timer t; +float dt; + +int main() +{ + + angle_x = 0; + angle_y = 0; + t.start(); + while(1) { + gyro.read(g); // Read and Interpret Gyro Data + speed_x = ((g[1])*0.07) - 1.12; + speed_y = ((g[0])*0.07) - 0.595; + + 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))); // Determine X component of gravity force + r2 = afy / (sqrt(pow(afx,2) + pow(afz,2))); // Determine Y component of gravity force + angle_x_a = atan(r1)*180/pi; // Determine X, Y angles + angle_y_a = atan(r2)*180/pi; + + force_mag = abs(az) + abs(ay) + abs(ax); // Check force magnitude to reduce effects of shocks/vibration + + + angle_x = 0.98*(angle_x + speed_x*0.005) + 0.02*angle_x_a; + output_angle_x = angle_x; + output_angle_d_x = output_angle_x; + + angle_y = 0.98*(angle_y + speed_y*0.005) + 0.02*angle_y_a; + output_angle_y = angle_y; + output_angle_d_y = output_angle_y; + + dt = t.read_us(); // timer to determine dt + //pc.printf("\n\rTime: %05f", dt); + t.reset(); + + //pc.printf("$%i %i;", output_angle_d_x, output_angle_d_y); //Print values for serial reader app + pc.printf("%d\n", output_angle_d_x); //Print values for MATLAB + //pc.printf("\n\rSpeed: %05f",speed); + + } +}
diff -r 000000000000 -r 51b08ae8fc55 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Nov 16 14:58:51 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/7c328cabac7e \ No newline at end of file