Diff: main.cpp

Revision:

2:3d159bed27c1

Parent:

1:b12b06e2fc2d

--- a/main.cpp	Thu Mar 31 12:03:04 2011 +0000
+++ b/main.cpp	Thu Nov 16 14:56:50 2017 +0000
@@ -3,12 +3,42 @@
 
 int main() {
 
-    AX12 myax12 (p9, p10, 1);
-
+    AX12 m7 (p28, p27, 7);
+    AX12 m8 (p28, p27, 8);
+    Serial pc(USBTX,USBRX);
+    float theta_a_x;
+    float theta_a_y;
+    float theta_a_x_rad;
+    float theta_8_x ;
+    float theta_7_x;
+    float theta_a_y_rad;
+    float theta_7_y;
+    float theta_8_r_y;
+    float theta_8_y;
+    float theta_7_m;
+    float theta_8_m;
+    
+    m7.SetGoal(150);
+    wait(1);
+    m8.SetGoal(150);
+    wait(1);
+        
     while (1) {
-        myax12.SetGoal(0);    // go to 0 degrees
-        wait (2.0);
-        myax12.SetGoal(300);  // go to 300 degrees
-        wait (2.0);
+        theta_a_x = 0;
+        theta_a_x_rad = theta_a_x*(3.14159/180);
+        theta_8_x = (theta_a_x/0.34);
+        theta_7_x = ((180/3.14159)*(asin((11.4/8)*theta_a_x_rad)));
+
+        theta_a_y = 0;
+        theta_a_y_rad = theta_a_y*3.14159/180;
+        theta_7_y = -1.43*theta_a_y;
+        theta_8_r_y = -asin((20.94/8)*sin(theta_a_y_rad));
+        theta_8_y= (theta_8_r_y*180/3.14159);
+        
+        theta_7_m = 150 + theta_7_x + theta_7_y;
+        theta_8_m = 150 - theta_8_x + theta_8_y;
+        m7.SetGoal(theta_7_m);
+        m8.SetGoal(theta_8_m);
+        pc.printf("\n\r7m: %05f 8m: %05f",theta_7_m,theta_8_m);
     }
 }
\ No newline at end of file