File content as of revision 2:4720140f7953:

#include "mbed.h"
#include "math.h"
#include "MODSERIAL.h"
#include <string>

//Define important constants in memory
#define     PI              3.14159265
#define     SAMPLE_RATE     0.002   //500 Hz EMG sample rate
#define     CONTROL_RATE    0.01    //100 Hz Control rate
#define     ENCODER1_CPR    4200    //encoders have 64 (X4), 32 (X2) counts per revolution of motor shaft
#define     ENCODER2_CPR    4200    //gearbox 1:131.25 ->  4200 counts per revolution of the output shaft (X2), 
#define     PWM_PERIOD      0.0001  //10k Hz pwm motor frequency. Higher -> too hot, lower -> motor doesnt respond correctly

DigitalIn button(SW2);
MODSERIAL pc(USBTX,USBRX);      //serial communication


Ticker      signal_emg;         //Ticker for signal simulation

double      emg_1 = 10;
double      emg_2 = 5;
double      l1 = 0.25;
double      l2 = 0.25;
double      x = 0.3;
double      y = 0.2;
double      rpc;
double      theta1;
double      theta2;
double      current_x;
double      current_y;
double      x_error;
double      y_error;
double      costheta2;
double      sintheta2;
double      dtheta1;
double      dtheta2;
double      m1_error;
double      m2_error;

void take_sample()
{
    emg_1 = emg_1 + 20;
    emg_2 = emg_2 + 10;
}

//Current position - Encoder counts -> current angle -> Forward Kinematics
int main()
{
    pc.baud(115200);            //serial baudrate
    while(true) {
        if (button == 0) {
            pc.printf("knop1\n");
            while(true) {
                signal_emg.attach(&take_sample,0.5);
                pc.printf("emg1 = %f\n",emg_1);

                rpc=(2*PI)/ENCODER1_CPR;               //radians per count (resolution) - 2pi divided by 4200
                theta1 = emg_1 * rpc;   //multiply resolution with number of counts
                theta2 = emg_2 * rpc;
                current_x = l1 * cos(theta1) + l2 * cos(theta1 + theta2);
                current_y = l1 * sin(theta1) + l2 * sin(theta1 + theta2);
                
                double theta1_degree = theta1 * (180/PI);
                double theta2_degree = theta2 * (180/PI);

                pc.printf("Theta1 = %f, theta2 = %f \r\n",theta1_degree,theta2_degree);

                //calculate error (refpos-currentpos) currentpos = forward kinematics
                x_error = x - current_x;
                y_error = y - current_y;
                
                pc.printf("Error x is %f, error y is %f \r\n",x_error,y_error);

                //inverse kinematics (refpos to refangle)

                costheta2 = (pow(x,2) + pow(y,2) - pow(l1,2) - pow(l2,2)) / (2*l1*l2) ;
                sintheta2 = sqrt( abs( 1 - pow(costheta2,2) ) );


                dtheta2 = atan2(sintheta2,costheta2);

                double k1 = l1 + l2*costheta2;
                double k2 = l2*sintheta2;

                dtheta1 = atan2(y, x) - atan2(k2, k1);
                
                double dtheta1_degree = dtheta1 * (180/PI);
                double dtheta2_degree = dtheta2 * (180/PI);
                
                pc.printf("Wanted theta1 is %f, wanted theta2 is %f \r\n",dtheta1_degree,dtheta2_degree); 

                /* alternative:
                costheta1 = ( x * (l1 + l2 * costheta2) + y * l2 * sintheta2 ) / ( pow(x,2) + pow(y,2) );
                sintheta1 = sqrt( abs( 1 - pow(costheta1,2) ) );

                dtheta1 = atan2(sintheta1,costheta1);
                */

                //Angle error
                m1_error = dtheta1-theta1;
                m2_error = dtheta2-theta2;

                pc.printf("Motor 1 error is %f, motor 2 error is %f \r\n",m1_error,m2_error);
                wait(0.5);
                
                if(x_error>0.002&&x_error<-0.002&&y_error>0.002&&y_error<-0.002)
                {
                    break;
                }
            }
        }
    }
}