File content as of revision 2:aa961ba3199e:

#include "mbed.h"
#include "QEI.h"

QEI encoder_Right(PB_3, PA_15, NC, 360, QEI::X4_ENCODING);
QEI encoder_Left(PA_1, PC_4, NC, 360, QEI::X4_ENCODING); 
double Kp = .02;
double Ki = .0000001;
double Kd = 0.001; 
PwmOut m_Right_F(PB_10);
PwmOut m_Right_B(PC_7);
PwmOut m_Left_F(PA_7); 
PwmOut m_Left_B(PB_6); 
double i_speed = 0.3;
int integrator = 0;
int decayFactor = 1;
int prevError = 0; 
Serial pc (PA_2, PA_3); //serial comm enabled on pins pa_2 and pa_3
Timer timer; 

double P_controller(int error) {
    double correction = (Kp*error);
    return correction;
}

double I_controller(int error) {
    integrator += error;
    double correction = Ki*integrator;
    integrator /= decayFactor;
    
    return correction;
}

double D_controller(int error) {
    int dError = error - prevError;
    int dt = timer.read_us();
    timer.reset();
    prevError = error;
    int correction = Kd*dError/dt;
    return correction;
}

Ticker systicker;
//speed = speed + P_Controller(error) + I_Controller(error) + D_Controller(error);
void systick() {
     int R_en_count = encoder_Right.getPulses()/100;
     int L_en_count = encoder_Left.getPulses()/100;
     int error = R_en_count - L_en_count; 
     double C_speed = P_controller(error) + I_controller(error) + D_controller(error);
     double f1_speed = C_speed + i_speed;
     double f2_speed = C_speed - i_speed;
     if(f1_speed >= 0.7)
     {
        f1_speed = 0.7;
     }
     if(f2_speed <= 0.2)
     {
         f2_speed = 0.2;
     }
       
     if (error > 0)
     {
         m_Left_F.write(f1_speed); 
         m_Right_F.write(f2_speed);
     }
     else
     {
        m_Right_F.write(f1_speed); 
        m_Left_F.write(f2_speed);  
     }
     
      
}

int main() {
    systicker.attach_us(&systick, 1000);
    m_Left_F.write(i_speed);
    m_Right_F.write(i_speed); 
    while(1)
    {
        wait(.1);
        pc.printf("Right Pulses sre: %i\n", encoder_Right.getPulses());
        pc.printf("Left Pulses are: %i\n", encoder_Left.getPulses());
    }
    
}