File content as of revision 8:bba05e863b68:

#include "mbed.h"

double L1 = 0.5;
double L2 = 0.7;
double x01 = 0.0;
double y01 = 0.2;

void forwardkinematics_function(double q1, double q2) {
    // input are joint angles, output are x and y position of end effector
    
    x = x01 + L1*cos(q1)-L2*cos(q2);
    y = y01 + L1 * sin(q1) - L2 * sin(q2);    
}

double inversekinematics_function(double q1, double q2, double reference) {
    // pseudo inverse jacobian to get joint speeds
    // input are desired vx and vy of end effector, output joint angle speeds
    // I assume here that reference is a vector (this should also be global I think)
    // I also assume that the end effector position is global here and is denoted by x
    // Assuming the ticker_rate is also set to be global
    
    double des_twist[2]; // the desired twist (DETERMINE IF WE WANT TO MAKE THIS STATIC)
    double q1_star_des; // desired joint velocity of q1_star
    double q2_star_des; // same as above but then for q2_star
    
    q1_star_des = 1/(L1*(-x*sin(q1)-(y+y01)*cos(q1)))*(-1*(-x+L1*cos(q1))*des_twist[0]-x*des_twist[1]);
    q2_star_des = 1/(L1*(-x*sin(q1)-(y+y01)*cos(q1)))*(-1*(-y+y01+L1*sin(q1))*des_twist[0]+1*(-y+y01))*des_twist[1]);
    
    
    return 5.5;    
}