File content as of revision 2:a8ee608177ae:

 #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);

InterruptIn button1(D10);

Serial pc(USBTX, USBRX);

// Definitie constanten
double L0 = 0.09;
double K_p1 = 0.5;
double K_p2 = 0.5;
double p_desired_x = 0.04;
double p_desired_y = 0.13;
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 error_length_angle;
double error_angle;

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)-L0;
    p_current_y = (length)*sin(angle);
    
    //p_dot_x = K_p1*(p_desired_x - p_current_x);
    //p_dot_y = K_p2*(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));

    error_length_angle = (length_desired-length)/r_pulley;
    error_angle = angle_desired-angle;
    
    motor1_pwm = K_p1*error_length_angle;
    motor2_pwm = K_p2*error_angle;
    
    //Motor 1 (Translatie)
    if (error_length_angle >= 0) {
        motor1direction = false; //Positieve bewegingsrichting (clockwise, towards end)
        }
    else {
        motor1direction = true; // Negatieve bewegingsrichting
        }
     motor1control.write(fabs(motor1_pwm));
        
    //Motor 2 (Rotatie)
    if (error_angle >= 0){
        motor2direction = false; // counterclockwise (arm clockwise)
        }
    else {
        motor2direction = true; // clockwise (arm counterclockwise)
        }
    motor2control.write(fabs(motor2_pwm));
}    

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*180/pi, 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", motor1_pwm, motor2_pwm);
    }
}