Ted Parrott / Mbed 2 deprecated FilterTest

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?

UserRevisionLine numberNew 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 }