File content as of revision 0:29e8568563e4:

#include "mbed.h"
#include "ContinuousServo.h"
#include "Tach.h"

Tach tLeft(p17,64);
Tach tRight(p13,64);

ContinuousServo left(p23);
ContinuousServo right(p26);

float l;
float r;
float speedL;
float speedR;
float errorL;
float errorR;
float sampling_per;

Serial pc(USBTX,USBRX);

float PIpwmL(float desired_speed,float speed); 
float PIpwmR(float desired_speed,float speed); 

int main() {
    wait(5);
    //go straight
    while(1){
        
        speedL = tLeft.getSpeed();
        speedR = tRight.getSpeed();
        l = PIpwmL(0.3, speedL);
        r = PIpwmR(0.3, speedR); 
        l = r + 0.1;
        left.speed(l);
        right.speed(-r);
        
        pc.printf("%f,%f\n",speedL,speedR);
        wait(0.05);
        r = l + 0.1;
        speedL = tLeft.getSpeed();
        speedR = tRight.getSpeed();
        left.speed(l);
        right.speed(-r);
        pc.printf("%f,%f\n",speedL,speedR);
    }
}

float PIpwmL(float desired_speed,float speed)
{
    float integral_errorL = 0.0;  
    float sampling_per = 0.05;    
    float errorL = desired_speed - speed;
    integral_errorL += (errorL*sampling_per);   
    float left = (0.07*integral_errorL) + (0.08*errorL) + 0.48;   
    return left;
    }
float PIpwmR(float desired_speed,float speed)
{
        float integral_errorR = 0.0;
        float sampling_per = 0.05;
        float errorR = desired_speed - speed;
        integral_errorR += (errorR*sampling_per);
        float right = (0.07*integral_errorR) + (0.08*errorR) + 0.25;
        return right;
    }