File content as of revision 1:629b077bdae3:

#include "mbed.h"

double pi = 3.14159265359;
double SetPx = 0.50;    //Setpoint position x-coordinate from changePosition (EMG dependent)
double SetPy = 0.04;    //Setpoint position y-coordinate from changePosition (EMG dependent)
double q1 = 0;          //Reference position q1 from calibration (only the first time)
double q2 = pi/2;       //Reference position q2 from calibration (only the first time)
double L1 = 0.30;       //Length arm 1
double L2 = 0.38;       //Length arm 2
double K = 1;           //Spring constant for movement end-joint to setpoint
double B1 = 1;          //Friction coefficient for motor 1
double B2 = 1;          //Friction coefficient for motot 2
double T = 1/500;       //Desired time step
double Motor1Set;       //Motor1 angle
double Motor2Set;       //Motor2 angle

void RKI()
{
    q1 = q1 + ((sin(q1)*L1 + sin(q2)*L2)*SetPy - (cos(q1)*L1 + cos(q2)*L2)*SetPx)*(K*T)/B1;     //Calculate desired joint 1 position
    q2 = q2 + ((SetPy - cos(q1)*L1)*sin(q2)*L2 + (sin(q1)*L1 - SetPx)*cos(q2)*L2)*(K*T)/B2;     //Calculate desired joint 2 position
    
    Motor1Set = q1/pi;           //Calculate the desired motor1 angle from the desired joint positions
    Motor2Set = (pi-q2-q1)/pi;   //Calculate the desired motor2 angle from the desired joint positions
}
    
int main()
{   
    RKI();
    printf("Motor1Set = %d",Motor1Set);
    printf("Motor2Set = %d",Motor2Set); 
}