NA

Dependencies:   AX12 BMA180_3 L3G4200D_2 mbed

Fork of AX12-HelloWorld by Chris Styles

Files at this revision

API Documentation at this revision

Comitter:
tedparrott6
Date:
Thu Nov 16 15:01:44 2017 +0000
Parent:
1:b12b06e2fc2d
Commit message:
NA

Changed in this revision

BMA180.lib Show annotated file Show diff for this revision Revisions of this file
L3G4200D.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r b12b06e2fc2d -r ce44bdd590ed BMA180.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BMA180.lib	Thu Nov 16 15:01:44 2017 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/tedparrott6/code/BMA180_3/#12c70c131e5b
diff -r b12b06e2fc2d -r ce44bdd590ed L3G4200D.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/L3G4200D.lib	Thu Nov 16 15:01:44 2017 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/tedparrott6/code/L3G4200D_2/#385ac07a940f
diff -r b12b06e2fc2d -r ce44bdd590ed main.cpp
--- a/main.cpp	Thu Mar 31 12:03:04 2011 +0000
+++ b/main.cpp	Thu Nov 16 15:01:44 2017 +0000
@@ -1,14 +1,131 @@
 #include "mbed.h"
+#include "BMA180.h"
+#include "math.h"
+#include "L3G4200D.h"
 #include "AX12.h"
 
-int main() {
+Serial pc(USBTX, USBRX);
+L3G4200D gyro(p9,p10);
+BMA180 my_BMA180(p5,p6,p7,p15,p16);
+
+AX12 m1 (p28, p27, 1);
+AX12 m2 (p28, p27, 2);
+AX12 m3 (p28, p27, 3);
+AX12 m4 (p28, p27, 4);
+AX12 m5 (p28, p27, 5);
+AX12 m6 (p28, p27, 6);
+AX12 m7 (p28, p27, 7);
+AX12 m8 (p28, p27, 8);
+AX12 m9 (p28, p27, 9);
+AX12 m10 (p28, p27, 10);
 
-    AX12 myax12 (p9, p10, 1);
+float theta_1;
+float theta_1_r;
+float theta_2;
+float theta_8;
+float theta_8_r;
+float theta_7;
+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 theta_roll;
+float theta_a_right;
+float theta_a_left;
+float theta_a_right_rad;
+float theta_a_left_rad;
+int x_msb, y_msb, z_msb;
+char x_lsb, y_lsb, z_lsb;
+short ax, ay, az;
+float afx, afy, afz;
 
+
+int main()
+{
+    m1.SetGoal(150);
+    wait(0.5);
+    m2.SetGoal(150);
+    wait(0.5);
+    m3.SetGoal(150);
+    wait(0.5);
+    m4.SetGoal(150);
+    wait(0.5);
+    m5.SetGoal(150);
+    wait(0.5);
+    m6.SetGoal(150);
+    wait(0.5);
+    m7.SetGoal(150);
+    wait(0.5);
+    m8.SetGoal(150);
+    wait(0.5);
+    m9.SetGoal(150);
+    wait(0.5);
+    m10.SetGoal(150);
+    wait(0.5);
+
+    angle_y = 0;
+    angle_x = 0;
     while (1) {
-        myax12.SetGoal(0);    // go to 0 degrees
-        wait (2.0);
-        myax12.SetGoal(300);  // go to 300 degrees
-        wait (2.0);
+        gyro.read(g);                                  // Read and Interpret Gyro Data
+        speed_x = ((g[1])*0.07) - 1.12;
+        speed_y = ((g[0])*0.07) - 0.3;
+
+        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;
+
+        angle_x = 0.98*(angle_x + speed_x*0.005) + 0.02*angle_x_a;
+
+        angle_y = 0.98*(angle_y + speed_y*0.005) + 0.02*angle_y_a;
+        theta_roll = angle_y + 4.5;
+
+        if(theta_roll < -3) {
+            theta_a_right = -7;
+        } else {
+            theta_a_right = 0;
+        }
+        if(theta_roll > 3) {
+            theta_a_left = 7;
+        } else {
+            theta_a_left = 0;
+        }
+
+        theta_a_right_rad = theta_a_right*3.14159/180;
+        theta_a_left_rad = theta_a_left*3.14159/180;
+        theta_2 = -1.43*theta_a_right + 150;
+        theta_7 = -1.43*theta_a_left + 150;
+        theta_1_r = -asin((20.94/8)*sin(theta_a_right_rad));
+        theta_1 = (theta_1_r*180/3.14159)+150;
+        theta_8_r = -asin((20.94/8)*sin(theta_a_left_rad));
+        theta_8 = (theta_8_r*180/3.14159)+150;
+        m1.SetGoal(theta_1);
+        m2.SetGoal(theta_2);
+        m7.SetGoal(theta_7);
+        m8.SetGoal(theta_8);
     }
 }
\ No newline at end of file