File content as of revision 5:949c2861a79b:

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

Serial      pc(USBTX, USBRX);        //Serial PC connectie
DigitalOut  motor1DirectionPin(D4);  //Motorrichting op D4 (connected op het bord)
PwmOut      motor1MagnitudePin(D5);  //Motorkracht op D5 (connected op het bord)
DigitalOut  motor2DirectionPin(D7);  //Motorrichting op D4 (connected op het bord)
PwmOut      motor2MagnitudePin(D6);  //Motorkracht op D5 (connected op het bord)
QEI         q1_enc(D13, D12, NC, 32);       //encoder motor 1 instellen
QEI         q2_enc(D11, D10, NC, 32);       // encoder motor 2 instellen
const double pi = 3.1415926535897;  // waarde voor pi aanmaken

Timer t;
Ticker aansturing;
Ticker stepres;

double ref1 = 0;
double refrad1;
double refrad2;
double ref2 = 0;
double Kp1 = 0.002;    //kp motor 2 = 0.0001
double Ki1 = 0;
double Kp2 = 0.0005;
double Ki2 = 0.0005;
int q1_puls;
int q2_puls;
double rad2pulses=(2100/pi);
double pulses2rad = (pi)/2100;
double ts = 0.001;
double PI1;
int n;

double stap = 0.01;

double error1_1;
double error2_1 = 0;
double error_I_1;
double error_I2_1 = 0;
double error1_2;
double error2_2 = 0;
double error_I_2;
double error_I2_2 = 0;
double PI2;

// kinematica gegevens
// lengte armen
double L1 = 0.250;
double L2 = 0.355;
double L3 = 0.150;

// reference position
double q1=0;    // positie q1 in radialen
double q2=0;    // positie q2 in radialen
double q1_pos;
double q2_pos;

// EMG Input_k
double vx = 0;
double vy = 0;


void kinematica()
{
    // encoders uitlezen    
    q1_puls = q1_enc.getPulses();
    q1_pos =  q1_puls*pulses2rad;       // berekent positie q1 in radialen
    q2_puls = q2_enc.getPulses();
    q2_pos =  q2_puls*pulses2rad;       // berekent positie q2 in radialen  
    
    
    px = oldpx + stap;
    py = oldpy + stap;
    
    if (px >= pxmax || px <= pxmin)
    {
        vx = oldvx;
    }
    else if (py >= pymax || py <= pymin)
    {
        vy = oldvy;   
    }
    
    q1 = 
    q2 = 

    ref1 = q1 * rad2pulses;
    ref2 = q2 * rad2pulses;  
    
    px = oldpx;
    py = oldpy;
    
}


void controller()
{     
kinematica();
    //PID 1
error1_1 = ref1 - q1_puls;
error_I_1 = error_I2_1 + ts*((error1_1 + error2_1)/2);

PI1 = Kp1*error1_1 + Ki1*error_I_1;

error2_1   = error1_1; // opslaan variabelen voor integraal onderdeel
error_I2_1 = error_I_1;
    
    //PID 2
error1_2 = ref2 - q2_puls;
error_I_2 = error_I2_2 + ts*((error1_2 + error2_2)/2);
error_I2_2 = error_I_2;
error2_2 = error1_2; 

PI2 = Kp2*error1_2 + Ki2*(error_I_2);


    if (PI1<=0)
    {
    motor1DirectionPin = 1;
    motor1MagnitudePin = fabs(PI1);
    }
    else if (PI1>0)
    {
    motor1DirectionPin = 0;
    motor1MagnitudePin = fabs(PI1);        
    }
    
    if (PI2>=0)
    {
    motor2DirectionPin = 1;
    motor2MagnitudePin = fabs(PI2);   
    }
    else if(PI2<0)
    {
    motor2DirectionPin = 0;
    motor2MagnitudePin = fabs(PI2);               
    }
    
    if(n==500){         
    printf("q1_puls = %d   q2_puls = %d    ref1 = %f   ref2 = %f\n\r", q1_puls, q2_puls, ref1, ref2);
    n=0;
    }
    else{
             n=n+1;
             }     

}


int main()
{
    pc.baud(115200);
    t.start();
    aansturing.attach_us(&controller, 1000);
    
    
    while(true){

        }


}