![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
NA
Dependencies: BMA180_1 L3G4200D mbed
main.cpp
- Committer:
- tedparrott6
- Date:
- 2017-11-16
- Revision:
- 0:51b08ae8fc55
File content as of revision 0:51b08ae8fc55:
#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); } }