File content as of revision 10:c35d16ad1b6b:

#include "mbed.h"
#include "math.h"

const float L0 = 0.15;                      // Length between two motors [meter]
const float L1 = 0.10;                      // Length first beam from right motor2 [meter]
const float L2 = 0.30;                      // Length second beam from right motor2 [meter]
const float L3 = 0.15;                      // Length beam between L2 and L4 [meter]
const float L4 = 0.30;                      // Length first beam from left motor1 [meter]
const float L5 = 0.35;                      // Length from L3 to end-effector [meter]
const float L6 = 1.00;                      // Length from frame 0 to motor 1
const double PI = 3.14159265359;
volatile float Pe_x;                        // x-coordinate of end-effector from frame 0 [meter]
volatile float Pe_y;                        // y-coordinate of end-effector from frame 0 [meter]
volatile static float des_motor_angle1;     // Desired angle of motor 1 (left) based on kinematics [rad]
volatile static float des_motor_angle2;     // Desired angle of motor 2 (right) based on kinematics [rad]

DigitalOut safetyLED(LED_BLUE);             // Safety check LED

Ticker kinematics_ticker;                   // Ticker function for inverse kinematics

void InverseKinematics()
{
    // Calculation of the position of joint 3 in frame 0
    float n = sqrt(pow((L6-Pe_x),2) + pow(Pe_y,2));                         // Radius between motor 1 and endeffector [meter]
    float omega = acos(-(pow(n,2) - pow(L4,2) - pow(L5,2))/(2*L4*L5));      // Angle between L4 and L5 [rad]
    float q4 = PI - omega;                                                  // Angle of joint 3 between L3 and L4
    float theta = acos( -(pow(L5,2) - pow(n,2) - pow(L4,2))/(2*n*L4) );     // Angle between n and L4
    float lambda_stuff = Pe_y/(L6-Pe_x);
    float lambda = PI - atan(lambda_stuff);                                 // Entire angle between L0 and n
    des_motor_angle1 = lambda - theta;
    float J3x_0 = L6 + L4*cos(des_motor_angle1);                            // x-coordinate of joint 3 in frame 0
    float J3y_0 = L4*sin(des_motor_angle1);                                 // y-coordinate of joint 3 in frame 0
    
    // Calculation of the position of joint 2 in frame 0 
    float S = J3y_0 - Pe_y;   // I CHANGED THIS!!!
                                             // Distance between height endeffector and joint 3
    float kappa = asin(S/L5);                                               // Angle of L5  
    float J2x_0 = (L3+L5)*cos(kappa) + Pe_x;                                // x-coordinate of joint 2 in frame 0
    float J2y_0 = (L3+L5)*sin(kappa) + Pe_y;                                // y-coordinate of joint 2 in frame 0
    
    // Calculation of the position of joint 1 in frame 0
    float J2x_1 = J2x_0 - L0 - L6;                                          // x-coordinate of joint 2 in frame 1
    float J2y_1 = J2y_0;                                                    // y-coordinate of joint 2 in frame 1
    float r = sqrt(pow(J2x_1,2) + pow(J2y_1,2));                            // Radius between origin frame 1 and J2
    float alfa = acos( -(pow(r,2) - pow(L1,2) - pow(L2,2))/(2*L1*L2) );     // Angle opposite of radius r
    float q2 = PI - alfa;                                                   // Angle between L1 and L2
    
    // Delta < 170 graden SAFETY LIMITATION
    if (r > 0.2399)                                                         // If delta is larger than 175 degrees than r is this
    {
        r = 0.2399;     // I CHANGED THIS
        safetyLED = 0;
    }

    // Calculation of motor_angle2     I CHANGED THIS!!!
    float beta = acos(- (pow(L2,2) - pow(r,2) - pow(L1,2))/(2*L1*r));       // Angle between r and L1
    float zeta = acos(J2x_1/r);                                             // Angle between r and x-axis of frame 1
    des_motor_angle2 = zeta - beta;
    

    // Determining angle delta for safety
    float J1x_0 = L0 + L6 + L1*cos(des_motor_angle2);                       // x-coordinate of joint 1 in frame 0
    float J1y_0 = L1*sin(des_motor_angle2);                                 // y-coordinate of joint 1 in frame 0   
    
    float m = sqrt(pow((J1x_0 - J3x_0),2) + pow((J3y_0 - J1y_0),2));        // Radius between Joint 1 and Joint 3
    float delta = acos(- (pow(m,2) - pow(L2,2) - pow(L3,2))/(2*L2*L3));     // Angle between L2 and L3


    // Implementing stops for safety
    // 45 < Motor_angle1 < 70 graden
    if (des_motor_angle1 < 0.785398)                             // If motor_angle is smaller than 45 degrees
    {
        des_motor_angle1 = 0.785398;
        safetyLED = 0;
    }
    else if (des_motor_angle1 > 1.22173)                         // If motor_angle is larger than 70 degrees
    {
        des_motor_angle1 = 1.22173;
        safetyLED = 0;
    }
    
    // -42 < Motor_angle2 < 85 graden
    if (des_motor_angle2 < -0.733038)                            // If motor_angle is smaller than -42 degrees
    {
        des_motor_angle2 = -0.733038;
        safetyLED = 0;
    }
    else if (des_motor_angle2 > 1.48353)                         // If motor_angle is larger than 85 degrees
    {
        des_motor_angle2 = 1.48353;
        safetyLED = 0;
    }
}    


int main()
{
    safetyLED = 1;
    while (true) {
    kinematics_ticker.attach(InverseKinematics,0.5);
    }
}