Ted Parrott
NA
Dependencies: AX12 BMA180_3 L3G4200D_2 mbed
Fork of
AX12-HelloWorld
by 
Chris Styles
Revision 2:ce44bdd590ed, committed 2017-11-16
- Comitter:
- tedparrott6
- Date:
- Thu Nov 16 15:01:44 2017 +0000
- Parent:
- 1:b12b06e2fc2d
- Commit message:
- NA
Changed in this revision
--- /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
--- /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
--- 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