File content as of revision 1:7969189824ff:

#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.0001;
double Ki2 = 0;
int q1_puls;
int q2_puls;
double rad2pulses=4200/(2*pi);
double ts = 0.001;
double PI1;
int n;

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;

void controller()
{   
    refrad2 =  1.57f*sin(t.read()); //motor 2
    ref2 = refrad2 * rad2pulses;   
    refrad1 = -0.5f*cos(t.read()) + 0.6f; //motor 1
    ref1 = refrad2 * rad2pulses;
    
    // encoders uitlezen    
    q1_puls = q1_enc.getPulses();
    q2_puls = q2_enc.getPulses();

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

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("error1 = %f   error2 = %f\n\r", error1_1, error1_2);
    n=0;
    }
    else{
             n=n+1;
             }     

}

void stapfunc()
{
 if (ref1==0){
     ref1 = 1000;
     }
     else if (ref1==1000)
     {
         ref1=0; 
     }  
}

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

        }


}