File content as of revision 3:f70ec68723df:

 #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 casebutton1(PTA4);                                                  //Button for switching the cases
InterruptIn casebutton2(PTC6);                                                  //Button for switching the cases

InterruptIn button1(D10);                                                       //Button for switching x-axes and for getting zero and max
InterruptIn button2(D11); 
Serial pc(USBTX, USBRX);

// Definitie constanten
double L0 = 0.09;
double K_p1 = 2.0;
double K_p2 = 7.0;
double v_x = 0.02; // speed in m/s
double v_y = 0.02; // speed in m/s

double motorValue1;
double motorValue2;

double r_pulley = 0.015915;
double pi = 3.141592653589793;
double gearratio = 3.857142857;

Ticker motor1ticker;
Ticker motor2ticker;

// 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_desired_x = 0.002;
double p_desired_y = 0.002;
double p_current_x;
double p_current_y;

void setMotor1() {
    motor1control.write(motorValue1);
    }
 
void setMotor2() {
    motor2control.write(motorValue2);
    }

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;
  
    if (button2 <=0.5){ //Button ingedrukt
        p_desired_x = p_desired_x + v_x*0.02;
        }
    else {
        p_desired_x = p_desired_x;
        }
    if (button1 <=0.5){ //Button ingedrukt
        p_desired_y = p_desired_y + v_y*0.02;
        }
    else {
        p_desired_y = p_desired_y;
        }
    
    if (casebutton2 == 0) {
        v_x = -v_x;
        }
    if (casebutton1 == 0) {
        v_y = -v_y;
        }

    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 (motor1_pwm >= 0) {
        motor1direction = false; //Positieve bewegingsrichting (clockwise, towards end)
        }
    else {
        motor1direction = true; // Negatieve bewegingsrichting
        }
     motorValue1 = fabs(motor1_pwm);
        
    //Motor 2 (Rotatie)
    if (motor2_pwm >= 0){
        motor2direction = false; // counterclockwise (arm clockwise)
        }
    else {
        motor2direction = true; // clockwise (arm counterclockwise)
        }
    motorValue2 = fabs(motor2_pwm);
}    

int main() {
    
motor1ticker.attach(&setMotor1, 0.02);
motor2ticker.attach(&setMotor2, 0.02);

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);
    pc.printf("p_desired_x = %f, p_desired_y = %f \r\n", p_desired_x, p_desired_y);

    }
}