Diff: main.cpp

Revision:

2:8a752bc2f2f8

Parent:

1:b12b06e2fc2d

--- a/main.cpp	Thu Mar 31 12:03:04 2011 +0000
+++ b/main.cpp	Thu Nov 16 15:02:21 2017 +0000
@@ -3,12 +3,139 @@
 
 int main() {
 
-    AX12 myax12 (p9, p10, 1);
-
+    AX12 m1 (p28, p27, 1);
+    AX12 m2 (p28, p27, 2);
+    AX12 m3 (p28, p27, 3);
+    AX12 m4 (p28, p27, 4);
+    AX12 m6 (p28, p27, 6);
+    AX12 m5 (p28, p27, 5);
+    AX12 m7 (p28, p27, 7);
+    AX12 m8 (p28, p27, 8);
+    AX12 m9 (p28, p27, 9);
+    AX12 m10 (p28, p27, 10);
+    Serial pc(USBTX,USBRX);
+    Timer t;
+    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 time;
+    float angle_read;
+    float angle_read_rad;
+    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);
+    t.start();
+    h_l = 0;
+    h_r = 0;
+    
     while (1) {
-        myax12.SetGoal(0);    // go to 0 degrees
-        wait (2.0);
-        myax12.SetGoal(300);  // go to 300 degrees
-        wait (2.0);
+        time = t.read();
+        angle_read = 5*sin(time);
+        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",angle_read);
     }
 }
\ No newline at end of file