File content as of revision 0:404560011fc8:

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

int SetPx;      //Setpoint position x-coordinate from changePosition (EMG dependent)
int SetPy;      //Setpoint position y-coordinate from changePosition (EMG dependent)
double q1;      //Reference position q1 from calibration (only the first time)
double q2;      //Reference position q2 from calibration (only the first time)
float L1 = 0.30;    //Length arm 1
float 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;           //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;         //Calculate the desired motor1 angle from the desired joint positions
    Motor2Set = pi-q2-q1;   //Calculate the desired motor2 angle from the desired joint positions
}