File content as of revision 1:087dfa2bff27:

#include "mbed.h"
#include "QEI.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, voltRF, voltLF, prv_voltRF, prv_voltLF;
int     IRread1, IRread2, IRread3, IRread4, IRread5;
int     OIRread1, OIRread2, OIRread3, OIRread4, OIRread5;

int     act, des = 2000;
double  alpha = 0.5;
double  speedDiff;
double  err, errL, errF, now_time, samp_time;
double  prv_time, prv_err, prv_errF;

double  now_ang1, prv_ang1, now_ang2, prv_ang2, now_omg1, now_omg2;
double   PI             = 3.1416;
double   fc             = 0.5;
double   tau            = 1/(2*PI*fc);

long     counts_per_rev = (48*75);

QEI wheel1 (p29, p27, NC, counts_per_rev, QEI::X2_ENCODING);
QEI wheel2 (p30, p28, NC, counts_per_rev, QEI::X2_ENCODING);

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

int main()
{
    Timer myTime;
    myTime.reset();
    myTime.start();
    pc.baud(57600);
    while(1) {
        IRread1     =   IRpin1.read();
        IRread2     =   IRpin2.read();
        IRread3     =   IRpin3.read();
        IRread4     =   IRpin4.read();
        IRread5     =   IRpin5.read();
        
        now_time    =   myTime.read_us()/1000000.0;
        samp_time   =   now_time - prv_time;
        
        now_ang1   = pulsesToDegrees(wheel1.getPulses());
        now_omg1   = (now_ang1 - prv_ang1)/samp_time;
        
        now_ang2   = pulsesToDegrees(wheel2.getPulses());
        now_omg2   = (now_ang2 - prv_ang2)/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  = ((IRread1*(0))+(IRread2*(1000))+(IRread3*(2000))+(IRread4*(3000))+(IRread5*(4000)))/ (IRread1 + IRread2 + IRread3 + IRread4 + IRread5) ;
        err  = des - act ;
        errL =  err + (alpha * ((err - prv_err)/samp_time ));
        //errF = ((errL*samp_time) + (tau*prv_errF))/( tau + samp_time );
        
        speedDiff = (errL*0.004);// + (0.0001*((err - prv_err)/samp_time));
        
        voltR = 12.0 - speedDiff;
        voltL = 12.0 + speedDiff;
        
        //voltR = voltR*1.0;
        //voltL = voltL*1.0;
        
       voltRF = ( samp_time * voltR + (tau * prv_voltRF) ) / ( samp_time + tau);
       voltLF = ( samp_time * voltL + (tau * prv_voltLF) ) / ( samp_time + tau);
       // voltRF = 10.0;
        //voltLF = 10.0;
        if (voltRF>8)     {voltRF = 8;}
        else            {voltRF = voltRF;}
        if (voltLF>8)     {voltLF = 8;}
        else            {voltLF = voltLF;}
        
        if (voltRF>0)    {  pin_R1 = 0; pin_R2 = 1; }
        else            {  pin_R1 = 1; pin_R2 = 0; }
        if (voltLF>0)    {  pin_L1 = 0; pin_L2 = 1; }
        else            {  pin_L1 = 1; pin_L2 = 0; }
        
        pwmR  = (abs(voltRF)/12.0); 
        pwmL  = (abs(voltLF)/12.0); 
        if (pwmR>1)     {pwmR = 1;}
        else            {pwmR = pwmR;}
        if (pwmL>1)     {pwmL = 1;}
        else            {pwmL = pwmL;}
        
        //pc.printf("  %f   %d   %d   %f  %f   %f   %f  %f     %d    %d     %d     %d     %d\r", now_time, des, act, err, errL, errF, voltL, voltR, IRread1, IRread2, IRread3, IRread4, IRread5 );
        //pc.printf("  %f    %f    %f\r", err, errL, errF);
        //pc.printf(" %d  %d  %f  %f  %f", des,act,err,now_omg1,now_omg2);
        pc.printf("%f  %f", now_omg1,now_omg2);
        printf("\n\r");
        //on left motor, red and black cables are crossed-connected to have forward movement when pin_L1=1 & pin_L2=0.
        //on breadboard, + line represent +5v. - line represent GND. 12v is only used to supply L298.
        prv_time    = now_time;
        prv_ang1    = now_ang1;
        prv_ang2    = now_ang2;
        prv_err     = err;
        prv_errF    = errF;
        prv_voltRF  = voltRF;
        prv_voltLF  = voltLF;
    }
}