Diff: main.cpp

Revision:

0:51b08ae8fc55

--- /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);
+        
+    }
+}