File content as of revision 2:8a752bc2f2f8:

#include "mbed.h"
#include "AX12.h"

int main() {

    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) {
        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);
    }
}