File content as of revision 29:d1e8eb135e6c:

#include "mbed.h"

double L_ua = 0.5;          //upper arm
double L_la = 0.7;          //lower arm, attached to the end effector
double x01 = 0.0;       
double y01 = 0.2;           //height base joint

void forwardkinematics_function(double& q1, double& q2, double& x, double& y) {
    // input are joint angles, output are x and y position of end effector

    x = x01 + L_ua * cos(q1) - L_la * cos(q2);
    y = y01 + L_ua * sin(q1) - L_la * sin(q2);   
     

}

void inversekinematics_function(double &x, double &y, const double &T, double &qref1, double &qref2, double &q1, double &q2, double &des_vx, double &des_vy) {
    // input is desired x and y velocity, output reference angles
    // reference angle spees are calculated using the inverse of jacobian
    // from the reference angle speeds the reference angles are computed

    double q1_star_des; // desired joint velocity of q1_star
    double q2_star_des; // same as above but then for q2_star

    // The calculation below assumes that the end effector position is calculated before this function is executed
    // In our case the determinant will not equal zero, hence no problems with singularies I think.
    q1_star_des = 1/(L1*(-x*sin(q1)-(-y+y01)*cos(q1)))*(-1*(-x+L1*cos(q1))*des_vx-x*des_vy);
    q2_star_des = 1/(L1*(-x*sin(q1)-(-y+y01)*cos(q1)))*(-1*(-y+y01+L1*sin(q1))*des_vx+1*(-y+y01)*des_vy);
    
    qref1 = q1+T*q1_star_des; // Yet to adapt all these equations
    qref2 = q2+T*(q2_star_des - q1_star_des);    
}