Diff: main.cpp

Revision:

2:f93868975538

Parent:

1:b12b06e2fc2d

--- a/main.cpp	Thu Mar 31 12:03:04 2011 +0000
+++ b/main.cpp	Thu Nov 16 15:00:17 2017 +0000
@@ -1,14 +1,232 @@
 #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 l_theta_k;
+float l_theta_h;
+float l_theta_a_x;
+float l_theta_a_x_rad;
+float l_theta_7_x;
+float l_theta_8_x;
+float l_theta_a_y;
+float l_theta_a_y_rad;
+float l_theta_7_y;
+float l_theta_8_y;
+float l_theta_8_r_y;
+float l_theta_5;
+float l_theta_7_m;
+float l_theta_8_m;
+float l_theta_6;
+float h_l;
+float r_theta_k;
+float r_theta_h;
+float r_theta_a_x;
+float r_theta_a_x_rad;
+float r_theta_2_x;
+float r_theta_1_x;
+float r_theta_a_y;
+float r_theta_a_y_rad;
+float r_theta_2_y;
+float r_theta_1_y;
+float r_theta_1_r_y;
+float r_theta_4;
+float r_theta_2_m;
+float r_theta_1_m;
+float r_theta_3;
+float h_r;
+float angle_read;
+float angle_read_rad;
+float speed_setpoint;
+float speed_error;
+float pos_setpoint;
+float pos_error;
+float kp;
+float kd;
+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; 
+float control_action;
+
+
+
+int main()
+{
+    m5.SetGoal(150);
+    wait(1);
+    m6.SetGoal(150);
+    wait(1);
+    m7.SetGoal(150);
+    wait(1);
+    m8.SetGoal(150);
+    wait(1);
+    m1.SetGoal(150);
+    wait(1);
+    m2.SetGoal(150);
+    wait(1);
+    m3.SetGoal(150);
+    wait(1);
+    m4.SetGoal(150);
+    wait(1);
+    m9.SetGoal(150);
+    wait(1);
+    m10.SetGoal(150);
+    wait(1);
+    h_l = 0;
+    h_r = 0;
+
+    angle_x = 0;
+    angle_y = 0;
+    speed_setpoint = 0;
+    pos_setpoint = -3;
+
+    kd = 0;
+    kp = 2;
+
+    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;
 
-    while (1) {
-        myax12.SetGoal(0);    // go to 0 degrees
-        wait (2.0);
-        myax12.SetGoal(300);  // go to 300 degrees
-        wait (2.0);
+        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;
+        
+        pos_error = pos_setpoint - output_angle_x;
+        speed_error = speed_setpoint - speed_x;
+        
+        control_action = kp*pos_error + kd*speed_error;
+        if(control_action > 8){
+            control_action = 8;
+        }else if(control_action < -8){
+            control_action = -8;
+        }else{
+            control_action = control_action;
+        } 
+        angle_read = control_action;
+        angle_read_rad = angle_read*(3.14159/180);
+        
+        if(angle_read == 0){
+            h_l = 0;
+            h_r = 0;
+        }else if(angle_read > 0){
+            h_l = 79*tan(angle_read_rad);
+            h_r = 0;
+        }else if(angle_read < 0){
+            angle_read_rad = abs(angle_read_rad);
+            h_l = 0;
+            h_r = 79*tan(angle_read_rad);
+        }
+        l_theta_h = ((180/3.14159)*acos(1-(h_l/204)));
+        l_theta_k = 2*l_theta_h;
+        l_theta_5 = 150 + l_theta_h;
+        l_theta_6 = 150 + l_theta_k;
+        
+        r_theta_h = ((180/3.14159)*acos(1-(h_r/204)));
+        r_theta_k = 2*r_theta_h;
+        r_theta_4 = 150 - r_theta_h;
+        r_theta_3 = 150 - r_theta_k;
+        
+        l_theta_a_x = l_theta_h;
+        l_theta_a_x_rad = l_theta_a_x*(3.14159/180);
+        l_theta_8_x = (l_theta_a_x/0.34);
+        l_theta_7_x = ((180/3.14159)*(asin((11.4/8)*l_theta_a_x_rad)));
+
+        l_theta_a_y = angle_read;
+        l_theta_a_y_rad = l_theta_a_y*3.14159/180;
+        l_theta_7_y = -1.43*l_theta_a_y;
+        l_theta_8_r_y = -asin((20.94/8)*sin(l_theta_a_y_rad));
+        l_theta_8_y= (l_theta_8_r_y*180/3.14159);
+        
+        r_theta_a_x = r_theta_h;
+        r_theta_a_x_rad = r_theta_a_x*(3.14159/180);
+        r_theta_1_x = (r_theta_a_x/0.34);
+        r_theta_2_x = ((180/3.14159)*(asin((11.4/8)*r_theta_a_x_rad)));
+
+        r_theta_a_y = angle_read;
+        r_theta_a_y_rad = r_theta_a_y*3.14159/180;
+        r_theta_2_y = -1.43*r_theta_a_y;
+        r_theta_1_r_y = -asin((20.94/8)*sin(r_theta_a_y_rad));
+        r_theta_1_y= (r_theta_1_r_y*180/3.14159);
+        
+        l_theta_7_m = 150 + l_theta_7_x + l_theta_7_y;
+        l_theta_8_m = 150 - l_theta_8_x + l_theta_8_y;
+        
+        r_theta_2_m = 150 - r_theta_2_x - r_theta_2_y;
+        r_theta_1_m = 150 + r_theta_1_x - r_theta_1_y;
+        
+        m1.SetGoal(r_theta_1_m);
+        m2.SetGoal(r_theta_2_m);
+        m3.SetGoal(r_theta_3);
+        m4.SetGoal(r_theta_4);
+        m5.SetGoal(l_theta_5);
+        m6.SetGoal(l_theta_6);
+        m7.SetGoal(l_theta_7_m);
+        m8.SetGoal(l_theta_8_m);
+        pc.printf("\n\rAngle: %05f",output_angle_y);
+           
     }
 }
\ No newline at end of file