File content as of revision 2:8db9af8bd3fd:

#include "QEI.h"
#include "mbed.h"
Serial pc(USBTX, USBRX);

PwmOut      pwmR    (p21);
DigitalOut  pin_R1  (p22);
DigitalOut  pin_R2  (p23);

PwmOut      pwmL    (p24);
DigitalOut  pin_L1  (p25);
DigitalOut  pin_L2  (p26);

DigitalIn   IRpin1  (p16);
DigitalIn   IRpin2  (p17);
DigitalIn   IRpin3  (p18);
DigitalIn   IRpin4  (p19);
DigitalIn   IRpin5  (p20);

double  voltR, voltL;
int     IRread1, IRread2, IRread3, IRread4, IRread5;
int     OIRread1, OIRread2, OIRread3, OIRread4, OIRread5;
int     prv_err, act, err;

int     des   = 0;
double  alpha = 0.4;
double  tau   = 0.1;
long    counts_per_rev = 64;
double  prv_ang, prv_time;
int     prv_errF = 0;
double  errL, errF, now_ang, now_omg, now_time, samp_time, GoLog;

QEI wheel (p29, p30, NC, counts_per_rev, QEI::X4_ENCODING);

float pulsesToDegrees(float pulses) { return ((pulses/counts_per_rev)*360); }

int main()
{
    Timer myTime;
    myTime.reset();
    myTime.start();
    pc.baud(9600);
    while(1) {
        IRread1     =   IRpin1.read();
        IRread2     =   IRpin2.read();
        IRread3     =   IRpin3.read();
        IRread4     =   IRpin4.read();
        IRread5     =   IRpin5.read();
        
        now_time    =   myTime.read_ms()/1000.0;
        samp_time   =   now_time - prv_time;
        now_ang     =   pulsesToDegrees(wheel.getPulses());
        now_omg     =   (now_ang - prv_ang)/samp_time;
        
        if(IRread1 == 0)    {OIRread1 = 1;} else if(IRread1 == 1)   {OIRread1 = 0;}
        if(IRread2 == 0)    {OIRread2 = 1;} else if(IRread2 == 1)   {OIRread2 = 0;}
        if(IRread3 == 0)    {OIRread3 = 1;} else if(IRread3 == 1)   {OIRread3 = 0;}
        if(IRread4 == 0)    {OIRread4 = 1;} else if(IRread4 == 1)   {OIRread4 = 0;}
        if(IRread5 == 0)    {OIRread5 = 1;} else if(IRread5 == 1)   {OIRread5 = 0;}
        
        act  = ((OIRread1*(-4))+(OIRread2*(-2))+(OIRread3*(0))+(OIRread4*(2))+(OIRread5*(4)))/ (IRread1 + IRread2 + IRread3 + IRread4 + IRread5) ;
        err  = des - act ;
        errL = double ( err + alpha * ((err - prv_err)/samp_time ));
        errF = ((errL*samp_time) + (tau*prv_errF))/( tau + samp_time );
        
        if     (errF < 0)    {   voltR = 0.00; voltL = 12.00; pin_R1 = 0; pin_R2 = 0; pin_L1 = 1; pin_L2 = 0; GoLog = 1;  }
        else if(errF > 0)    {   voltR = 12.00; voltL = 0.00; pin_R1 = 1; pin_R2 = 0; pin_L1 = 0; pin_L2 = 0; GoLog = -1; }
        else if(errF == 0)   {   voltR = 12.00; voltL = 12.00; pin_R1 = 1; pin_R2 = 0; pin_L1 = 1; pin_L2 = 0; GoLog = 0;  }           
        pwmR = (voltR/12.0); 
        pwmL = (voltL/12.0); 
        
        //pc.printf("   %d   %f  %d  %d  %d  %d  %d  %f\r", err, errL, IRread1, IRread2, IRread3, IRread4, IRread5, GoLog);
        pc.printf("   %d %f %f\r", err, errL, errF);
        printf("\n\r");
        prv_ang  = now_ang;
        prv_time = now_time;
        prv_err  = err;
        prv_errF = errF;
    }
}