File content as of revision 1:b19f21fdf69f:

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

ContinuousServo left(p23);                  //Set up left wheel driver
ContinuousServo right(p26);                 //Set up right wheel driver
Tach tleft(p17,64);                         //Set up left tachometer
Tach tright(p13, 64);                       //Set up right tachometer

float wl;                                   //Left wheel velocity
float wr;                                   //Right wheel velocity
float e;                                    //Error

float r = 1.3125;                           //radius of wheels
float l = 4.0625;                           //width of wheel base
//For these robots, a positive omega is a right turn and a negative is left
float omega = 0.2;                          //angular velocity of wheels
float v = 0.0;                              //translational velocity of wheels
float kp = 1.0;                             //Proportional control gain

float vl = ((omega*l)+(2*v))/(2*r);         //Calculates left wheel velocity
float vr = ((omega*l)-(2*v))/(2*r);         //Calculates right wheel velocity

int main() {
    while(1){                               //Forever
        left.speed(vl);                     //Move the left wheel
        right.speed(vr);                    //Move the right wheel
        wl = tleft.getSpeed();              //Read the left tachometer
        wr = tright.getSpeed();             //Read the right tachometer
    
        e = wl - wr;                        //Calculate the error
        vr = vr + (kp*e);                   //Readjust the right wheel velocity
    }
}