File content as of revision 1:907507671a38:

 #include "mbed.h"
#include "math.h"
#include "BiQuad.h"
#include <string>
#include "QEI.h"

//----------------- INITIAL -------------------------
QEI Encoder1(D12,D13,NC,64,QEI::X2_ENCODING);
QEI Encoder2(D2,D3,NC,64,QEI::X2_ENCODING);
Ticker EncoderTicker;

DigitalOut motor1direction(D7);
PwmOut motor1control(D6);
 
DigitalOut motor2direction(D4);
PwmOut motor2control(D5);

Serial pc(USBTX, USBRX);

// Definitie constanten
double L0 = 0.09;
double K_p1 = 0.30;
double K_p2 = 0.30;
double p_desired_x = 0.15;
double p_desired_y = 0.05;
double r_pulley = 0.015915;
double pi = 3.141592653589793;
double gearratio = 3.857142857;

// Definitie variabelen
double angle_trans;
double translatie;
double angle;
double length;
double angle_desired;
double length_desired;
double motor1_pwm;
double length_dot;
double motor2_pwm;

double p_current_x;
double p_current_y;

void EncoderFunc() 
{
    angle_trans = Encoder1.getPulses() * 0.0857142857*0.0174532925;                  // Translation [rad]
    translatie = angle_trans * r_pulley;           // Translatie arm [m]
    angle = Encoder2.getPulses() * 0.0857142857*0.0174532925/gearratio;                        // Angle arm [rad]
    length = translatie+L0;

    p_current_x = (length)*cos(angle);
    p_current_y = (length)*sin(angle);
    
    //p_dot_x = v_constant*(p_desired_x - p_current_x);
    //p_dot_y = v_constant*(p_desired_y - p_current_y);
    
    angle_desired = atan2(p_desired_y,p_desired_x+L0);
    length_desired = sqrt(pow(p_desired_x+L0,2)+pow(p_desired_y,2));

    motor1_pwm = K_p1*(length_desired-length)/r_pulley;
    motor2_pwm = K_p2*(angle_desired-angle);
        
    motor1control.write((motor1_pwm));
    motor2control.write((motor2_pwm));
    //motor1direction = motor1_pwm < 0;
    //motor2direction = motor2_pwm < 0;
 }


int main()
{
EncoderTicker.attach(&EncoderFunc, 0.02);
pc.baud(115200);
motor1direction = false;
motor2direction = false;

while(true)
    {
    wait(0.1);
    pc.printf("angle = %f, length = %f \r\n", angle, length);
    pc.printf("x = %f, y = %f \r\n", p_current_x, p_current_y);
    pc.printf("motor1_pwm = %f, motor2_pwm = %f \r\n\r\n", motor1_pwm, motor2_pwm);
    }
}