main.cpp

00001 #include "mbed.h"
00002 #include "AX12.h"
00003 
00004 int main() {
00005 
00006     AX12 m7 (p28, p27, 7);
00007     AX12 m8 (p28, p27, 8);
00008     Serial pc(USBTX,USBRX);
00009     float theta_a_x;
00010     float theta_a_y;
00011     float theta_a_x_rad;
00012     float theta_8_x ;
00013     float theta_7_x;
00014     float theta_a_y_rad;
00015     float theta_7_y;
00016     float theta_8_r_y;
00017     float theta_8_y;
00018     float theta_7_m;
00019     float theta_8_m;
00020     
00021     m7.SetGoal(150);
00022     wait(1);
00023     m8.SetGoal(150);
00024     wait(1);
00025         
00026     while (1) {
00027         theta_a_x = 0;
00028         theta_a_x_rad = theta_a_x*(3.14159/180);
00029         theta_8_x = (theta_a_x/0.34);
00030         theta_7_x = ((180/3.14159)*(asin((11.4/8)*theta_a_x_rad)));
00031 
00032         theta_a_y = 0;
00033         theta_a_y_rad = theta_a_y*3.14159/180;
00034         theta_7_y = -1.43*theta_a_y;
00035         theta_8_r_y = -asin((20.94/8)*sin(theta_a_y_rad));
00036         theta_8_y= (theta_8_r_y*180/3.14159);
00037         
00038         theta_7_m = 150 + theta_7_x + theta_7_y;
00039         theta_8_m = 150 - theta_8_x + theta_8_y;
00040         m7.SetGoal(theta_7_m);
00041         m8.SetGoal(theta_8_m);
00042         pc.printf("\n\r7m: %05f 8m: %05f",theta_7_m,theta_8_m);
00043     }
00044 }