Diff: main.cpp

Revision:

2:056f36912bf0

Parent:

1:b12b06e2fc2d

--- a/main.cpp	Thu Mar 31 12:03:04 2011 +0000
+++ b/main.cpp	Thu Nov 16 15:05:18 2017 +0000
@@ -1,14 +1,179 @@
 #include "mbed.h"
+#include "BMA180.h"
+#include "math.h"
+#include "L3G4200D.h"
 #include "AX12.h"
 
-int main() {
+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);
+
+Timer t_1;
+Timer t_2;
 
-    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 time_1;
+float time_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 theta_roll;
+float theta_a_right;
+float theta_a_left;
+float theta_a_right_rad;
+float theta_a_left_rad;
+float time_sum;
+float period;
+int n;
+float A;
+float B;
+float C;
+float leg_control_signal;
+float theta_4;
+float theta_5;
+int dir_0;
+int dir_1;
+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);
+
+    t_1.start();
+    t_2.start();
+    time_sum = 0;
+    n = 0;
+    angle_y = 0;
+    dir_1 = 0;
+    dir_0 = 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;
+
+        dir_0 = dir_1;
+
+        if(theta_roll >= 0) {
+            dir_1 = 1;
+        } else {
+            dir_1 = 0;
+        }
+
+        if(dir_1 != dir_0) {
+            time_2 = t_2.read();
+            t_2.reset();
+            time_sum = time_sum + time_2;
+            n=n+1;
+        }
+
+        period = 2*(time_sum/n);
+        B = 2*3.14159/period;
+        C = -(period/4)*B;
+        A = 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;
+        }
+
+        time_1 = t_1.read();
+        leg_control_signal = A*sin(B*time_1 + C);
+
+        theta_4 = 150 + leg_control_signal;
+        theta_5 = 150 + leg_control_signal;
+
+        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);
+        m4.SetGoal(theta_4);
+        m5.SetGoal(theta_5);
     }
 }
\ No newline at end of file