File content as of revision 0:0170bad0f358:

#include "mbed.h"
#include "Motor.h"
Serial pc(USBTX, USBRX);

 
InterruptIn Rencoder(p30);
InterruptIn Lencoder(p29);

DigitalOut led(LED1);
DigitalOut flash(LED4);

Motor LMotor(p22, p23, p24); // pwmA, fwd, rev,  
Motor RMotor(p21, p26, p25); // pwmB, fwd, rev, 

int Rcount = 0;
int Lcount = 0;
int Error = 0;
float Rspeed = .5;
float Lspeed = .5;
int Instr = 1;
int Speed = 50;
int Rtot;
int Ltot;

Ticker Sampler;

 
void RSample() {
    Rcount++;
//    Rtot = Rtot + Rcount;
// pc.printf("Rcount: %d\n\r",Rcount);  
}

void LSample() {
    Lcount++;
//    Ltot = Ltot + Lcount;
}

void callback() {
    // Note: you need to actually read from the serial to clear the RX interrupt
    printf("%c\n",pc.getc());
    led = !led;
}
 
int main() {
    pc.attach(&callback);
    
    Rencoder.mode(PullUp);
    Rencoder.rise(&RSample);
    Lencoder.mode(PullUp);
    Lencoder.rise(&LSample);    
    
    while(1) { 
    switch (Instr){
    
    // stop instruction
        case (0):
        Rspeed = 0;
        Lspeed = 0;
        break;
    // forward instruction
        case (1) :  //Make sure the robot is moving forward or reversed and that left wheel hasn't stopped. 
        Error =  Lcount - Rcount;
        //pc.printf("Lcount in sampler: %f\n\r",Lcount);
        //pc.printf("RCount in sampler: %f\n\r",Rcount);
        //pc.printf("Error in sampler: %f\n\r",Error);
        //pc.printf("Rspeed: %f \n \r", Rspeed);
        Rspeed = Rspeed + ((float)Error / 80);
        break;
        
    // Left turn Instruction
        case (2):   
        Error =  Lcount - Rcount;
        //pc.printf("Lcount in sampler: %f\n\r",Lcount);
        //pc.printf("RCount in sampler: %f\n\r",Rcount);
        //pc.printf("Error in sampler: %f\n\r",Error);
        //pc.printf("Rspeed: %f \n \r", Rspeed);
        Rspeed = Rspeed + ((float)Error / 80);
        LMotor.speed(0);
        RMotor.speed(0);
        if (Lspeed >0){
        Lspeed = -Lspeed;}
        break;
        
    // Right turn Instruction
        case (3):   
        Error =  Lcount - Rcount;
        //pc.printf("Lcount in sampler: %f\n\r",Lcount);
        //pc.printf("RCount in sampler: %f\n\r",Rcount);
        //pc.printf("Error in sampler: %f\n\r",Error);
        //pc.printf("Rspeed: %f \n \r", Rspeed);
        Rspeed = Rspeed + ((float)Error / 80);
        LMotor.speed(0);
        RMotor.speed(0);
        if (Rspeed >0){
        Rspeed = -Rspeed;}
        break;
        
    // reverse instruction
        case (4):  //Make sure the robot is moving forward or reversed and that left wheel hasn't stopped. 
        Error =  Rcount - Lcount;
        //pc.printf("Lcount in sampler: %f\n\r",Lcount);
        //pc.printf("RCount in sampler: %f\n\r",Rcount);
        //pc.printf("Error in sampler: %f\n\r",Error);
        //pc.printf("Rspeed: %f \n \r", Rspeed);
        Rspeed = Rspeed + ((float)Error / 80);
        if (Rspeed > 0){
        Rspeed = -Rspeed;
        }
        if (Lspeed > 0){
        Lspeed = -Lspeed;
        }
        LMotor.speed(0);
        RMotor.speed(0);
        break;
       }
       
LMotor.speed(Lspeed);
RMotor.speed(Rspeed);
Lcount = 0; //Restart the counters
Rcount = 0;
wait(.02);
}
}