File content as of revision 1:3b55e7add58b:

#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  RFvelo, LFvelo, prv_RFvelo, prv_LFvelo;
double  err, errL, errF, now_time, samp_time;
double  prv_time, prv_err, prv_errF,prv_RveloErr,prv_LveloErr;
double  a,b,c;
double  a2,b2,c2;
double  now_ang1, prv_ang1, now_ang2, prv_ang2, now_omg1, now_omg2;
double  RF2velo,pr2v_RF2velo, prv_RF2velo,LF2velo, pr2v_LF2velo, prv_LF2velo;
double  now_x,prv_x,Rlevantvelo,prv_Rlevantvelo,now_e,now_u1,prv_u1,now_x1,prv_x1,Llevantvelo,prv_Llevantvelo,now_e1,now_u2,prv_u2;
double  W   = 17.0;
double  PI  = 3.1416;
double  fc  = 2.5;
double  tau = 1/(2*PI*fc);
double   C              = (2.0*PI*fc)*(2.0*PI*fc);
double   B              = (2.0*PI*fc)*sqrt(2.0);
double   E              = 500.0 * (10 ^ (4));
double   alpha2          = 1.1 * E;
double   lambda         = sqrt(E);
double  RactVelo, LactVelo, RdesVelo, LdesVelo, RveloErr, LveloErr, omega, VV;
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);
}
double sign(double xe) 
{
  if (xe > 0) { return 1; } 
  else { return -1; }
}

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());
        RactVelo  = (now_ang1 - prv_ang1)/samp_time;
        
        now_ang2  = pulsesToDegrees(wheel2.getPulses());
        LactVelo  = (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 ));
        
        omega    = (err*0.05) + (((err - prv_err)/samp_time)*0.1);
        VV       = (390.0);
        RdesVelo = VV - (omega*(W/2.0));
        LdesVelo = VV + (omega*(W/2.0));
        
        RFvelo = ( samp_time * RactVelo + (tau * prv_RFvelo) ) / ( samp_time + tau);
        LFvelo = ( samp_time * LactVelo + (tau * prv_LFvelo) ) / ( samp_time + tau);
       
        a         = ((samp_time*samp_time)*C)*RactVelo;
        b         = (2.0+(samp_time*B))*prv_RF2velo;
        c         = pr2v_RF2velo;
        RF2velo   = (a+b-c)/(((samp_time*samp_time)*C)+(samp_time*B)+1.0);
        
        a2         = ((samp_time*samp_time)*C)*LactVelo;
        b2         = (2.0+(samp_time*B))*prv_LF2velo;
        c2         = pr2v_LF2velo;
        LF2velo    = (a2+b2-c2)/(((samp_time*samp_time)*C)+(samp_time*B)+1.0);
        
         now_x       = (prv_Rlevantvelo* samp_time) + prv_x;
         now_e       = now_x - now_ang1;
         now_u1      = (-alpha2 * sign(now_e) * samp_time) + prv_u1;
         Rlevantvelo = now_u1 - (lambda * sqrt(abs(now_e)) * sign(now_e));
         
         now_x1       = (prv_Llevantvelo* samp_time) + prv_x1;
         now_e1       = now_x1 - now_ang2;
         now_u2       = (-alpha2 * sign(now_e1) * samp_time) + prv_u2;
         Llevantvelo  = now_u1 - (lambda * sqrt(abs(now_e1)) * sign(now_e1));
        
        
        RveloErr = RdesVelo -RFvelo ;
        LveloErr = LdesVelo -LFvelo;
        
        voltR  = RveloErr *0.03+(((RveloErr - prv_RveloErr)/samp_time)*0); 
        voltL  = LveloErr*0.03+(((LveloErr - prv_LveloErr)/samp_time)*0);
        
        voltRF = ( samp_time * voltR + (tau * prv_voltRF) ) / ( samp_time + tau);
        voltLF = ( samp_time * voltL + (tau * prv_voltLF) ) / ( samp_time + tau);
                
        if (voltR>0)    {  pin_R1 = 0; pin_R2 = 1; }
        else            {  pin_R1 = 1; pin_R2 = 0; }
        if (voltL>0)    {  pin_L1 = 0; pin_L2 = 1; }
        else            {  pin_L1 = 1; pin_L2 = 0; }
        
        pwmR  = (abs(voltR)/12.0); 
        pwmL  = (abs(voltL)/12.0); 
        if (pwmR>1)     {pwmR = 1;}
        else            {pwmR = pwmR;}
        if (pwmL>1)     {pwmL = 1;}
        else            {pwmL = pwmL;}
        
        pc.printf("  %f     %f   ",RactVelo,Rlevantvelo);
        //pc.printf("  %f", samp_time );
        
        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;
        prv_RFvelo  = RFvelo;
        prv_LFvelo  = LFvelo;
        prv_RveloErr = RveloErr;
        prv_LveloErr = LveloErr;
        prv_RF2velo = RF2velo;
        pr2v_RF2velo = prv_RF2velo;
        prv_LF2velo  = LF2velo;
        pr2v_LF2velo = prv_LF2velo;
        prv_x        =now_x;
        prv_u1       =now_u1;
        prv_x1       =now_x1;
        prv_u2       =now_u2;
        prv_Rlevantvelo =Rlevantvelo;
        prv_Llevantvelo =Llevantvelo;
            
    }
}