File content as of revision 50:5d2176b93a65:

#pragma once

#include "mbed.h"
#include "constants.h"


// Pass the two values that you want the x and y outputs in, as 3rd and 4th arguments (3rd and 4th arguments are outpurs).
void forward_kinematics(double main_angle, double sec_angle, double &output_x, double &output_y)
{
    // Calculation of position joint 1 expressed in frame 0
    float J1x_0 = L6 + L0 + L1*cos(sec_angle);
    float J1y_0 = L1*sin(sec_angle);
    
    // Calculation of position joint 3 expressed in frame 0
    float J3x_0 = L6 + L4*cos(main_angle); 
    float J3y_0 = L4*sin(main_angle);

    // Calculation of Joint 2 expressed in frame 2
    float m_y = J3y_0 - J1y_0;
    float m_x = J1x_0 - J3x_0;
    float m = sqrt(pow(m_y,2) + pow(m_x,2));                              // Radius between Joint 1 and 3
    float delta = acos(- ( pow(m,2) - pow(L2,2) - pow(L3,2))/(2*L2*L3) );
    float mu = acos( (pow(L2,2) - pow(L3,2) + pow(m,2))/(2*m*L2) );               // Angle between L2 and m

    float t_y = J3y_0;
    float t_x = (L0 + L6) - J3x_0;
    float t = sqrt(pow(t_y,2) + pow(t_x,2));                              // Radius between frame 1 and Joint 3
    float phi = acos( (pow(L1,2) - pow(t,2) + pow(m,2))/(2*m*L1) );            // Angle between L1 and m

    float q2 = PI - mu - phi;                                     // Angle that L2 makes in frame 2
    float J2x_2 = L2*cos(q2);
    float J2y_2 = L2*sin(q2);

    // Calculation of Joint 2 expressed in frame 0
    float J1x_1 = L1*cos(sec_angle);                              // Joint 1 expressed in frame 1
    float J1y_1 = L1*sin(sec_angle); 
    float J2x_0 = J2x_2*cos(sec_angle) - J2y_2 * sin(sec_angle) + J1x_1 + L0 + L6;    // Joint 2 expressed in frame 0
    float J2y_0 = J2x_2*sin(sec_angle) + J2y_2 * cos(sec_angle) + J1y_1;
 
    // Calculation of End-effector
    float f_x = J2x_0 - L6;
    float f_y = J2y_0;
    float f = sqrt(pow(f_x,2) + pow(f_y,2));                              // Radius between motor 1 and Joint 2
    float xhi = acos( -(pow(f,2) - pow(L3,2) - pow(L4,2))/(2*L3*L4) );         // Angle between L3 and L4
    float omega = PI - xhi;                                       // Angle between L4 and L5
    float n = sqrt(pow(L4,2) + pow(L5,2) - 2*L4*L5*cos(omega));          // Radius between end effector and motor 1

    float theta = acos( (pow(L4,2) - pow(L5,2) + pow(n,2))/(2*n*L4) );          // Angle between n and L4
    float rho = PI - theta - main_angle;                              // Angle between n and L4

    float Pe_x = L6 - n*cos(rho);                                 // y-coordinate end-effector
    float Pe_y = n*sin(rho);                                      // x-coordinate end-effector


    output_x = Pe_x;
    output_y = Pe_y; 
}