File content as of revision 1:6a1ffe0cb8bd:

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


Serial      pc(USBTX, USBRX);        //Serial PC connectie
QEI encoder2(D11, D10, NC, 32);


DigitalOut  motor2DirectionPin(D7);  //Motorrichting op D4 (connected op het bord)
PwmOut      motor2MagnitudePin(D6);  //Motorkracht op D5 (connected op het bord)

Ticker      qwe;                //toets ticker
Ticker      besturing;
double fs = 1000;               // sample frequentie
double ts = 0.001;
char key;
int pulses;
double ref = 0;


// PID variablen
double Kp = 0.001;
double Ki = 0;
double PI;

double error1;
double error2 = 0;
double error_I = 0;
double error_I2= 0;



void toetsen()
{
    if(pc.readable()==true)  
    {   key = pc.getc();
        if (key=='w')
        {
        ref=25;        //reference wordt 500 pulses
        }
        else if(key=='s')
        {
        ref=-25;          //reference wordt 0 pulses
        }
     }
}

void controller()
{
pulses = encoder2.getPulses();
error1 = ref - pulses;
error_I = error_I2 + ts*((error1 - error2)/2);

PI = Kp*error1 + Ki*(error_I);

error2   = error1; 
error_I2 = error_I;

    //Motor control
    if (PI<0 )
    {
    motor2DirectionPin = 0;
    motor2MagnitudePin = fabs(PI);

    }
    else if (PI>0)
    {
    motor2DirectionPin = 1;
    motor2MagnitudePin = fabs(PI);
    }
}



int main()
{
    pc.baud(115200);
    qwe.attach_us(&toetsen, 100000); //ticker voor toetsen aflezen
    besturing.attach_us(&controller, 1000); //ticker voor PI controller
    
    while(true) {
        }
}