File content as of revision 0:8336c89b41ab:

#include "mbed.h"
#include "Serial.h"
#include "QEI.h"
#include "math.h"


Serial      pc(USBTX, USBRX);        //Serial PC connectie
QEI encoder(D13, D12, NC, 32);       //encoder instellen
DigitalOut  motor1DirectionPin(D4);  //Motorrichting op D4 (connected op het bord)
PwmOut      motor1MagnitudePin(D5);  //Motorkracht op D5 (connected op het bord)
Ticker      controller;                //toets sample tijd
double fs = 10000;                     // sample frequentie
double ts = 1/fs;

// variablen voor void
int pulses;
int ref;
int er;
float Kp=0.0005;
float P;
float D;
float Dif;
float Kd=0.00012;
float PID;
int er2=0;

// constantes_k
float L1 = 0.250;
float L2 = 0.355;
float L3 = 0.150;


// Reference position_k
float q1_1 = 0;
float q2_1 = 0;
float q1_2;
float q2_2;

// EMG Input_k
float vx ;
float vy ;


// Encoder input_k
float q1_enc; //encoder actuator 1
float q2_enc; //encoder actuator 2


void PD()
{
char key;
    if(pc.readable()==true)  
    {   key = pc.getc();
        if (key=='q')
        {
        ref=-500;        //reference wordt 500 pulses
        }
        else if(key=='w')
        {
        ref=0;
        }
        else if(key=='e')
        {
        ref=500;          //reference wordt 0 pulses
        }
     }   
//error bepalen    
pulses=encoder.getPulses();
er=ref-pulses;

//PID
//Proportional part
P = Kp*er;

//Differential part
Dif=(er2-er)/ts;
D=Kd*Dif;

//PID sum
PID=P+D;
er2=er;

    //Motor control
    if (P<0)
    {
    motor1MagnitudePin = fabs(P);
    motor1DirectionPin = 1;
    }
    else if (P>0)
    {
    motor1MagnitudePin = fabs(P);
    motor1DirectionPin = 0;
    }

}

// Calculating q_set_k
void Kinematics()
    {
        q1_1 = q1_2;
        q2_1 = q2_2;
               
        q1_2 = ((-(L3 + L2*cos(q2_1))/(L1*L3*cos(q1_1) + L1*L2*cos(q1_1)*cos(q2_1) + L1*L2*sin(q1_1)*sin(q2_1))) * vx  +  (-(L2*sin(q2_1))/(L1*L3*cos(q1_1) + L1*L2*cos(q1_1)*cos(q2_1) + L1*L2*sin(q1_1)*sin(q2_1))) * vy) * ts + q1_enc;
        q2_2 = (((L3 + L2*cos(q2_1) - L1*sin(q1_1))/(L1*L3*cos(q1_1) + L1*L2*cos(q1_1)*cos(q2_1) + L1*L2*sin(q1_1)*sin(q2_1)) * vx) + ((L1*cos(q1_1) + L2*sin(q2_1))/(L1*L3*cos(q1_1) + L1*L2*cos(q1_1)*cos(q2_1) + L1*L2*sin(q1_1)*sin(q2_1))) * vy) * ts + q2_enc;
        }

int main()
{
controller.attach_us(&PD, 10000);
   
while(true)
{
}

    
}